feat(low_controller): Integrate LowController for low-level command handling

- Added LowController class to manage low-level commands and communication.
- Updated CustomRobot to initialize and manage LowController, including methods for processing low-level commands.
- Enhanced logger functionality with variadic template methods for formatted logging.
- Updated CMakeLists.txt to include the new low_controller.cpp source file and low_controller.hpp header.

CMakeLists.txt

@@ -37,6 +37,7 @@ set(SOURCES
     src/logger.cpp
     src/mqtt.cpp
     src/navigation.cpp
+    src/low_controller.cpp
 )
 
 add_executable(main ${SOURCES})

include/custom_robot.hpp

@@ -5,6 +5,7 @@
 #include "logger.hpp"
 #include "mqtt.hpp"
 #include "navigation.hpp"
+#include "low_controller.hpp"
 
 #include <memory>
 #include <atomic>
@@ -40,11 +41,13 @@ public:
     bool processRscCmd(const std::string& cmd, const nlohmann::json& message);
     bool processNavCmd(const std::string& cmd, const nlohmann::json& message);
     bool processMscCmd(const std::string& cmd, const nlohmann::json& message);
+    bool processLowCmd(const std::string& cmd, const nlohmann::json& message);
     void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);
 
 private:
     std::string generateRandomClientId() const;
     std::unique_ptr<Controller> controller_;
+    std::unique_ptr<LowController> low_controller_;
     std::unique_ptr<Navigation> navigation_;
     std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
     std::unique_ptr<MqttClient> mqttClient_;

include/logger.hpp

@@ -30,6 +30,17 @@ public:
     void warn(const std::string& message);
     void error(const std::string& message);
 
+    template<typename... Args>
+    void log(LogLevel level, const char* format, Args... args);
+    template<typename... Args>
+    void debug(const char* format, Args... args);
+    template<typename... Args>
+    void info(const char* format, Args... args);
+    template<typename... Args>
+    void warn(const char* format, Args... args);
+    template<typename... Args>
+    void error(const char* format, Args... args);
+
 private:
     Logger() = default;
     ~Logger() = default;
@@ -45,9 +56,9 @@ private:
 };
 
 // Convenience macros
-#define LOG_DEBUG(msg) Logger::getInstance().debug(msg)
+#define LOG_DEBUG(msg, ...) custom::Logger::getInstance().debug(msg, ##__VA_ARGS__)
-#define LOG_INFO(msg) Logger::getInstance().info(msg)
+#define LOG_INFO(msg, ...) custom::Logger::getInstance().info(msg, ##__VA_ARGS__)
-#define LOG_WARN(msg) Logger::getInstance().warn(msg)
+#define LOG_WARN(msg, ...) custom::Logger::getInstance().warn(msg, ##__VA_ARGS__)
-#define LOG_ERROR(msg) Logger::getInstance().error(msg)
+#define LOG_ERROR(msg, ...) custom::Logger::getInstance().error(msg, ##__VA_ARGS__)
 
 } // namespace custom

include/low_controller.hpp

@@ -0,0 +1,43 @@
+#ifndef LOW_CONTROLLER_HPP
+#define LOW_CONTROLLER_HPP
+
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <atomic>
+
+#define TOPIC_LOWCMD "rt/lowcmd"
+
+constexpr double PosStopF = (2.146E+9f);
+constexpr double VelStopF = (16000.0f);
+
+namespace custom {
+
+class LowController {
+public:
+    LowController();
+    ~LowController();
+
+    bool initialize();
+    bool start();
+    bool stop();
+    void shutdown() { stop(); }
+    bool isRunning() const { return running_; }
+
+    void requestAutoCharge(bool enable);
+    void powerOff();
+
+private:
+    void publishLoop();
+    void initLowCmd();
+    static uint32_t crc32_core(uint32_t* ptr, uint32_t len);
+
+    unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher_;
+    unitree_go::msg::dds_::LowCmd_ low_cmd_{};
+    unitree::common::ThreadPtr publish_thread_;
+    std::atomic<bool> running_{false};
+};
+
+} // namespace custom
+
+#endif // LOW_CONTROLLER_HPP  

src/custom_robot.cpp

