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.

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_INFO(msg) Logger::getInstance().info(msg)
-#define LOG_WARN(msg) Logger::getInstance().warn(msg)
-#define LOG_ERROR(msg) Logger::getInstance().error(msg)
+#define LOG_DEBUG(msg, ...) custom::Logger::getInstance().debug(msg, ##__VA_ARGS__)
+#define LOG_INFO(msg, ...) custom::Logger::getInstance().info(msg, ##__VA_ARGS__)
+#define LOG_WARN(msg, ...) custom::Logger::getInstance().warn(msg, ##__VA_ARGS__)
+#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