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()
相关推荐
Jason_zhao_MR3 小时前
RK3576机器人核心:三屏异显+八路摄像头,重塑机器人交互与感知
linux·人工智能·嵌入式硬件·计算机视觉·机器人·嵌入式·交互
音视频牛哥4 小时前
SmartMediaKit 在检测机器人中的视频链路重构:从播放(RTSP)到二次水印编码再推流(RTSP|RTMP)
机器人·音视频·大牛直播sdk·rtsp二次编码·rtsp流二次水印保存mp4·rtsp流添加动态水印·检测机器人rtsp低延迟
具身新纪元5 小时前
现代机器人学习入门:一份来自Hugging Face与牛津大学的综合教程开源SOTA资源库
学习·机器人
搞科研的小刘选手5 小时前
【多所高校主办】第七届机器人、智能控制与人工智能国际学术会议(RICAI 2025)
人工智能·机器学习·ai·机器人·无人机·传感器·智能控制
深眸财经8 小时前
蓝思科技Q3营收创同期新高,人形机器人及四足机器狗出货规模攀升
人工智能·科技·机器人
No0d1es10 小时前
电子学会青少年机器人技术(三级)等级考试试卷-实操题(2025年9月)
青少年编程·机器人·电子学会·真题·三级·实际操作
柠檬甜不甜呀11 小时前
海康相机与机器人标定
数码相机·计算机视觉·机器人
CyberSoma14 小时前
机器人模仿学习运动基元数学编码方法还有用吗?
人工智能·算法·计算机视觉·机器人
MarkHD15 小时前
Dify从入门到精通 第33天 基于GPT-4V构建图片描述生成器与视觉问答机器人
人工智能·机器人
泽众云测试16 小时前
PRA(流程机器人自动化)与智能体(AI Agent)主要区别与分析
机器人·自动化·rpa