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 modelself.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} # 初始化电机型号为sts3215self.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 100self.motors_bus.write("Acceleration", 50, name)  # Set acceleration to 50def set_torque(self, enable):torque_value = 1 if enable else 0for 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 valuemotor_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 中提取所有电机IDmotor_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.")returnexcept Exception as e:self.get_logger().error(f"Reconnection failed: {e}")time.sleep(1)  # Wait for 1 second before retryingjoint_state_msg = JointState()joint_state_msg.header.stamp = self.get_clock().now().to_msg()joint_state_msg.name = self.motor_namesjoint_state_msg.position = [((-pos / 4095.0) * (2 * 3.14) - 3.14) for pos in positions]  # Convert motor value to radiansself.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.trajectoryself.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 JointTrajectoryPointclass 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 modelself.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} # 初始化电机型号为sts3215self.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 100self.motors_bus.write("Acceleration", 50, name)  # Set acceleration to 50def set_torque(self, enable):torque_value = 1 if enable else 0for 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.trajectoryself.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 中提取所有电机IDmotor_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.")returnexcept Exception as e:self.get_logger().error(f"Reconnection failed: {e}")time.sleep(1)  # Wait for 1 second before retryingjoint_state_msg = JointState()joint_state_msg.header.stamp = self.get_clock().now().to_msg()joint_state_msg.name = self.motor_namesjoint_state_msg.position = [((-pos / 4095.0) * (2 * 3.14) - 3.14) for pos in positions]  # Convert motor value to radiansself.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()