refactor(controller): Update TrajectoryFollow method signature and improve path handling

- Changed the signature of TrajectoryFollow to accept a vector of unitree::robot::go2::PathPoint instead of std::array<float, 6>.
- Updated the implementation of TrajectoryFollow in controller.cpp to utilize the new path type.
- Removed unnecessary status message in CMakeLists.txt related to unitree_sdk2 detection.
- Added a new function in custom_robot.cpp to create example trajectories for different path types (line, square, circle) and integrated it into the command processing for TrajectoryFollow.

CMakeLists.txt

@@ -18,7 +18,6 @@ if(NOT PahoMqttC_FOUND)
 endif()
 
 find_package(unitree_sdk2 REQUIRED)
-message(STATUS "Found unitree_sdk2 in: ${unitree_sdk2_DIR}")
 
 # Include directories
 include_directories(

include/controller.hpp

@@ -60,7 +60,7 @@ public:
     bool FreeAvoid(bool flag);
     bool WalkUpright(bool flag);
     bool CrossStep(bool flag);
-    bool TrajectoryFollow(const std::vector<std::array<float, 6>>& path);
+    bool TrajectoryFollow(const std::vector<unitree::robot::go2::PathPoint>& path);
     
     // Obstacle 
     bool SwitchSet(bool enable);

src/controller.cpp

@@ -339,9 +339,9 @@ bool Controller::CrossStep(bool flag) {
     });
 }
 
-bool Controller::TrajectoryFollow(const std::vector<std::array<float, 6>>& path) {
+bool Controller::TrajectoryFollow(const std::vector<unitree::robot::go2::PathPoint>& path) {
-    return ExecuteSportCmd([](auto* sc) {
+    return ExecuteSportCmd([&path](auto* sc) {
-        return 0;
+        return sc->TrajectoryFollow(path);
     });
 }
 

src/custom_robot.cpp

@@ -8,10 +8,94 @@
 #include <iomanip>
 #include <nlohmann/json.hpp>
 #include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/go2/sport/sport_client.hpp>
 #include "recharge.hpp"
+#include <cmath>
 
 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)
@@ -700,7 +784,17 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
         } else if (cmd == "CrossStep") {
             return controller_->CrossStep(message["param"]["flag"]);
         } else if (cmd == "TrajectoryFollow") {
-            return controller_->TrajectoryFollow(message["param"]["path"]);
+            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 == "FootRaiseHeight") {
             return controller_->FootRaiseHeight(message["param"]["height"]);
         }