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.

src/custom_robot.cpp

@@ -42,6 +42,11 @@ CustomRobot::~CustomRobot() {
         controller_->shutdown();
         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