From c5cff9fbdb97798dd9fa812ec06420af2be2d20b Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Mon, 22 Sep 2025 19:11:20 +0800 Subject: [PATCH] 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 | 1 + include/custom_robot.hpp | 3 + include/logger.hpp | 19 ++++-- include/low_controller.hpp | 43 +++++++++++++ src/custom_robot.cpp | 55 ++++++++++++++++ src/logger.cpp | 60 +++++++++++++++++ src/low_controller.cpp | 128 +++++++++++++++++++++++++++++++++++++ 7 files changed, 305 insertions(+), 4 deletions(-) create mode 100644 include/low_controller.hpp create mode 100644 src/low_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e7c2e80..bdb79ac 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index e909272..d8b7c3c 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -5,6 +5,7 @@ #include "logger.hpp" #include "mqtt.hpp" #include "navigation.hpp" +#include "low_controller.hpp" #include #include @@ -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& serviceList, int filterStatus = -1); private: std::string generateRandomClientId() const; std::unique_ptr controller_; + std::unique_ptr low_controller_; std::unique_ptr navigation_; std::unique_ptr rsc_; std::unique_ptr mqttClient_; diff --git a/include/logger.hpp b/include/logger.hpp index 33fb3da..655a868 100644 --- a/include/logger.hpp +++ b/include/logger.hpp @@ -30,6 +30,17 @@ public: void warn(const std::string& message); void error(const std::string& message); + template + void log(LogLevel level, const char* format, Args... args); + template + void debug(const char* format, Args... args); + template + void info(const char* format, Args... args); + template + void warn(const char* format, Args... args); + template + 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 diff --git a/include/low_controller.hpp b/include/low_controller.hpp new file mode 100644 index 0000000..52b25ed --- /dev/null +++ b/include/low_controller.hpp @@ -0,0 +1,43 @@ +#ifndef LOW_CONTROLLER_HPP +#define LOW_CONTROLLER_HPP + +#include +#include +#include +#include + +#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 lowcmd_publisher_; + unitree_go::msg::dds_::LowCmd_ low_cmd_{}; + unitree::common::ThreadPtr publish_thread_; + std::atomic running_{false}; +}; + +} // namespace custom + +#endif // LOW_CONTROLLER_HPP \ No newline at end of file diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 040c866..ace16ac 100644 --- a/src/custom_robot.cpp +++ b/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_->initialize(); + low_controller_ = std::make_unique(); + low_controller_->initialize(); + navigation_ = std::make_unique(); 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"); diff --git a/src/logger.cpp b/src/logger.cpp index cfcabe2..8c5c86e 100644 --- a/src/logger.cpp +++ b/src/logger.cpp @@ -46,6 +46,42 @@ void Logger::log(LogLevel level, const std::string& message) { } } +template +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 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 +void Logger::debug(const char* format, Args... args) +{ + log(LogLevel::DEBUG, format, args...); +} + +template +void Logger::info(const char* format, Args... args) +{ + log(LogLevel::INFO, format, args...); +} + +template +void Logger::warn(const char* format, Args... args) +{ + log(LogLevel::WARN, format, args...); +} + +template +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); diff --git a/src/low_controller.cpp b/src/low_controller.cpp new file mode 100644 index 0000000..8b414e4 --- /dev/null +++ b/src/low_controller.cpp @@ -0,0 +1,128 @@ +#include "low_controller.hpp" +#include +#include +#include + +namespace custom { + +LowController::LowController() {} + +LowController::~LowController() +{ + stop(); +} + +bool LowController::initialize() +{ + lowcmd_publisher_.reset(new unitree::robot::ChannelPublisher(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