手柄替代键盘

键盘控制版本改写成了 ROS 2 手柄控制版本使用游戏手柄Xbox/PS/ 通用手柄控制机械臂末端运动支持线性运动 旋转运动、坐标系切换、安全急停功能完整可直接运行。手柄控制机械臂 Servo 节点ROS 2 Humble/Iron 兼容功能说明左摇杆控制末端前后左右XY 轴右摇杆控制末端旋转Roll/PitchLB/RB控制末端上下Z 轴Y 键切换基坐标系X 键切换末端坐标系A 键急停 / 停止运动BACK 键退出节点完整代码servo_joy_teleop.cppcpp运行/********************************************************************* * Software License Agreement (BSD License) *********************************************************************/ #include rclcpp/rclcpp.hpp #include geometry_msgs/msg/twist_stamped.hpp #include sensor_msgs/msg/joy.hpp #include signal.h #include stdlib.h // 话题名称与Servo节点保持一致 const std::string TWIST_TOPIC /servo_demo_node/delta_twist_cmds; const std::string JOY_TOPIC /joy; const size_t ROS_QUEUE_SIZE 10; // 坐标系定义 const std::string EEF_FRAME_ID panda_hand; const std::string BASE_FRAME_ID panda_link0; // 安全速度参数可根据需求调整 const double LINEAR_VEL 0.15; // 线速度 const double ANGULAR_VEL 0.25; // 角速度 // 全局退出信号 bool g_quit false; void quit(int sig) { (void)sig; g_quit true; rclcpp::shutdown(); exit(0); } class JoyServo : public rclcpp::Node { public: JoyServo() : Node(servo_joy_teleop), frame_id_(BASE_FRAME_ID) { // 创建发布器发送Twist指令给Servo节点 twist_pub_ this-create_publishergeometry_msgs::msg::TwistStamped( TWIST_TOPIC, ROS_QUEUE_SIZE); // 创建订阅器接收手柄数据 joy_sub_ this-create_subscriptionsensor_msgs::msg::Joy( JOY_TOPIC, ROS_QUEUE_SIZE, std::bind(JoyServo::joyCallback, this, std::placeholders::_1)); RCLCPP_INFO(this-get_logger(), ); RCLCPP_INFO(this-get_logger(), 手柄控制机械臂末端); RCLCPP_INFO(this-get_logger(), ); RCLCPP_INFO(this-get_logger(), 左摇杆前后左右(XY) | 右摇杆旋转(Roll/Pitch)); RCLCPP_INFO(this-get_logger(), LB下降(Z-) | RB上升(Z)); RCLCPP_INFO(this-get_logger(), Y基坐标系 | X末端坐标系 | A急停); RCLCPP_INFO(this-get_logger(), BACK退出节点); RCLCPP_INFO(this-get_logger(), ); } private: /** * brief 手柄数据回调函数 */ void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy) { auto twist_msg std::make_uniquegeometry_msgs::msg::TwistStamped(); twist_msg-header.stamp this-now(); twist_msg-header.frame_id frame_id_; // 坐标系切换 // Y键 → 基坐标系 if (joy-buttons[3] 1) { frame_id_ BASE_FRAME_ID; RCLCPP_INFO(this-get_logger(), 已切换至基坐标系); } // X键 → 末端坐标系 if (joy-buttons[2] 1) { frame_id_ EEF_FRAME_ID; RCLCPP_INFO(this-get_logger(), 已切换至末端坐标系); } // 急停A键 if (joy-buttons[0] 1) { // 发布零速度指令 twist_pub_-publish(std::move(twist_msg)); RCLCPP_WARN(this-get_logger(), ⚠️ 急停已停止所有运动); return; } // 退出BACK键 if (joy-buttons[6] 1) { RCLCPP_INFO(this-get_logger(), 退出手柄控制节点); rclcpp::shutdown(); return; } // 轴控制 // 左摇杆左右 → Y轴左右 twist_msg-twist.linear.y LINEAR_VEL * joy-axes[0]; // 左摇杆前后 → X轴前后 twist_msg-twist.linear.x LINEAR_VEL * joy-axes[1]; // 右摇杆左右 → 旋转(Roll) twist_msg-twist.angular.z ANGULAR_VEL * joy-axes[3]; // 右摇杆上下 → 旋转(Pitch) twist_msg-twist.angular.y ANGULAR_VEL * joy-axes[4]; // LB/RB → Z轴上下 if (joy-buttons[4] 1) { // LB下降 twist_msg-twist.linear.z -LINEAR_VEL; } if (joy-buttons[5] 1) { // RB上升 twist_msg-twist.linear.z LINEAR_VEL; } // 发布指令 twist_pub_-publish(std::move(twist_msg)); } // 成员变量 rclcpp::Publishergeometry_msgs::msg::TwistStamped::SharedPtr twist_pub_; rclcpp::Subscriptionsensor_msgs::msg::Joy::SharedPtr joy_sub_; std::string frame_id_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); signal(SIGINT, quit); auto node std::make_sharedJoyServo(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }配套 CMakeLists.txtcmakecmake_minimum_required(VERSION 3.8) project(servo_joy_teleop) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang) add_compile_options(-Wall -Wextra -Wpedantic) endif() # 依赖 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) add_executable(servo_joy_teleop src/servo_joy_teleop.cpp) ament_target_dependencies( servo_joy_teleop rclcpp geometry_msgs sensor_msgs ) install(TARGETS servo_joy_teleop DESTINATION lib/${PROJECT_NAME}) ament_package()配套 package.xmlxml?xml version1.0? ?xml-model hrefhttp://download.ros.org/schema/package_format3.xsd schematypenshttp://www.w3.org/2001/XMLSchema? package format3 nameservo_joy_teleop/name version0.0.0/version descriptionServo control with joystick/description maintainer emailyourmail.comYour Name/maintainer licenseBSD/license buildtool_dependament_cmake/buildtool_depend dependrclcpp/depend dependgeometry_msgs/depend dependsensor_msgs/depend test_dependament_lint_auto/test_depend test_dependament_lint_common/test_depend export build_typeament_cmake/build_type /export /package使用方法1. 安装 ROS 2 手柄驱动bash运行sudo apt install ros-$ROS_DISTRO-joy2. 运行手柄节点bash运行ros2 run joy joy_node3. 运行本控制节点bash运行ros2 run servo_joy_teleop servo_joy_teleop手柄按键映射Xbox 手柄标准表格按键 / 摇杆功能左摇杆左右末端左右移动Y 轴左摇杆前后末端前后移动X 轴右摇杆末端旋转Roll/PitchLB 键末端下降Z-RB 键末端上升ZY 键切换到基坐标系X 键切换到末端坐标系A 键急停立即停止运动BACK 键退出程序可自定义参数你可以直接修改代码中的常量调整手感cpp运行const double LINEAR_VEL 0.15; // 线速度大小 const double ANGULAR_VEL 0.25; // 旋转速度大小总结这是纯 C ROS 2 手柄控制版本替代原键盘版本支持线性运动 旋转运动、坐标系切换、安全急停兼容Xbox/PS/ 通用 USB 手柄开箱即用话题名与原代码完全一致可直接对接 Servo 节点