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.

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);