ros2 humble 控制真实机械臂（以lerobot为例）

基础版

0.确保串口访问权限

sudo chmod 666 /dev/ttyARM0  # 确保串口访问权限

1.下载 lerobot 驱动功能包

git clone https://gitee.com/kong-yue1/lerobot_devices.git

2.编写控制节点（完整代码）

主要功能是与 Feetech 电机总线进行通信，以控制机械臂的关节运动，并发布关节的实际状态信息。

# 与 Feetech 电机总线进行通信，以控制机械臂的关节运动，并发布关节的实际状态信息

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState  # JointState: ROS标准消息类型，用于描述关节状态（位置、速度、力矩）
from lerobot_driver.motors.feetech import FeetechMotorsBus  # 自定义的Feetech电机总线通信接口
import time
from serial.serialutil import SerialException # 处理串口通信异常

class HardwareInterface(Node):
    def __init__(self):
        super().__init__('hardware_interface') # 创建一个名为 hardware_interface 的节点

        # 订阅器：监听joint_states话题，接收目标关节状态（如来自MoveIt的指令）
        self.subscription = self.create_subscription(JointState, 'joint_states', self.joint_state_callback, 10)
        # 发布器：发布real_joint_states话题，反馈实际关节状态（从电机读取）
        self.publisher_ = self.create_publisher(JointState, 'real_joint_states', 10)
        # 定时器：每20ms（50Hz）调用一次publish_real_joint_states，实时更新关节状态
        self.timer = self.create_timer(0.02, self.publish_real_joint_states)  # 0.02 -> 50 Hz

        # 电机总线初始化：通过串口/dev/ttyARM0连接Feetech电机总线
        self.motors_bus = FeetechMotorsBus(
            port="/dev/ttyARM0", # 需要根据自己情况修改
            motors={},
        )
        self.motors_bus.connect()

        # Define the mapping between joint names and motor IDs 关节与电机ID映射
        self.joint_to_motor_id = {
            "Joint_1": 1,
            "Joint_2": 2,
            "Joint_3": 3,
            "Joint_4": 4,
            "Joint_5": 5,
            "Joint_6": 6,
            "Joint_Gripper": 7,
        }

        # Initialize motors with the correct IDs and model
        self.motor_names = list(self.joint_to_motor_id.keys())
        self.motors_bus.motors = {name: (self.joint_to_motor_id[name], "sts3215") for name in self.motor_names} # 初始化电机型号为sts3215

        self.configure_motors() # 配置电机参数
        self.set_torque(True)   # 启用/禁用电机扭矩

    def configure_motors(self):
        for name in self.motor_names:
            self.motors_bus.write("Goal_Speed", 100, name)  # Set speed to 100
            self.motors_bus.write("Acceleration", 50, name)  # Set acceleration to 50

    def set_torque(self, enable):
        torque_value = 1 if enable else 0
        for name in self.motor_names:
            self.motors_bus.write("Torque_Enable", torque_value, name)
        self.get_logger().info(f"Torque {'enabled' if enable else 'disabled'} for all motors.")

    def joint_state_callback(self, msg):
        motor_ids = []
        motor_values = []
        motor_models = []

        for name, position in zip(msg.name, msg.position):
            if name in self.motor_names:
                motor_id = self.joint_to_motor_id[name]
                motor_value = int((-position + 3.14) * (4095 / (2 * 3.14)))  # Convert radians to motor value
                motor_ids.append(motor_id)
                motor_values.append(motor_value)
                motor_models.append("sts3215")

        if motor_ids:
            self.motors_bus.write_with_motor_ids(motor_models, motor_ids, "Goal_Position", motor_values)
            # self.get_logger().info(f"Joint positions: {list(zip(msg.name, motor_values))}")

    def publish_real_joint_states(self):
        motor_ids = list(self.joint_to_motor_id.values())   # 从字典 self.joint_to_motor_id 中提取所有电机ID
        motor_models = ["sts3215"] * len(motor_ids)         # 创建一个与电机数量相同的列表，所有元素均为电机型号 "sts3215"
        try:
            positions = self.motors_bus.read_with_motor_ids(motor_models, motor_ids, "Present_Position") # 调用 FeetechMotorsBus 的方法，通过电机ID和型号读取当前值
        except (ConnectionError, SerialException) as e:
            self.get_logger().error(f"Connection error: {e}")
            self.get_logger().info("Attempting to reconnect...")
            while True:
                try:
                    self.motors_bus.reconnect()
                    self.get_logger().info("Reconnected to motors.")
                    return
                except Exception as e:
                    self.get_logger().error(f"Reconnection failed: {e}")
                    time.sleep(1)  # Wait for 1 second before retrying

        joint_state_msg = JointState()
        joint_state_msg.header.stamp = self.get_clock().now().to_msg()
        joint_state_msg.name = self.motor_names
        joint_state_msg.position = [((-pos / 4095.0) * (2 * 3.14) - 3.14) for pos in positions]  # Convert motor value to radians

        self.publisher_.publish(joint_state_msg)
        # self.get_logger().info(f"Published real joint states: {positions}")
        self.get_logger().info(f"Published real joint states (radians): {joint_state_msg.position}")

