Subscribe to the standard ROS2 cmd_vel topic (geometry_msgs/Twist) and convert velocity commands into motion control commands for the Unitree robot

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)
 
 

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>

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;
+}
+