当前位置：首页 > news >正文

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

news 2025/10/1 7:04:54

一、安装启动joy

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

二、确定键位

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

三、运行瑞尔曼节点

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_PATHros2 launch rm_driver rm_65_driver.launch.py

查看所有话题：

ros2 topic list

查看话题属性：

ros2 topic info -v /rm_driver/movel_cmd

查看消息定义：

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

一句话测试：

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

四、新建包

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

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_eulerclass 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 = Noneself.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 = msgif not self.initialized:self.initialized = Trueself.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('摇杆归零，机械臂停止！')@staticmethoddef quaternion_multiply(x1, y1, z1, w1, x2, y2, z2, w2):"""四元数乘法"""w = w1*w2 - x1*x2 - y1*y2 - z1*z2x = w1*x2 + x1*w2 + y1*z2 - z1*y2y = w1*y2 - x1*z2 + y1*w2 + z1*x2z = w1*z2 + x1*y2 - y1*x2 + z1*w2return x, y, z, wdef 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_posnew_y = self.curr_pose.position.y - msg.axes[0] * self.step_pos# Z 按钮控制new_z = self.curr_pose.position.zif msg.axes[7] == 1:new_z += self.step_zelif msg.axes[7] == -1:new_z -= self.step_z# 姿态增量roll_inc = msg.axes[2] * self.step_rot   # 右摇杆上下控制 Rollpitch_inc = msg.axes[3] * self.step_rot  # 右摇杆左右控制 Pitchyaw_inc = 0.0if msg.buttons[3]:  # 按钮3 顺时针旋转yaw_inc += self.step_yawif 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.wqx, qy, qz, qw = self.quaternion_multiply(qx, qy, qz, qw, *q_inc)# 发布 Movel 消息movel = Movel()movel.pose.position.x = new_xmovel.pose.position.y = new_ymovel.pose.position.z = new_zmovel.pose.orientation.x = qxmovel.pose.orientation.y = qymovel.pose.orientation.z = qzmovel.pose.orientation.w = qwmovel.speed = 40movel.trajectory_connect = Falsemovel.block = Falseself.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()