def main(args=None):
    rclpy.init(args=args)
    hardware_interface = HardwareInterface()
    rclpy.spin(hardware_interface)
    hardware_interface.motors_bus.disconnect()
    hardware_interface.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

进阶版

针对MoveIt2集成所需的关键修改步骤及代码：

1. 添加FollowJointTrajectory动作服务器

# 在文件顶部添加新的依赖
from rclpy.action import ActionServer
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

# 在__init__方法中替换原有订阅器，添加动作服务器
def __init__(self):
    super().__init__('hardware_interface')
    
    # 替换原有订阅器和发布器
    self._action_server = ActionServer(
        self,
        FollowJointTrajectory,
        '/joint_trajectory_controller/follow_joint_trajectory',
        self.execute_trajectory_callback
    )
    
    self.joint_pub = self.create_publisher(JointState, '/joint_states', 10)
    self.timer = self.create_timer(0.02, self.publish_real_joint_states)

2. 实现轨迹执行回调函数

def execute_trajectory_callback(self, goal_handle):
    trajectory = goal_handle.request.trajectory
    self.get_logger().info("Received new trajectory with {} points".format(len(trajectory.points)))

    # 验证关节名称匹配
    if set(trajectory.joint_names) != set(self.joint_to_motor_id.keys()):
        goal_handle.abort()
        return FollowJointTrajectory.Result(
            error_code=FollowJointTrajectory.Result.INVALID_JOINTS
        )

    # 执行轨迹点
    for point in trajectory.points:
        start_time = self.get_clock().now()
        self.execute_positions(point.positions)
        
        # 非阻塞式时间等待
        target_duration = rclpy.duration.Duration(
            seconds=point.time_from_start.sec,
            nanoseconds=point.time_from_start.nanosec
        )
        while (self.get_clock().now() - start_time) < target_duration:
            rclpy.spin_once(self, timeout_sec=0.001)  # 保持回调处理

    goal_handle.succeed()
    return FollowJointTrajectory.Result(error_code=FollowJointTrajectory.Result.SUCCESSFUL)

def execute_positions(self, positions):
    motor_values = [
        int((-pos + 3.1416) * (4095 / (2 * 3.1416)))  # 精确弧度转换
        for pos in positions
    ]
    self.motors_bus.write_with_motor_ids(
        ["sts3215"]*len(self.joint_to_motor_id),
        list(self.joint_to_motor_id.values()),
        "Goal_Position",
        motor_values
    )

3.完整代码如下

# 与 Feetech 电机总线进行通信，以控制机械臂的关节运动，并发布关节的实际状态信息

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState  # JointState: ROS标准消息类型，用于描述关节状态（位置、速度、力矩）
from lerobot_driver.motors.feetech import FeetechMotorsBus  # 自定义的Feetech电机总线通信接口
import time
from serial.serialutil import SerialException # 处理串口通信异常


# 在文件顶部添加新的依赖
from rclpy.action import ActionServer
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint


