From 4a062046451a94db2f8994be63e7f6f7d1479131 Mon Sep 17 00:00:00 2001
From: Sucan126 <632190820@qq.com>
Date: Tue, 23 Sep 2025 18:55:31 +0800
Subject: [PATCH] feat(trajectory): Implement dynamic trajectory control in
 CustomRobot

- Added methods for trajectory control: trajControl, startTraj, and stopTraj.
- Integrated trajectory control into command processing for starting and stopping dynamic trajectories.
- Introduced a recurrent thread for continuous trajectory updates at a frequency of 50Hz.
- Initialized trajectory control state management with atomic flags.
---
 include/custom_robot.hpp |  7 ++++
 src/custom_robot.cpp     | 71 +++++++++++++++++++++++++++++++++++++++-
 2 files changed, 77 insertions(+), 1 deletion(-)

diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp
index eb9b2cf..da5692e 100644
--- a/include/custom_robot.hpp
+++ b/include/custom_robot.hpp
@@ -15,6 +15,7 @@
 #include <vector>
 #include <nlohmann/json.hpp>
 #include <unitree/robot/go2/robot_state/robot_state_client.hpp>
+#include <unitree/common/thread/recurrent_thread.hpp>
 
 namespace custom {
 
@@ -47,6 +48,10 @@ public:
     bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message);
     void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);
 
+    void trajControl();
+    bool startTraj();
+    void stopTraj();
+
 private:
     std::string generateRandomClientId() const;
     std::unique_ptr<Controller> controller_;
@@ -59,6 +64,8 @@ private:
     CustomConfig config_;  
     std::atomic<bool> running_;
     std::atomic<bool> initialized_;
+    unitree::common::ThreadPtr traj_th_;
+    std::atomic<bool> traj_running_;
 };
 
 } // namespace custom
diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp
index 26ee3bb..5426ab7 100644
--- a/src/custom_robot.cpp
+++ b/src/custom_robot.cpp
@@ -102,7 +102,8 @@ CustomRobot::CustomRobot()
     , rsc_(nullptr)
     , mqttClient_(nullptr)
     , running_(false)
-    , initialized_(false) {
+    , initialized_(false)
+    , traj_running_(false) {
     config_.loadDefaults();
     
     try {
@@ -116,6 +117,8 @@ CustomRobot::CustomRobot()
 CustomRobot::~CustomRobot() {
     LOG_INFO("Shutting down CustomRobot");
     
+    stopTraj();
+
     if (mqttClient_) {
         mqttClient_->stopMessageProcessor();
         mqttClient_->disconnect();
@@ -225,6 +228,67 @@ bool CustomRobot::start() {
     return true;
 }
 
+void CustomRobot::trajControl()
+{
+    if (!controller_) {
+        LOG_ERROR("Controller not available for dynamic trajectory.");
+        return;
+    }
+
+    const float dt = 0.02f; // Control frequency 50Hz
+    const float delta = 0.06f; // Time between path points
+    static float count = 0;
+    count += dt;
+
+    std::vector<unitree::robot::go2::PathPoint> path;
+    for (int i=0; i<30; i++) {
+        unitree::robot::go2::PathPoint p;
+        float var = (count + i * delta);
+        float vx = 0.3f; // Example constant velocity
+
+        p.timeFromStart = i * delta;
+        p.x = vx * var;
+        p.y = 0.6f * sin(M_PI * vx * var);
+        p.yaw = 2 * 0.6f * vx * M_PI * cos(M_PI * vx * var);
+        p.vx = vx;
+        p.vy = M_PI * vx * (0.6f * cos(M_PI * vx * var));
+        p.vyaw = -M_PI * vx * 2 * 0.6f * 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;
+    
+    const float dt = 0.02f; // 50Hz
+    traj_th_ = unitree::common::CreateRecurrentThread(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_->Stop();
+            traj_th_.reset();
+        }
+    }
+}
+
 bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
     if (!initialized_ || !rsc_) {
         LOG_ERROR("Robot not initialized or stateClient is null");
@@ -797,6 +861,11 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
             return controller_->TrajectoryFollow(path);
         } else if (cmd == "FootRaiseHeight") {
             return controller_->FootRaiseHeight(message["param"]["height"]);
+        } else if (cmd == "start_traj") {
+            return startTraj();
+        } else if (cmd == "stop_traj") {
+            stopTraj();
+            return true;
         }
         else {
             LOG_ERROR("Unknown Sport command: " + cmd);