From 76aa4f299420a7faca45c4aea69f6bcfae46aa6a Mon Sep 17 00:00:00 2001
From: Wuhall <focuson@yeah.net>
Date: Thu, 11 Dec 2025 16:41:22 +0800
Subject: [PATCH] =?UTF-8?q?=E8=AE=A2=E9=98=85=E6=A0=87=E5=87=86=E7=9A=84RO?=
 =?UTF-8?q?S2=20cmd=5Fvel=E8=AF=9D=E9=A2=98=EF=BC=88geometry=5Fmsgs/Twist?=
 =?UTF-8?q?=EF=BC=89=EF=BC=8C=E5=B9=B6=E5=B0=86=E9=80=9F=E5=BA=A6=E5=91=BD?=
 =?UTF-8?q?=E4=BB=A4=E8=BD=AC=E6=8D=A2=E4=B8=BAunitree=E6=9C=BA=E5=99=A8?=
 =?UTF-8?q?=E4=BA=BA=E7=9A=84=E8=BF=90=E5=8A=A8=E6=8E=A7=E5=88=B6=E5=91=BD?=
 =?UTF-8?q?=E4=BB=A4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 example/src/CMakeLists.txt             |  7 +++
 example/src/package.xml                |  1 +
 example/src/src/cmd_vel_subscriber.cpp | 63 ++++++++++++++++++++++++++
 3 files changed, 71 insertions(+)
 create mode 100644 example/src/src/cmd_vel_subscriber.cpp

diff --git a/example/src/CMakeLists.txt b/example/src/CMakeLists.txt
index 81950a02..4b6474d2 100755
--- a/example/src/CMakeLists.txt
+++ b/example/src/CMakeLists.txt
@@ -27,6 +27,7 @@ set (
   rclcpp
   std_msgs
   rosbag2_cpp
+  geometry_msgs
 )
 
 # find dependencies
@@ -37,6 +38,7 @@ find_package(unitree_api REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rosbag2_cpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 
 add_executable(low_level_ctrl src/low_level_ctrl.cpp src/common/motor_crc.cpp)
@@ -50,6 +52,9 @@ add_executable(read_wireless_controller src/read_wireless_controller.cpp)
 add_executable(go2_sport_client src/go2/go2_sport_client.cpp src/common/ros2_sport_client.cpp)
 target_compile_features(go2_sport_client PRIVATE cxx_std_20)
 
+add_executable(cmd_vel_subscriber src/cmd_vel_subscriber.cpp src/common/ros2_sport_client.cpp)
+target_compile_features(cmd_vel_subscriber PRIVATE cxx_std_20)
+
 add_executable(go2_stand_example src/go2/go2_stand_example.cpp src/common/motor_crc.cpp)
 target_compile_features(go2_stand_example PRIVATE cxx_std_20)
 
@@ -96,6 +101,7 @@ ament_target_dependencies(read_motion_state ${DEPENDENCY_LIST})
 ament_target_dependencies(read_wireless_controller ${DEPENDENCY_LIST})
 #ament_target_dependencies(record_bag ${DEPENDENCY_LIST})
 ament_target_dependencies(go2_sport_client ${DEPENDENCY_LIST})
+ament_target_dependencies(cmd_vel_subscriber ${DEPENDENCY_LIST})
 ament_target_dependencies(go2_stand_example ${DEPENDENCY_LIST})
 
 
@@ -137,6 +143,7 @@ install(TARGETS
         read_wireless_controller
         go2_sport_client
         go2_stand_example
+        cmd_vel_subscriber
         DESTINATION)
 
 
diff --git a/example/src/package.xml b/example/src/package.xml
index 89cc2c19..9c3bc858 100755
--- a/example/src/package.xml
+++ b/example/src/package.xml
@@ -15,6 +15,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>rosbag2_cpp</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/example/src/src/cmd_vel_subscriber.cpp b/example/src/src/cmd_vel_subscriber.cpp
new file mode 100644
index 00000000..c32981dd
--- /dev/null
+++ b/example/src/src/cmd_vel_subscriber.cpp
@@ -0,0 +1,63 @@
+/**********************************************************************
+ Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
+***********************************************************************/
+/**
+ * This example demonstrates how to subscribe to standard ROS2 cmd_vel topic
+ * and convert it to Unitree robot motion commands
+ **/
+#include <geometry_msgs/msg/twist.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <unitree_api/msg/request.hpp>
+
+#include "common/ros2_sport_client.h"
+
+class CmdVelSubscriber : public rclcpp::Node {
+ public:
+  CmdVelSubscriber() : Node("cmd_vel_subscriber") {
+    // Subscribe to standard cmd_vel topic
+    cmd_vel_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
+        "/cmd_vel", 10,
+        [this](const geometry_msgs::msg::Twist::SharedPtr msg) {
+          cmdVelCallback(msg);
+        });
+
+    // Create sport client for sending motion commands
+    sport_client_ = std::make_unique<SportClient>(this);
+
+    RCLCPP_INFO(this->get_logger(),
+                "CmdVel subscriber initialized. Listening to /cmd_vel topic.");
+  }
+
+ private:
+  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) {
+    // Extract velocity components from Twist message
+    // For mobile robots: linear.x = forward velocity, angular.z = rotation
+    float vx = static_cast<float>(msg->linear.x);
+    float vy = static_cast<float>(msg->linear.y);
+    float vyaw = static_cast<float>(msg->angular.z);
+
+    // Log received velocities
+    RCLCPP_INFO_THROTTLE(
+        this->get_logger(), *this->get_clock(), 1000,
+        "Received cmd_vel: vx=%.3f, vy=%.3f, vyaw=%.3f", vx, vy, vyaw);
+
+    // Convert to Unitree motion command
+    // Create request message
+    unitree_api::msg::Request req;
+    
+    // Send velocity command using SportClient
+    sport_client_->Move(req, vx, vy, vyaw);
+  }
+
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber_;
+  std::unique_ptr<SportClient> sport_client_;
+};
+
+int main(int argc, char* argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<CmdVelSubscriber>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
+