3. 编写Python节点实现关节空间规划

任务目标

编写一个ROS2 Python节点，通过MoveIt2 Python API将机器人规划到指定的关节空间目标位置，并执行该运动。节点应能够连接到MoveGroup接口，设置关节目标，规划并执行，最终输出执行结果。

环境依赖

• ROS2（Humble或更高版本）
• MoveIt2（包含moveit_py或moveit_commander）
• 已配置的机器人（如panda机械臂）及其MoveIt2配置包

关键完整代码

创建文件 joint_space_planner.py：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander
from moveit_msgs.msg import RobotState
from sensor_msgs.msg import JointState
import sys

class JointSpacePlanner(Node):
    def __init__(self):
        super().__init__('joint_space_planner')
        self.declare_parameter('joint_goal', [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785])
        self.declare_parameter('planning_group', 'panda_arm')
        
        group_name = self.get_parameter('planning_group').value
        self.move_group = MoveGroupCommander(group_name, node=self)
        self.robot = RobotCommander(node=self)
        self.scene = PlanningSceneInterface(node=self)
        
        # 设置规划时间与容差
        self.move_group.set_planning_time(5.0)
        self.move_group.set_goal_joint_tolerance(0.01)
        self.move_group.set_num_planning_attempts(10)

    def plan_and_execute(self):
        joint_goal = self.get_parameter('joint_goal').value
        if len(joint_goal) != 7:
            self.get_logger().error(f"关节目标数量应为7，当前为{len(joint_goal)}")
            return False
        
        self.get_logger().info(f"设置关节目标: {joint_goal}")
        self.move_group.set_joint_value_target(joint_goal)
        
        # 规划
        success, plan, planning_time, error_code = self.move_group.plan()
        if not success:
            self.get_logger().error(f"规划失败，错误码: {error_code}")
            return False
        
        self.get_logger().info(f"规划成功，耗时 {planning_time:.2f} 秒")
        
        # 执行
        result = self.move_group.execute(plan, wait=True)
        if result:
            self.get_logger().info("运动执行成功")
            return True
        else:
            self.get_logger().error("运动执行失败")
            return False

def main(args=None):
    rclpy.init(args=args)
    node = JointSpacePlanner()
    
    try:
        success = node.plan_and_execute()
        if success:
            node.get_logger().info("关节空间规划任务完成")
        else:
            node.get_logger().error("任务失败")
    except Exception as e:
        node.get_logger().error(f"发生异常: {e}")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

运行方式

1. 启动机器人仿真环境（以Panda为例）：

ros2 launch panda_moveit_config demo.launch.py

2. 在另一个终端运行节点：

ros2 run your_package joint_space_planner

可通过参数指定关节目标：

ros2 run your_package joint_space_planner --ros-args -p joint_goal:="[0.5,-0.5,0.0,-2.0,0.0,1.5,0.5]"

验证方法

• 终端输出：节点打印“规划成功”和“运动执行成功”即表示任务完成。
• 可视化观察：在RViz中观察机器人从初始位姿平滑运动到目标关节位置，且无碰撞。
• 状态确认：执行完成后，可通过以下命令查看当前关节角度是否与目标一致：

ros2 topic echo /joint_states --once

对比输出的关节位置与设定的joint_goal，误差应在0.01弧度以内。

常见问题

• 规划失败：检查目标关节值是否在关节限位内，或增大set_num_planning_attempts。
• 找不到MoveGroup：确认机器人描述和MoveIt2配置已正确加载，且planning_group名称与SRDF中定义一致。
• 执行中断：确保没有其他节点抢占控制权，且机器人处于非奇异位姿。