fix: 更新MQTT代理地址并修复注释中的拼写错误

将MQTT代理地址从192.168.11.24更改为120.24.241.164以连接到新服务器修复generateRandomClientId函数注释中的多余字符'R'
feat: 添加生成随机客户端ID的功能
2025-10-15 15:37:51 +08:00 · 2025-10-09 19:48:24 +08:00 · 2025-10-09 19:32:44 +08:00 · 2025-10-09 19:24:01 +08:00 · 2025-10-09 19:17:26 +08:00 · 2025-10-09 18:26:07 +08:00
4 changed files with 124 additions and 113 deletions
--- a/include/config.hpp
+++ b/include/config.hpp
@@ -19,7 +19,7 @@ enum class Gait : int {
 constexpr std::string_view NETWORK_INTERFACE = "eth0";

 // MQTT settings
-constexpr std::string_view MQTT_BROKER = "192.168.2.236";
+constexpr std::string_view MQTT_BROKER = "120.24.241.164";
 constexpr int MQTT_PORT = 1883;
 constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client";
 constexpr std::string_view MQTT_USERNAME = "lzwc";
--- a/include/custom_robot.hpp
+++ b/include/custom_robot.hpp
@@ -7,6 +7,7 @@
 #include "navigation.hpp"
 #include "recharge.hpp"
 // #include "low_controller.hpp"
+#include "monitor.hpp"

 #include <memory>
 #include <atomic>
@@ -46,12 +47,13 @@ public:
    bool processLowCmd(const std::string& cmd, const nlohmann::json& message);
    bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message);
    void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);
-
-    bool startTraj();
-    void stopTraj();
+    
+    // Continuous movement control
+    bool startContinuousMove(float vx, float vy, float vyaw);
+    bool stopContinuousMove();
+    void continuousMoveLoop();

 private:
-    void trajControl();
    std::string generateRandomClientId() const;
    std::unique_ptr<Controller> controller_;
    // std::unique_ptr<LowController> low_controller_;
@@ -59,14 +61,18 @@ private:
    std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
    std::unique_ptr<MqttClient> mqttClient_;
    std::unique_ptr<Recharge> recharge_;
+    std::unique_ptr<Monitor> monitor_;
    
    CustomConfig config_;  
    std::atomic<bool> running_;
    std::atomic<bool> initialized_;
    unitree::common::ThreadPtr traj_th_;
-    std::atomic<bool> traj_running_;
-    float traj_dt_;
-    float traj_count_;
+    
+    // Continuous movement control variables
+    std::atomic<bool> continuous_move_enabled_;
+    std::atomic<float> continuous_vx_;
+    std::atomic<float> continuous_vy_;
+    std::atomic<float> continuous_vyaw_;
 };

 } // namespace custom
--- a/src/custom_robot.cpp
+++ b/src/custom_robot.cpp
@@ -20,10 +20,11 @@ CustomRobot::CustomRobot()
    , rsc_(nullptr)
    , mqttClient_(nullptr)
    , running_(false)
