ROS2下利用遥控手柄控制瑞尔曼RM65-B机器人

一、安装启动joy

bash 复制代码

sudo apt install ros-humble-joy
ros2 run joy joy_node

二、确定键位

bash 复制代码

ros2 topic echo /joy --once

默认axis值:

axes:

-0.0
-0.0
-0.0
-0.0
1.0
1.0
0.0
0.0

测试得出遥感键位对应值，是浮点数：

左遥感上极限： axes[1]=1.0

左遥感下极限： axes[1]=-1.0

左遥感左极限： axes[0]=1.0

左遥感右极限： axes[0]=-1.0

左侧十字按键上：axes[7]=1.0

左侧十字按键上：axes[7]=-1.0

右遥感上极限： axes[3]=1.0

右遥感下极限： axes[3]=-1.0

右遥感左极限： axes[2]=1.0

右遥感右极限： axes[2]=-1.0

右侧X按钮：buttons[3]=1

右侧B按钮：buttons[1]=1

三、运行瑞尔曼节点

bash 复制代码

source ./install/setup.sh
export LD_LIBRARY_PATH=/home/zc9527/ros2_joy_rm65_ws/src/ros2_rm_robot/rm_driver/lib/linux_x86_c++_v1.1.1:$LD_LIBRARY_PATH

ros2 launch rm_driver rm_65_driver.launch.py

查看所有话题：

bash 复制代码

ros2 topic list

查看话题属性：

bash 复制代码

ros2 topic info -v /rm_driver/movel_cmd

查看消息定义：

bash 复制代码

source ./install/setup.sh
ros2 interface show rm_ros_interfaces/msg/Movel

定义如下：

geometry_msgs/Pose pose

Point position

float64 x

float64 y

float64 z

Quaternion orientation

float64 x 0

float64 y 0

float64 z 0

float64 w 1

uint8 speed

uint8 trajectory_connect #0 代表立即规划，1 代表和下一条轨迹一起规划，当为 1 时，轨迹不会立即执行

bool block

一句话测试：

bash 复制代码

ros2 topic pub /rm_driver/movel_cmd rm_ros_interfaces/msg/Movel "{pose: {position: {x: 0.3, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, speed: 50, trajectory_connect: 0, block: true}" -1

四、新建包

bash 复制代码

ros2 pkg create --build-type ament_python joystick_movel_control --dependencies rclpy rm_ros_interfaces

bash 复制代码

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from rm_ros_interfaces.msg import Movel
from geometry_msgs.msg import Pose
from std_msgs.msg import Empty
from tf_transformations import quaternion_from_euler

class JoystickMovelControl(Node):
    def __init__(self):
        super().__init__('joystick_movel_control')

        # 订阅手柄
        self.subscription = self.create_subscription(Joy, '/joy', self.joy_callback, 10)
        # 发布绝对末端位姿
        self.publisher = self.create_publisher(Movel, '/rm_driver/movel_cmd', 10)
        # 订阅末端位姿反馈
        self.sub_fb = self.create_subscription(Pose, '/rm_driver/udp_arm_position', self.feedback_callback, 10)
        # 停止命令
        self.stop_pub = self.create_publisher(Empty, '/rm_driver/move_stop_cmd', 10)

        # 当前末端位姿
        self.curr_pose = None
        self.initialized = False

        # 步长
        self.step_pos = 10.005  # XY 增量(m)
        self.step_z = 10.005    # Z 增量(m)
        self.step_rot = 10.01   # 角度增量(rad)
        self.step_yaw = 10.02   # Yaw增量(rad)

    def feedback_callback(self, msg: Pose):
        self.curr_pose = msg
        if not self.initialized:
            self.initialized = True
            self.get_logger().info(
                f'初始末端位姿: x={msg.position.x:.3f}, y={msg.position.y:.3f}, z={msg.position.z:.3f}, '
                f'orientation=({msg.orientation.x:.3f},{msg.orientation.y:.3f},{msg.orientation.z:.3f},{msg.orientation.w:.3f})'
            )

    def stop_arm(self):
        stop_msg = Empty()
        self.stop_pub.publish(stop_msg)
        self.get_logger().info('摇杆归零，机械臂停止！')

    @staticmethod
    def quaternion_multiply(x1, y1, z1, w1, x2, y2, z2, w2):
        """四元数乘法"""
        w = w1*w2 - x1*x2 - y1*y2 - z1*z2
        x = w1*x2 + x1*w2 + y1*z2 - z1*y2
        y = w1*y2 - x1*z2 + y1*w2 + z1*x2
        z = w1*z2 + x1*y2 - y1*x2 + z1*w2
        return x, y, z, w

    def joy_callback(self, msg: Joy):
        if not self.initialized or self.curr_pose is None:
            return

        # XY摇杆归零且Z按钮归零且右摇杆归零且Yaw按钮未按，立即停止
        if all(abs(a) < 1e-3 for a in msg.axes[:2]) and msg.axes[7] == 0 \
           and all(abs(a) < 1e-3 for a in msg.axes[2:4]) \
           and not (msg.buttons[3] or msg.buttons[1]):
            self.stop_arm()
            return

        # XY 绝对位姿
        new_x = self.curr_pose.position.x - msg.axes[1] * self.step_pos
        new_y = self.curr_pose.position.y - msg.axes[0] * self.step_pos

        # Z 按钮控制
        new_z = self.curr_pose.position.z
        if msg.axes[7] == 1:
            new_z += self.step_z
        elif msg.axes[7] == -1:
            new_z -= self.step_z

        # 姿态增量
        roll_inc = msg.axes[2] * self.step_rot   # 右摇杆上下控制 Roll
        pitch_inc = msg.axes[3] * self.step_rot  # 右摇杆左右控制 Pitch
        yaw_inc = 0.0
        if msg.buttons[3]:  # 按钮3 顺时针旋转
            yaw_inc += self.step_yaw
        if msg.buttons[1]:  # 按钮1 逆时针旋转
            yaw_inc -= self.step_yaw                             # 如果需要Yaw可用其它轴
        q_inc = quaternion_from_euler(roll_inc, pitch_inc, yaw_inc)

        # 当前姿态四元数
        qx, qy, qz, qw = self.curr_pose.orientation.x, self.curr_pose.orientation.y, \
                         self.curr_pose.orientation.z, self.curr_pose.orientation.w
        qx, qy, qz, qw = self.quaternion_multiply(qx, qy, qz, qw, *q_inc)

        # 发布 Movel 消息
        movel = Movel()
        movel.pose.position.x = new_x
        movel.pose.position.y = new_y
        movel.pose.position.z = new_z
        movel.pose.orientation.x = qx
        movel.pose.orientation.y = qy
        movel.pose.orientation.z = qz
        movel.pose.orientation.w = qw
        movel.speed = 40
        movel.trajectory_connect = False
        movel.block = False

        self.publisher.publish(movel)
        self.get_logger().info(
            f'发布 Pose: x={new_x:.3f}, y={new_y:.3f}, z={new_z:.3f}, roll_inc={roll_inc:.3f}, pitch_inc={pitch_inc:.3f}'
        )

def main(args=None):
    rclpy.init(args=args)
    node = JoystickMovelControl()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()