@@ -43,6 +43,11 @@ CustomRobot::~CustomRobot() {
         controller_.reset();
     }
 
+    if (low_controller_) {
+        low_controller_->shutdown();
+        low_controller_.reset();
+    }
+    
     if (rsc_) {
         rsc_.reset();
     }
@@ -66,6 +71,9 @@ bool CustomRobot::initialize() {
         controller_ = std::make_unique<Controller>();
         controller_->initialize();
 
+        low_controller_ = std::make_unique<LowController>();
+        low_controller_->initialize();
+
         navigation_ = std::make_unique<Navigation>();
         navigation_->Init();
 
@@ -115,6 +123,12 @@ bool CustomRobot::start() {
         LOG_ERROR("Failed to start robot controller");
         return false;
     }
+
+    if (!low_controller_->start()) {
+        LOG_ERROR("Failed to start low-level controller");
+        return false;
+    }
+
     running_ = true;
     LOG_INFO("CustomRobot started successfully");
     return true;
@@ -266,6 +280,8 @@ void CustomRobot::processCmd(const nlohmann::json& message) {
             success = processNavCmd(cmd, message);
         } else if (type == "msc") {
             success = processMscCmd(cmd, message);
+        } else if (type == "low") {
+            success = processLowCmd(cmd, message);
         } else {
             LOG_ERROR("Unknown command type: " + type);
             return;
@@ -277,6 +293,45 @@ void CustomRobot::processCmd(const nlohmann::json& message) {
     }
 }
 
+bool CustomRobot::processLowCmd(const std::string& cmd, const nlohmann::json& message)
+{
+    if (!low_controller_)
+    {
+        LOG_ERROR("LowController not initialized");
+        return false;
+    }
+
+    try
+    {
+        if (cmd == "requestAutoCharge")
+        {
+            if (!message.contains("param") || !message["param"].contains("enable"))
+            {
+                LOG_ERROR("requestAutoCharge cmd missing 'enable' parameter");
+                return false;
+            }
+            bool enable = message["param"]["enable"];
+            low_controller_->requestAutoCharge(enable);
+            return true;
+        }
+        else if (cmd == "powerOff")
+        {
+            low_controller_->powerOff();
+            return true;
+        }
+        else
+        {
+            LOG_ERROR("Unknown Low command: " + cmd);
+            return false;
+        }
+    }
+    catch (const std::exception& e)
+    {
+        LOG_ERROR("Error executing Low command " + cmd + ": " + std::string(e.what()));
+        return false;
+    }
+}
+
 bool CustomRobot::processOacCmd(const std::string& cmd, const nlohmann::json& message) {
     if (!controller_) {
         LOG_ERROR("Controller not initialized");

src/logger.cpp

@@ -46,6 +46,42 @@ void Logger::log(LogLevel level, const std::string& message) {
     }
 }
 
+template<typename... Args>
+void Logger::log(LogLevel level, const char* format, Args... args)
+{
+    if (level < currentLevel_) {
+        return;
+    }
+
+    // A buffer to hold the formatted string.
+    // First, we try with a small buffer.
+    char buffer[256];
+    int size = snprintf(buffer, sizeof(buffer), format, args...);
+    if (size < 0) {
+        // Encoding error
+        error("Encoding error in log message.");
+        return;
+    }
+
+    std::string message;
+    if (size < sizeof(buffer)) {
+        // The message fit in the buffer.
+        message = buffer;
+    } else {
+        // The buffer was too small, we need to allocate a larger one.
+        std::unique_ptr<char[]> dynamic_buffer(new char[size + 1]);
+        int new_size = snprintf(dynamic_buffer.get(), size + 1, format, args...);
+        if (new_size >= 0 && new_size <= size) {
+            message = dynamic_buffer.get();
+        } else {
+            error("Encoding error in log message after buffer resize.");
+            return;
+        }
+    }
+
+    log(level, message);
+}
+
 void Logger::debug(const std::string& message) {
     log(LogLevel::DEBUG, message);
 }
@@ -62,6 +98,30 @@ void Logger::error(const std::string& message) {
     log(LogLevel::ERROR, message);
 }
 
+template<typename... Args>
+void Logger::debug(const char* format, Args... args)
+{
+    log(LogLevel::DEBUG, format, args...);
+}
+
+template<typename... Args>
+void Logger::info(const char* format, Args... args)
+{
+    log(LogLevel::INFO, format, args...);
+}
+
+template<typename... Args>
+void Logger::warn(const char* format, Args... args)
+{
+    log(LogLevel::WARN, format, args...);
+}
+
+template<typename... Args>
+void Logger::error(const char* format, Args... args)
+{
+    log(LogLevel::ERROR, format, args...);
+}
+
 std::string Logger::getCurrentTime() {
     auto now = std::chrono::system_clock::now();
     auto time_t = std::chrono::system_clock::to_time_t(now);

src/low_controller.cpp

@@ -0,0 +1,128 @@
+#include "low_controller.hpp"
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <iostream>
+
+namespace custom {
+
+LowController::LowController() {}
+
+LowController::~LowController()
+{
+    stop();
+}
+
+bool LowController::initialize()
+{
+    lowcmd_publisher_.reset(new unitree::robot::ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
+    lowcmd_publisher_->InitChannel();
+
+    initLowCmd();
+    return true;
+}
+
+bool LowController::start()
+{
+    if (running_)
+    {
+        return true;
+    }
+    running_ = true;
+    publish_thread_ = unitree::common::CreateRecurrentThreadEx("low_cmd_pub", unitree::common::UT_CPU_ID_NONE, 2000, &LowController::publishLoop, this);
+    return true;
+}
+
+bool LowController::stop()
+{
+    if(running_)
+    {
+        running_ = false;
+        if (publish_thread_)
+        {
+            publish_thread_->join();
+            publish_thread_.reset();
+        }
+    }
+    return true;
+}
+
+void LowController::initLowCmd()
+{
+    low_cmd_.head()[0] = 0xFE;
+    low_cmd_.head()[1] = 0xEF;
+    low_cmd_.level_flag() = 0xFF;
+    low_cmd_.gpio() = 0;
+
+    for (int i = 0; i < 4; i++)
+    {
+        low_cmd_.wireless_remote()[i] = 0;
+    }
+
+    for (int i = 0; i < 20; i++)
+    {
+        low_cmd_.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode
+        low_cmd_.motor_cmd()[i].q() = (PosStopF);
+        low_cmd_.motor_cmd()[i].kp() = (0);
+        low_cmd_.motor_cmd()[i].dq() = (VelStopF);
+        low_cmd_.motor_cmd()[i].kd() = (0);
+        low_cmd_.motor_cmd()[i].tau() = (0);
+    }
+}
+
+void LowController::publishLoop()
+{
+    // The crc is calculated on the data part of the message, excluding the crc field itself.
+    low_cmd_.crc() = crc32_core((uint32_t *)&low_cmd_, (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
+    lowcmd_publisher_->Write(low_cmd_);
+}
+
+void LowController::requestAutoCharge(bool enable)
+{
+    if (enable)
+    {
+        low_cmd_.gpio() &= 0xFE;
+    }
+    else
+    {
+        low_cmd_.gpio() |= 0x01;
+    }
+}
+
+void LowController::powerOff()
+{
+    low_cmd_.bms_cmd().off() = 0xA5;
+}
+
+uint32_t LowController::crc32_core(uint32_t* ptr, uint32_t len)
+{
+    unsigned int xbit = 0;
+    unsigned int data = 0;
+    unsigned int CRC32 = 0xFFFFFFFF;
+    const unsigned int dwPolynomial = 0x04c11db7;
+
+    for (unsigned int i = 0; i < len; i++)
+    {
+        xbit = 1 << 31;
+        data = ptr[i];
+        for (unsigned int bits = 0; bits < 32; bits++)
+        {
+            if (CRC32 & 0x80000000)
+            {
+                CRC32 <<= 1;
+                CRC32 ^= dwPolynomial;
+            }
+            else
+            {
+                CRC32 <<= 1;
+            }
+
+            if (data & xbit)
+                CRC32 ^= dwPolynomial;
+            xbit >>= 1;
+        }
+    }
+
+    return CRC32;
+}
+
+} // namespace custom