一、安装启动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_PATH
ros2 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_interfacesimport 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()
