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