-    , initialized_(false) 
-    , traj_running_(false)
-    , traj_dt_(0.02f)
-    , traj_count_(0.0f) {
+    , initialized_(false)
+    , continuous_move_enabled_(false)
+    , continuous_vx_(0.0f)
+    , continuous_vy_(0.0f)
+    , continuous_vyaw_(0.0f) {
    config_.loadDefaults();
    
    try {
@@ -36,8 +37,6 @@ CustomRobot::CustomRobot()

 CustomRobot::~CustomRobot() {
    LOG_INFO("Shutting down CustomRobot");
-    
-    stopTraj();

    if (mqttClient_) {
        mqttClient_->stopMessageProcessor();
@@ -66,6 +65,10 @@ CustomRobot::~CustomRobot() {
        recharge_.reset();
    }
    
+    if (monitor_) {
+        monitor_.reset();
+    }
+    
    try {
        unitree::robot::ChannelFactory::Instance()->Release();
        LOG_INFO("ChannelFactory released");
@@ -94,6 +97,9 @@ bool CustomRobot::initialize() {
        recharge_ = std::make_unique<Recharge>();
        recharge_->Init();

+        monitor_ = std::make_unique<Monitor>();
+        monitor_->initialize();
+
        if (!initializeMqtt()) {
            LOG_ERROR("Failed to initialize MQTT client");
            return false;
@@ -143,64 +149,15 @@ bool CustomRobot::start() {
    // }

    running_ = true;
+    
+    // Start continuous movement thread
+    traj_th_ = unitree::common::CreateRecurrentThreadEx("continuous_move", UT_CPU_ID_NONE, 100, 
+        &CustomRobot::continuousMoveLoop, this);
+    
    LOG_INFO("CustomRobot started successfully");
    return true;
 }

-void CustomRobot::trajControl()
-{
-    if (!controller_) {
-        LOG_ERROR("Controller not available for dynamic trajectory.");
-        return;
-    }
-    float vx = 0.3f;
-    float delta = 0.06f;
-    traj_count_ += traj_dt_;
-    std::vector<unitree::robot::go2::PathPoint> path;
-    for (int i=0; i<30; i++) {
-        unitree::robot::go2::PathPoint p;
-        float var = (traj_count_ + i * delta);
-        p.timeFromStart = i * delta;
-        p.x = vx * var;
-        p.y = 0.6 * sin(M_PI * vx * var);
-        p.yaw = 2 * 0.6 * vx * M_PI * cos(M_PI * vx * var);
-        p.vx = vx;
-        p.vy = M_PI * vx * (0.6 * cos(M_PI * vx * var));
-        p.vyaw = - M_PI * vx * 2 * 0.6 * vx * M_PI * sin(M_PI * vx * var);
-        path.push_back(p);
-    }
-
-    if (!controller_->TrajectoryFollow(path)) {
-        LOG_WARN("Failed to send dynamic trajectory command.");
-    }
-}
-
-bool CustomRobot::startTraj()
-{
-    if (traj_running_) {
-        LOG_WARN("Dynamic trajectory is already running.");
-        return true;
-    }
-
-    LOG_INFO("Starting dynamic trajectory control...");
-    traj_running_ = true;
-    traj_count_ = 0.0f;
-    
-    traj_th_ = unitree::common::CreateRecurrentThread(static_cast<int64_t>(traj_dt_ * 1000000), std::bind(&CustomRobot::trajControl, this));
-    
-    return true;
-}
-
-void CustomRobot::stopTraj()
-{
-    if (traj_running_) {
-        LOG_INFO("Stopping dynamic trajectory control...");
-        traj_running_ = false;
-        if (traj_th_) {
-            traj_th_.reset();
-        }
-    }
-}

 bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
    if (!initialized_ || !rsc_) {
@@ -747,12 +704,27 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
            return controller_->WalkUpright(message["param"]["flag"]);
        } else if (cmd == "CrossStep") {
            return controller_->CrossStep(message["param"]["flag"]);
-        } else if (cmd == "start_traj") {
-            return startTraj();
-        } else if (cmd == "stop_traj") {
-            stopTraj();
-            return true;
-        }
+        } else if (cmd == "ContinuousMove") {
+            if (!message.contains("param")) {
+                LOG_ERROR("ContinuousMove cmd missing 'param'");
+                return false;
+            }
+            auto param = message["param"];
+            if(param.contains("on")){
+                if(param["on"]){
+                    if (!param.contains("vx") || !param.contains("vy") || !param.contains("vyaw")) {
+                    LOG_ERROR("ContinuousMove missing vx, vy, or vyaw parameters");
+                    return false;
+                    }
+                    float vx = param["vx"];
+                    float vy = param["vy"];
+                    float vyaw = param["vyaw"];
+                    return startContinuousMove(vx, vy, vyaw);
+                }else if(!param["on"]){
+                    return stopContinuousMove();
+                }
+            }
+        } 
        else {
            LOG_ERROR("Unknown Sport command: " + cmd);
            return false;
@@ -763,25 +735,58 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
    }
 }

+// Continuous movement control implementation
+bool CustomRobot::startContinuousMove(float vx, float vy, float vyaw) {
+    if (!controller_) {
+        LOG_ERROR("Controller not initialized");
+        return false;
+    }
+    
+    continuous_vx_ = vx;
+    continuous_vy_ = vy;
+    continuous_vyaw_ = vyaw;
+    continuous_move_enabled_ = true;
+    
+    LOG_INFO("Started continuous move with vx=" + std::to_string(vx) + 
+             ", vy=" + std::to_string(vy) + ", vyaw=" + std::to_string(vyaw));
+    return true;
+}
+
+bool CustomRobot::stopContinuousMove() {
+    continuous_move_enabled_ = false;
+    
+    if (controller_) {
+        controller_->StopMove();
+    }
+    
+    LOG_INFO("Stopped continuous move");
+    return true;
+}
+
+void CustomRobot::continuousMoveLoop() {
+    if (!continuous_move_enabled_ || !controller_) {
+        return;
+    }
+    
+    float vx = continuous_vx_.load();
+    float vy = continuous_vy_.load();
+    float vyaw = continuous_vyaw_.load();
+    
+    controller_->Move(vx, vy, vyaw);
+}
+
 std::string CustomRobot::generateRandomClientId() const {
-    std::string baseId = config_.mqtt_client_id;
-    
-    std::string safeBaseId;
-    for (char c : baseId) {
-        if (std::isalnum(c) || c == '_' || c == '-') {
-            safeBaseId += c;
-        }
-    }
-    if (safeBaseId.empty()) {
-        safeBaseId = "unitree_go2_client";
-    }
-    
    std::random_device rd;
    std::mt19937 gen(rd());
-    std::uniform_int_distribution<> dis(1000, 9999);
+    std::uniform_int_distribution<> dis(0, 15);
    
    std::stringstream ss;
-    ss << safeBaseId << "_" << std::time(nullptr) << "_" << dis(gen);
+    ss << "go2_client_";
+    
+    // 生成8位随机十六进制字符R
+    for (int i = 0; i < 8; ++i) {
+        ss << std::hex << dis(gen);
+    }
    
    return ss.str();
 }
--- a/src/monitor.cpp
+++ b/src/monitor.cpp
@@ -40,37 +40,37 @@ void Monitor::sportModeStateCallback(const void* message) {
        static_cast<const unitree_go::msg::dds_::SportModeState_*>(message);
    
    // Print position data
-    std::cout << "Position: " 
-              << state->position()[0] << ", " 
-              << state->position()[1] << ", " 
-              << state->position()[2] << std::endl;
+    // std::cout << "Position: " 
+    //           << state->position()[0] << ", " 
+    //           << state->position()[1] << ", " 
+    //           << state->position()[2] << std::endl;
    
-    // Print IMU data
-    const auto& imu = state->imu_state();
+    // // Print IMU data
+    // const auto& imu = state->imu_state();
    
-    std::cout << "IMU Quaternion: " 
-              << imu.quaternion()[0] << ", " 
-              << imu.quaternion()[1] << ", " 
-              << imu.quaternion()[2] << ", " 
-              << imu.quaternion()[3] << std::endl;
+    // std::cout << "IMU Quaternion: " 
+    //           << imu.quaternion()[0] << ", " 
+    //           << imu.quaternion()[1] << ", " 
+    //           << imu.quaternion()[2] << ", " 
+    //           << imu.quaternion()[3] << std::endl;
    
-    std::cout << "IMU Gyroscope: " 
-              << imu.gyroscope()[0] << ", " 
-              << imu.gyroscope()[1] << ", " 
-              << imu.gyroscope()[2] << std::endl;
+    // std::cout << "IMU Gyroscope: " 
+    //           << imu.gyroscope()[0] << ", " 
+    //           << imu.gyroscope()[1] << ", " 
+    //           << imu.gyroscope()[2] << std::endl;
    
-    std::cout << "IMU Accelerometer: " 
-              << imu.accelerometer()[0] << ", " 
-              << imu.accelerometer()[1] << ", " 
-              << imu.accelerometer()[2] << std::endl;
+    // std::cout << "IMU Accelerometer: " 
+    //           << imu.accelerometer()[0] << ", " 
+    //           << imu.accelerometer()[1] << ", " 
+    //           << imu.accelerometer()[2] << std::endl;
    
-    std::cout << "IMU RPY: " 
-              << imu.rpy()[0] << ", " 
-              << imu.rpy()[1] << ", " 
-              << imu.rpy()[2] << std::endl;
+    // std::cout << "IMU RPY: " 
+    //           << imu.rpy()[0] << ", " 
+    //           << imu.rpy()[1] << ", " 
+    //           << imu.rpy()[2] << std::endl;
    
-    std::cout << "IMU Temperature: " << imu.temperature() << std::endl;
-    std::cout << "---" << std::endl;
+    // std::cout << "IMU Temperature: " << imu.temperature() << std::endl;
+    // std::cout << "---" << std::endl;
 }

 void Monitor::odometryCallback(const void* message) {
Author	SHA1	Message	Date
Sucan126	4ca81dbf4d	fix: 更新MQTT代理地址并修复注释中的拼写错误将MQTT代理地址从192.168.11.24更改为120.24.241.164以连接到新服务器修复generateRandomClientId函数注释中的多余字符'R'	2025-10-15 15:37:51 +08:00
Sucan126	6e669bb027	feat: 添加生成随机客户端ID的功能	2025-10-09 19:48:24 +08:00
Sucan126	73ce74ab04	fix(custom_robot): 修复ContinuousMove命令参数检查逻辑重构ContinuousMove命令处理逻辑，合并Start/Stop功能添加对'on'参数的检查，根据其值决定启动或停止连续移动修复参数缺失时的错误处理逻辑	2025-10-09 19:32:44 +08:00
Sucan126	827ccdb438	perf(continuous_move): 将连续运动线程频率从50Hz提升至100Hz以改善运动平滑度	2025-10-09 19:24:01 +08:00
Sucan126	2e60da5b5b	refactor(robot_control): 移除轨迹模式功能并添加连续移动控制移除原有的复杂轨迹模式实现（包括正弦波、圆形、正方形等路径生成），替换为更简单的连续移动控制接口，支持直接设置速度参数(vx,vy,vyaw)。修改了相关线程控制和运动命令处理逻辑，简化了代码结构。	2025-10-09 19:17:26 +08:00
Sucan126	06bf2fa208	feat(轨迹控制): 为start_traj命令添加参数解析功能扩展start_traj命令以支持从消息中解析轨迹参数，包括模式、比例、速度等配置项。这使得轨迹控制更加灵活可配置。	2025-10-09 18:26:07 +08:00
Sucan126	993ed111a6	feat(轨迹控制): 添加多种轨迹模式支持并重构轨迹生成逻辑新增轨迹模式枚举和参数结构体，支持正弦波、圆形、正方形、菱形和8字形五种轨迹模式重构轨迹生成逻辑为独立的生成函数，增加参数验证和配置灵活性	2025-10-09 17:49:08 +08:00
Sucan126	5b5d390bc4	refactor(monitor): 移除调试用的打印语句以清理输出	2025-10-09 16:56:53 +08:00
Sucan126	80060562e8	fix: 更新MQTT代理服务器IP地址将MQTT代理服务器地址从192.168.2.236更改为192.168.11.24以匹配新的网络配置	2025-10-09 16:43:13 +08:00
Sucan126	3a209734de	feat(robot): 添加监控模块到自定义机器人在自定义机器人中添加Monitor模块，用于监控机器人状态。包括在头文件中添加Monitor成员变量声明，在实现文件中添加初始化和资源释放逻辑。	2025-09-30 16:38:00 +08:00