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 @@
rclcpp
std_msgs
rosbag2_cpp
+ geometry_msgs
ament_lint_auto
ament_lint_common
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
+#include
+#include
+
+#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(
+ "/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(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(msg->linear.x);
+ float vy = static_cast(msg->linear.y);
+ float vyaw = static_cast(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::SharedPtr cmd_vel_subscriber_;
+ std::unique_ptr sport_client_;
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}
+