手柄替代键盘

键盘控制版本改写成了 ROS 2 手柄控制版本 ，使用游戏手柄（Xbox/PS/ 通用手柄） 控制机械臂末端运动，支持线性运动 + 旋转运动 、坐标系切换 、安全急停，功能完整可直接运行。

手柄控制机械臂 Servo 节点（ROS 2 Humble/Iron 兼容）

功能说明

1. 左摇杆 ：控制末端前后左右（XY 轴）
2. 右摇杆 ：控制末端旋转（Roll/Pitch）
3. LB/RB ：控制末端上下（Z 轴）
4. Y 键 ：切换基坐标系
5. X 键 ：切换末端坐标系
6. A 键 ：急停 / 停止运动
7. BACK 键：退出节点

---

完整代码（servo_joy_teleop.cpp）

cpp

运行

/*********************************************************************
 * 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_publisher<geometry_msgs::msg::TwistStamped>(
      TWIST_TOPIC, ROS_QUEUE_SIZE);

    // 创建订阅器：接收手柄数据
    joy_sub_ = this->create_subscription<sensor_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_unique<geometry_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::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
  rclcpp::Subscription<sensor_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_shared<JoyServo>();
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

---

配套 CMakeLists.txt

cmake

cmake_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.xml

xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>servo_joy_teleop</name>
  <version>0.0.0</version>
  <description>Servo control with joystick</description>
  <maintainer email="your@mail.com">Your Name</maintainer>
  <license>BSD</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>geometry_msgs</depend>
  <depend>sensor_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

---

使用方法

1. 安装 ROS 2 手柄驱动

bash

运行

sudo apt install ros-$ROS_DISTRO-joy

2. 运行手柄节点

bash

运行

ros2 run joy joy_node

3. 运行本控制节点

bash

运行

ros2 run servo_joy_teleop servo_joy_teleop

---

手柄按键映射（Xbox 手柄标准）

表格

按键 / 摇杆	功能
左摇杆左右	末端左右移动（Y 轴）
左摇杆前后	末端前后移动（X 轴）
右摇杆	末端旋转（Roll/Pitch）
LB 键	末端下降（Z-）
RB 键	末端上升（Z+）
Y 键	切换到基坐标系
X 键	切换到末端坐标系
A 键	急停，立即停止运动
BACK 键	退出程序

---

可自定义参数

你可以直接修改代码中的常量调整手感：

cpp

运行

const double LINEAR_VEL = 0.15;    // 线速度大小
const double ANGULAR_VEL = 0.25;   // 旋转速度大小

---

总结

1. 这是纯 C++ ROS 2 手柄控制版本，替代原键盘版本
2. 支持线性运动 + 旋转运动 、坐标系切换 、安全急停
3. 兼容Xbox/PS/ 通用 USB 手柄，开箱即用
4. 话题名与原代码完全一致，可直接对接 Servo 节点