class HardwareInterface(Node):
    def __init__(self):
        super().__init__('hardware_interface') # 创建一个名为 hardware_interface 的节点

        # 订阅器：监听joint_states话题，接收目标关节状态（如来自MoveIt的指令）
        # self.subscription = self.create_subscription(JointState, 'joint_states', self.joint_state_callback, 10)

        self._action_server = ActionServer(
            self,
            FollowJointTrajectory,
            '/joint_trajectory_controller/follow_joint_trajectory',
            self.execute_trajectory_callback
        )
        self.publisher_ = self.create_publisher(JointState, 'real_joint_states', 10)
        self.timer = self.create_timer(0.02, self.publish_real_joint_states)  # 0.02 -> 50 Hz

        # 电机总线初始化：通过串口/dev/ttyARM0连接Feetech电机总线
        self.motors_bus = FeetechMotorsBus(
            port="/dev/ttyARM0", # 需要根据自己情况修改
            motors={},
        )
        self.motors_bus.connect()

        # Define the mapping between joint names and motor IDs 关节与电机ID映射
        self.joint_to_motor_id = {
            "Joint_1": 1,
            "Joint_2": 2,
            "Joint_3": 3,
            "Joint_4": 4,
            "Joint_5": 5,
            "Joint_6": 6,
            "Joint_Gripper": 7,
        }

        # Initialize motors with the correct IDs and model
        self.motor_names = list(self.joint_to_motor_id.keys())
        self.motors_bus.motors = {name: (self.joint_to_motor_id[name], "sts3215") for name in self.motor_names} # 初始化电机型号为sts3215

        self.configure_motors() # 配置电机参数
        self.set_torque(True)   # 启用/禁用电机扭矩

    def configure_motors(self):
        for name in self.motor_names:
            self.motors_bus.write("Goal_Speed", 100, name)  # Set speed to 100
            self.motors_bus.write("Acceleration", 50, name)  # Set acceleration to 50

    def set_torque(self, enable):
        torque_value = 1 if enable else 0
        for name in self.motor_names:
            self.motors_bus.write("Torque_Enable", torque_value, name)
        self.get_logger().info(f"Torque {'enabled' if enable else 'disabled'} for all motors.")

    def execute_trajectory_callback(self, goal_handle):
        trajectory = goal_handle.request.trajectory
        self.get_logger().info("Received new trajectory with {} points".format(len(trajectory.points)))

        # 验证关节名称匹配
        if set(trajectory.joint_names) != set(self.joint_to_motor_id.keys()):
            goal_handle.abort()
            return FollowJointTrajectory.Result(
                error_code=FollowJointTrajectory.Result.INVALID_JOINTS
            )

        # 执行轨迹点
        for point in trajectory.points:
            start_time = self.get_clock().now()
            self.execute_positions(point.positions)
            
            # 非阻塞式时间等待
            target_duration = rclpy.duration.Duration(
                seconds=point.time_from_start.sec,
                nanoseconds=point.time_from_start.nanosec
            )
            while (self.get_clock().now() - start_time) < target_duration:
                rclpy.spin_once(self, timeout_sec=0.001)  # 保持回调处理

        goal_handle.succeed()
        return FollowJointTrajectory.Result(error_code=FollowJointTrajectory.Result.SUCCESSFUL)

    def execute_positions(self, positions):
        motor_values = [
            int((-pos + 3.1416) * (4095 / (2 * 3.1416)))  # 精确弧度转换
            for pos in positions
        ]
        self.motors_bus.write_with_motor_ids(
            ["sts3215"]*len(self.joint_to_motor_id),
            list(self.joint_to_motor_id.values()),
            "Goal_Position",
            motor_values
        )




    def publish_real_joint_states(self):
        motor_ids = list(self.joint_to_motor_id.values())   # 从字典 self.joint_to_motor_id 中提取所有电机ID
        motor_models = ["sts3215"] * len(motor_ids)         # 创建一个与电机数量相同的列表，所有元素均为电机型号 "sts3215"
        try:
            positions = self.motors_bus.read_with_motor_ids(motor_models, motor_ids, "Present_Position") # 调用 FeetechMotorsBus 的方法，通过电机ID和型号读取当前值
        except (ConnectionError, SerialException) as e:
            self.get_logger().error(f"Connection error: {e}")
            self.get_logger().info("Attempting to reconnect...")
            while True:
                try:
                    self.motors_bus.reconnect()
                    self.get_logger().info("Reconnected to motors.")
                    return
                except Exception as e:
                    self.get_logger().error(f"Reconnection failed: {e}")
                    time.sleep(1)  # Wait for 1 second before retrying

        joint_state_msg = JointState()
        joint_state_msg.header.stamp = self.get_clock().now().to_msg()
        joint_state_msg.name = self.motor_names
        joint_state_msg.position = [((-pos / 4095.0) * (2 * 3.14) - 3.14) for pos in positions]  # Convert motor value to radians

        self.publisher_.publish(joint_state_msg)
        # self.get_logger().info(f"Published real joint states: {positions}")
        self.get_logger().info(f"Published real joint states (radians): {joint_state_msg.position}")

def main(args=None):
    rclpy.init(args=args)
    hardware_interface = HardwareInterface()
    rclpy.spin(hardware_interface)
    hardware_interface.motors_bus.disconnect()
    hardware_interface.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()