refactor(custom_robot): Reorganize trajectory control and remove example trajectory generation

- Moved the trajControl method to the private section of the CustomRobot class for better encapsulation.
- Introduced traj_dt_ and traj_count_ member variables to manage trajectory timing and state.
- Removed the createExampleTrajectory function to streamline the code and eliminate unused functionality in trajectory handling.

include/custom_robot.hpp

@@ -47,11 +47,11 @@ 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:
+    void trajControl();
     std::string generateRandomClientId() const;
     std::unique_ptr<Controller> controller_;
     // std::unique_ptr<LowController> low_controller_;
@@ -65,6 +65,8 @@ private:
     std::atomic<bool> initialized_;
     unitree::common::ThreadPtr traj_th_;
     std::atomic<bool> traj_running_;
+    float traj_dt_;
+    float traj_count_;
 };
 
 } // namespace custom

src/custom_robot.cpp

@@ -14,88 +14,6 @@
 
 namespace custom {
 
-std::vector<unitree::robot::go2::PathPoint> createExampleTrajectory(const std::string& type) {
-    std::vector<unitree::robot::go2::PathPoint> path;
-    const int num_points = 30;
-    const float total_time = 5.0f; // seconds
-
-    if (type == "line") {
-        // Move forward 1 meter in a straight line
-        for (int i = 0; i < num_points; ++i) {
-            unitree::robot::go2::PathPoint p;
-            float t = (static_cast<float>(i) / (num_points - 1)) * total_time;
-            p.timeFromStart = t;
-            p.x = 1.0f * (t / total_time); // x from 0 to 1
-            p.y = 0.0f;
-            p.yaw = 0.0f;
-            p.vx = 1.0f / total_time; // constant velocity
-            p.vy = 0.0f;
-            p.vyaw = 0.0f;
-            path.push_back(p);
-        }
-    } else if (type == "square") {
-        // Trace a 1x1 meter square
-        const int points_per_side = num_points / 4;
-        float side_time = total_time / 4.0f;
-        float speed = 1.0f / side_time;
-
-        // Side 1: move in +x
-        for (int i = 0; i < points_per_side; ++i) {
-            unitree::robot::go2::PathPoint p;
-            float t = (static_cast<float>(i) / (points_per_side -1)) * side_time;
-            p.timeFromStart = t;
-            p.x = speed * t; p.y = 0; p.yaw = 0;
-            p.vx = speed; p.vy = 0; p.vyaw = 0;
-            path.push_back(p);
-        }
-        // Side 2: move in +y
-        for (int i = 0; i < points_per_side; ++i) {
-            unitree::robot::go2::PathPoint p;
-            float t = (static_cast<float>(i) / (points_per_side - 1)) * side_time;
-            p.timeFromStart = side_time + t;
-            p.x = 1.0; p.y = speed * t; p.yaw = M_PI / 2.0f;
-            p.vx = 0; p.vy = speed; p.vyaw = 0;
-            path.push_back(p);
-        }
-        // Side 3: move in -x
-        for (int i = 0; i < points_per_side; ++i) {
-            unitree::robot::go2::PathPoint p;
-            float t = (static_cast<float>(i) / (points_per_side - 1)) * side_time;
-            p.timeFromStart = 2 * side_time + t;
-            p.x = 1.0 - speed * t; p.y = 1.0; p.yaw = M_PI;
-            p.vx = -speed; p.vy = 0; p.vyaw = 0;
-            path.push_back(p);
-        }
-        // Side 4: move in -y
-        for (int i = 0; i < points_per_side; ++i) {
-            unitree::robot::go2::PathPoint p;
-            float t = (static_cast<float>(i) / (points_per_side - 1)) * side_time;
-            p.timeFromStart = 3 * side_time + t;
-            p.x = 0; p.y = 1.0 - speed * t; p.yaw = -M_PI / 2.0f;
-            p.vx = 0; p.vy = -speed; p.vyaw = 0;
-            path.push_back(p);
-        }
-    } else if (type == "circle") {
-        // Move in a circle with radius 0.5m
-        float radius = 0.5f;
-        for (int i = 0; i < num_points; ++i) {
-            unitree::robot::go2::PathPoint p;
-            float t = (static_cast<float>(i) / (num_points - 1)) * total_time;
-            float angle = 2.0f * M_PI * (t / total_time);
-            p.timeFromStart = t;
-            p.x = radius * std::cos(angle);
-            p.y = radius * std::sin(angle);
-            p.yaw = angle + M_PI / 2.0f;
-            p.vx = -radius * (2.0f * M_PI / total_time) * std::sin(angle);
-            p.vy = radius * (2.0f * M_PI / total_time) * std::cos(angle);
-            p.vyaw = 2.0f * M_PI / total_time;
-            path.push_back(p);
-        }
-    }
-
-    return path;
-}
-
 CustomRobot::CustomRobot()
     : controller_(nullptr)
     , navigation_(nullptr)
@@ -103,7 +21,9 @@ CustomRobot::CustomRobot()
     , mqttClient_(nullptr)
     , running_(false)
     , initialized_(false) 
-    , traj_running_(false) {
+    , traj_running_(false)
+    , traj_dt_(0.02f)
+    , traj_count_(0.0f) {
     config_.loadDefaults();
     
     try {
@@ -234,24 +154,20 @@ void CustomRobot::trajControl()
         return;
     }
 
-    const float dt = 0.02f; // Control frequency 50Hz
+    float vx = 0.3f;
-    const float delta = 0.06f; // Time between path points
+    float delta = 0.06f;
-    static float count = 0;
+    traj_count_ += traj_dt_;
-    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 var = (traj_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.y = 0.6 * sin(M_PI * vx * var);
-        p.yaw = 2 * 0.6f * vx * M_PI * cos(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.6f * cos(M_PI * vx * var));
+        p.vy = M_PI * vx * (0.6 * cos(M_PI * vx * var));
-        p.vyaw = -M_PI * vx * 2 * 0.6f * vx * M_PI * sin(M_PI * vx * var);
+        p.vyaw = - M_PI * vx * 2 * 0.6 * vx * M_PI * sin(M_PI * vx * var);
         path.push_back(p);
     }
 
@@ -269,9 +185,9 @@ bool CustomRobot::startTraj()
 
     LOG_INFO("Starting dynamic trajectory control...");
     traj_running_ = true;
+    traj_count_ = 0.0f;
     
-    const float dt = 0.02f; // 50Hz
+    traj_th_ = unitree::common::CreateRecurrentThread(static_cast<int64_t>(traj_dt_ * 1000000), std::bind(&CustomRobot::trajControl, this));
-    traj_th_ = unitree::common::CreateRecurrentThread(dt * 1000000, std::bind(&CustomRobot::trajControl, this));
     
     return true;
 }
@@ -832,18 +748,6 @@ 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 == "TrajectoryFollow") {
-            if (!message.contains("param") || !message["param"].contains("path_type")) {
-                LOG_ERROR("TrajectoryFollow cmd missing 'path_type' parameter");
-                return false;
-            }
-            std::string path_type = message["param"]["path_type"];
-            auto path = createExampleTrajectory(path_type);
-            if (path.empty()) {
-                LOG_ERROR("Unknown or empty path_type for TrajectoryFollow: " + path_type);
-                return false;
-            }
-            return controller_->TrajectoryFollow(path);
         } else if (cmd == "start_traj") {
             return startTraj();
         } else if (cmd == "stop_traj") {