From 6c8a4a3b381cb152a4a4a79322610a805e48183e Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Tue, 23 Sep 2025 18:40:42 +0800 Subject: [PATCH] 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. - 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 | 1 - include/controller.hpp | 2 +- src/controller.cpp | 6 +-- src/custom_robot.cpp | 96 +++++++++++++++++++++++++++++++++++++++++- 4 files changed, 99 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 54d3832..c80d730 100644 --- a/CMakeLists.txt +++ b/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( diff --git a/include/controller.hpp b/include/controller.hpp index 4197b28..0a67f42 100644 --- a/include/controller.hpp +++ b/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>& path); + bool TrajectoryFollow(const std::vector& path); // Obstacle bool SwitchSet(bool enable); diff --git a/src/controller.cpp b/src/controller.cpp index 5cd7c69..eb0e015 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -339,9 +339,9 @@ bool Controller::CrossStep(bool flag) { }); } -bool Controller::TrajectoryFollow(const std::vector>& path) { - return ExecuteSportCmd([](auto* sc) { - return 0; +bool Controller::TrajectoryFollow(const std::vector& path) { + return ExecuteSportCmd([&path](auto* sc) { + return sc->TrajectoryFollow(path); }); } diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 6ce1fab..26ee3bb 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -8,10 +8,94 @@ #include #include #include +#include #include "recharge.hpp" +#include namespace custom { +std::vector createExampleTrajectory(const std::string& type) { + std::vector 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(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(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(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(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(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(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"]); }