11. MoveIt 2 仿真项目实战

前面 10 讲分别学了运动学、ROS2 通信、tf2、URDF、MoveIt 2 基础。这一讲把它们拼成一个完整系统：订阅目标点 → IK 求解 → MoveIt 规划 → 执行，加入障碍物，一键启动。

前言：从零件到系统

前面 10 讲，我们分别学了：

运动学（FK/IK）
ROS2 节点、Topic、Service、Action
参数系统和 Launch
tf2 坐标系管理
URDF 机器人建模
MoveIt 2 基础规划

这些都是零件。本讲的任务是把它们拼成一台机器。

目标系统的数据流：

用户发布目标点 (/target_pose)
        ↓ Topic
  规划客户端节点
        ↓ moveit_py
    move_group
        ↓ 规划 + 执行
  robot_state_publisher
        ↓ TF
      RViz2 显示

11.1 项目包结构

按职责拆成 4 个包，每个包只做一件事：

ros2_ws/src/
├── my_robot_description/     # URDF 模型（第 9 讲已创建）
├── my_robot_interfaces/      # 自定义消息/服务/动作（第 6 讲已创建）
├── my_robot_moveit_config/   # MoveIt 配置（第 10 讲已生成）
├── my_robot_planner/         # 规划客户端节点（本讲完善）
└── my_robot_bringup/         # 一键启动 Launch 文件（本讲新建）

11.2 完善规划客户端

11.2.1 接收 Topic 目标点的规划节点

上一讲的 planner_node.py 只规划一次固定目标。现在改成订阅 /target_pose，收到目标就规划：

vim $ROS_WS/src/my_robot_planner/my_robot_planner/topic_planner_node.py

# my_robot_planner/topic_planner_node.py
# 运行方式：ros2 run my_robot_planner topic_planner_node
#
# ============================================================
# 节点目标：订阅 /target_pose（目标末端位置），收到后自动完成
#           IK 求解 → MoveIt 规划 → 执行的完整流程
# 执行逻辑：
#   1. __init__ 初始化 MoveItPy，订阅 /target_pose
#   2. 收到目标点 → target_callback
#   3. 调用 IK 求解关节角（不可达则警告并忽略）
#   4. 调用 _plan_to_joint_angles 规划并执行
#   5. _planning 标志位防止并发规划
# ============================================================

import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Point
from moveit.planning import MoveItPy, PlanRequestParameters
from moveit.core.robot_state import RobotState

from my_robot_basics.two_link_kinematics import inverse_kinematics


class TopicPlannerNode(Node):
    def __init__(self):
        super().__init__('topic_planner_node')

        self.declare_parameter('l1', 1.0)
        self.declare_parameter('l2', 0.8)
        self.L1 = self.get_parameter('l1').value
        self.L2 = self.get_parameter('l2').value

        self.get_logger().info('初始化 MoveIt...')
        self.moveit = MoveItPy(node_name='topic_planner_node')
        self.arm = self.moveit.get_planning_component('arm')
        self.get_logger().info('MoveIt 就绪，等待目标点...')

        self.subscription = self.create_subscription(
            Point,
            '/target_pose',
            self.target_callback,
            10,
        )

        # 规划重入保护标志：规划和执行是阻塞操作，期间可能收到新目标
        # _planning=True 时忽略新目标，避免并发规划导致状态混乱
        self._planning = False

    def target_callback(self, msg):
        if self._planning:
            self.get_logger().warn('上一次规划还未完成，忽略本次目标')
            return

        x, y = msg.x, msg.y
        self.get_logger().info(f'收到目标点: ({x:.3f}, {y:.3f})')

        # 先用 IK 检查目标是否可达，不可达直接返回，不进入规划流程
        try:
            solutions = inverse_kinematics(x, y, self.L1, self.L2)
        except ValueError as e:
            self.get_logger().warn(f'目标不可达: {e}')
            return

        t1_deg, t2_deg = solutions['elbow_down']
        self.get_logger().info(f'IK 解: θ1={t1_deg:.2f}°, θ2={t2_deg:.2f}°')

        # 设置标志位，开始规划
        self._planning = True
        try:
            self._plan_to_joint_angles(
                math.radians(t1_deg),
                math.radians(t2_deg),
            )
        finally:
            # finally 确保无论规划成功还是抛出异常，标志位都会被清除
            # 如果不用 finally，规划失败时 _planning 永远为 True，节点就"卡死"了
            self._planning = False

    def _plan_to_joint_angles(self, theta1_rad, theta2_rad):
        # 每次规划前都要重新设置起始状态，确保从当前实际位置出发
        self.arm.set_start_state_to_current_state()

        robot_state = RobotState(self.moveit.get_robot_model())
        robot_state.set_joint_group_positions('arm', [theta1_rad, theta2_rad])
        self.arm.set_goal_state(robot_state=robot_state)

        plan_params = PlanRequestParameters(self.moveit, 'ompl')
        plan_params.planning_pipeline = 'ompl'
        plan_params.planner_id = 'RRTConnect'
        plan_result = self.arm.plan(single_plan_parameters=plan_params)
        if not plan_result:
            self.get_logger().warn('规划失败，目标可能被障碍物阻挡')
            return

        self.get_logger().info('规划成功，执行中...')
        self.moveit.execute(plan_result.trajectory, controllers=['arm_controller'])
        self.get_logger().info('执行完成')


def main(args=None):
    rclpy.init(args=args)
    node = TopicPlannerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

11.2.2 障碍物管理节点

vim $ROS_WS/src/my_robot_planner/my_robot_planner/scene_manager_node.py

import rclpy
from rclpy.node import Node
from moveit_msgs.msg import CollisionObject
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import Pose


class SceneManagerNode(Node):
    def __init__(self):
        super().__init__('scene_manager_node')

        # 直接向 move_group 的 /collision_object topic 发布障碍物
        # 不需要 MoveItPy，move_group 会自动更新 Planning Scene
        self.publisher = self.create_publisher(CollisionObject, '/collision_object', 10)

        # 延迟 2 秒，等待 move_group 完全启动并订阅 /collision_object
        self.timer = self.create_timer(2.0, self.setup_scene)

    def setup_scene(self):
        self.timer.cancel()

        obj = CollisionObject()
        obj.id = 'wall'
        obj.header.frame_id = 'base_link'

        box = SolidPrimitive()
        box.type = SolidPrimitive.BOX
        box.dimensions = [0.02, 0.3, 0.5]  # 2cm 厚，30cm 宽，50cm 高

        pose = Pose()
        pose.position.x = 0.7
        pose.position.y = 0.3
        pose.position.z = 0.25
        pose.orientation.w = 1.0

        obj.primitives = [box]
        obj.primitive_poses = [pose]
        obj.operation = CollisionObject.ADD

        self.publisher.publish(obj)
        self.get_logger().info('障碍物 "wall" 已发布到规划场景')


def main(args=None):
    rclpy.init(args=args)
    node = SceneManagerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

11.2.3 注册入口点

在 my_robot_planner/setup.py 的 console_scripts 里追加两个新节点（planner_node 是上一讲已有的）：

'console_scripts': [
    'planner_node        = my_robot_planner.planner_node:main',
    'topic_planner_node  = my_robot_planner.topic_planner_node:main',
    'scene_manager_node  = my_robot_planner.scene_manager_node:main',
],

同时在 data_files 里确认 launch 目录已注册（上一讲已添加）：

data_files=[
    ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
    ('share/' + package_name, ['package.xml']),
    ('share/' + package_name + '/launch', ['launch/planner.launch.py']),
],

11.3 创建 bringup 包

cd $ROS_WS/src
ros2 pkg create --build-type ament_cmake my_robot_bringup
mkdir -p my_robot_bringup/launch

11.3.1 完整系统 Launch 文件

注意：这个 Launch 文件必须把所有节点内联在一起，不能用 IncludeLaunchDescription 嵌套 demo_sim.launch.py。嵌套会导致 ros2_control_node 在子 launch 上下文里崩溃，spawner 无法联系 controller_manager。

vim $ROS_WS/src/my_robot_bringup/launch/full_system.launch.py

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import TimerAction


def generate_launch_description():
    moveit_config = (
        MoveItConfigsBuilder("two_link_arm", package_name="my_robot_moveit_config")
        .to_moveit_configs()
    )

    # robot_state_publisher
    rsp_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[moveit_config.robot_description],
    )

    # ros2_control_node：加载 mock hardware
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            moveit_config.robot_description,
            str(moveit_config.package_path / "config/ros2_controllers.yaml"),
        ],
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"],
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["arm_controller"],
    )

    # move_group + RViz2
    move_group = generate_move_group_launch(moveit_config)
    rviz = generate_moveit_rviz_launch(moveit_config)

    # 构造 pipeline 参数（MoveItPy 底层需要 planning_pipelines.pipeline_names）
    pp = moveit_config.planning_pipelines
    pipeline_params = {
        "planning_pipelines.pipeline_names": ["ompl"],
        "default_planning_pipeline": "ompl",
    }
    for k, v in pp.get("ompl", {}).items():
        if k != "planner_configs":
            pipeline_params[f"ompl.{k}"] = v

    # topic_planner_node 延迟 5 秒，等待 move_group 和控制器完全就绪
    topic_planner = TimerAction(
        period=5.0,
        actions=[
            Node(
                package='my_robot_planner',
                executable='topic_planner_node',
                name='topic_planner_node',
                output='screen',
                parameters=[
                    moveit_config.robot_description,
                    moveit_config.robot_description_semantic,
                    moveit_config.robot_description_kinematics,
                    moveit_config.joint_limits,
                    moveit_config.trajectory_execution,
                    pipeline_params,
                    {'l1': 1.0, 'l2': 0.8},
                ],
            )
        ]
    )

    # scene_manager 直接发 /collision_object topic，不需要 MoveItPy
    scene_manager = TimerAction(
        period=6.0,
        actions=[
            Node(
                package='my_robot_planner',
                executable='scene_manager_node',
                name='scene_manager_node',
                output='screen',
            )
        ]
    )

    return LaunchDescription(
        [rsp_node, ros2_control_node, joint_state_broadcaster_spawner, arm_controller_spawner]
        + move_group.entities
        + rviz.entities
        + [topic_planner, scene_manager]
    )

11.3.2 配置 CMakeLists.txt

ros2 pkg create 生成的模板里没有安装 launch 目录的指令，在 find_package(ament_cmake REQUIRED) 后面加一段：

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

其余模板内容保持不动。

11.4 构建并运行完整系统

cd $ROS_WS
colcon build --packages-select my_robot_planner my_robot_bringup
source install/setup.bash

# 一键启动整个系统
ros2 launch my_robot_bringup full_system.launch.py

等待约 5 秒，所有节点启动完成后，发送目标点：

# 新开终端
source $ROS_WS/install/setup.bash

# 发送一个目标点
ros2 topic pub --once /target_pose geometry_msgs/msg/Point \
  "{x: 1.2, y: 0.5, z: 0.0}"

RViz2 里应该看到机械臂规划并执行运动。

11.5 逐步验证清单

完成项目后，逐项验证：

# 1. 模型是否正确加载
ros2 topic echo /robot_description --once | head -5

# 2. TF 树是否完整
ros2 run tf2_tools view_frames

# 3. move_group 是否在运行
ros2 node list | grep move_group

# 4. 规划场景是否有障碍物
ros2 topic echo /monitored_planning_scene --once | grep wall

# 5. 发送目标点后是否触发规划
ros2 topic pub --once /target_pose geometry_msgs/msg/Point "{x: 1.2, y: 0.5, z: 0.0}"

11.6 典型排障路径

11.6.1 问题 1：系统启动后 RViz2 里没有模型

# 检查 robot_description 参数
ros2 param get /robot_state_publisher robot_description | head -3

# 检查 TF 是否在发布
ros2 topic hz /tf

11.6.2 问题 2：发送目标点后没有反应

# 确认 topic_planner_node 在运行
ros2 node list | grep planner

# 确认 /target_pose 收到了消息
ros2 topic echo /target_pose

# 查看节点日志
ros2 node info /topic_planner_node

11.6.3 问题 3：规划失败

# 先用 IK 服务验证目标是否可达
ros2 service call /solve_ik my_robot_interfaces/srv/SolveIK \
  "{x: 1.2, y: 0.5, l1: 1.0, l2: 0.8}"

# 在 RViz2 的 MotionPlanning 面板里手动规划同一目标，看错误信息

11.6.4 问题 4：有障碍物但路径没有绕开

# 确认障碍物在规划场景里
ros2 topic echo /monitored_planning_scene --once | grep -A5 wall

# 在 RViz2 里添加 PlanningScene 显示，确认障碍物可见

11.7 练习题

11.7.1 练习 1：发送多个目标点

连续发送 3 个不同目标点，观察机械臂依次运动：

ros2 topic pub --once /target_pose geometry_msgs/msg/Point "{x: 1.0, y: 0.0, z: 0.0}"
# 等待执行完成后
ros2 topic pub --once /target_pose geometry_msgs/msg/Point "{x: 0.8, y: 0.6, z: 0.0}"
# 等待执行完成后
ros2 topic pub --once /target_pose geometry_msgs/msg/Point "{x: 0.5, y: 0.8, z: 0.0}"

11.7.2 练习 2：发送不可达目标

# 超出工作空间（L1+L2=1.8）
ros2 topic pub --once /target_pose geometry_msgs/msg/Point "{x: 2.0, y: 0.0, z: 0.0}"

观察节点日志里的警告信息，确认系统能优雅处理不可达目标。

11.7.3 练习 3：添加第二个障碍物

在 scene_manager_node.py 里再添加一个障碍物，位置在 (0.5, 0.5, 0.0)，观察规划路径的变化。

本讲核心总结

概念	一句话理解
项目包结构	description/interfaces/moveit_config/planner/bringup，各司其职
`topic_planner_node`	订阅目标点 → IK 求解 → moveit_py 规划 → 执行
`scene_manager_node`	向 `/collision_object` topic 发布障碍物，move_group 自动更新 Planning Scene
`TimerAction`	Launch 里延迟启动节点，等待依赖节点就绪
`PlanRequestParameters`	MoveItPy 规划时必须显式指定 pipeline 和 planner，否则报 "No planning pipeline available"
验证清单	模型→TF→move_group→场景→规划，逐步验证比一次性调试高效得多

参考代码

本讲对应的完整参考代码位于：

ros_ws/reference/src/my_robot_planner/my_robot_planner/
├── topic_planner_node.py       ← 11.2 订阅目标点自动规划
└── scene_manager_node.py       ← 11.2 障碍物管理

ros_ws/reference/src/my_robot_bringup/
├── launch/full_system.launch.py ← 11.3 完整系统一键启动
├── CMakeLists.txt
└── package.xml

使用方式：

# 规划节点
cp $ROS_WS/reference/src/my_robot_planner/my_robot_planner/topic_planner_node.py \
   $ROS_WS/src/my_robot_planner/my_robot_planner/
cp $ROS_WS/reference/src/my_robot_planner/my_robot_planner/scene_manager_node.py \
   $ROS_WS/src/my_robot_planner/my_robot_planner/

# bringup 包（新建）
ros2 pkg create --build-type ament_cmake my_robot_bringup
mkdir -p $ROS_WS/src/my_robot_bringup/launch
cp $ROS_WS/reference/src/my_robot_bringup/launch/full_system.launch.py \
   $ROS_WS/src/my_robot_bringup/launch/

full_system.launch.py 注释中详细说明了 TimerAction 延迟启动的原因，以及 MoveItPy 所需的 pipeline 参数格式转换逻辑。

下一讲预告

12. 课程总结与进阶方向

至此，我们已经走完了从"运动学概念"到"MoveIt 2 仿真系统"的完整链路。最后一讲我们会：

回顾整条知识主线，梳理各讲之间的依赖关系
用 7 个自测问题检验掌握程度
给出 4 条清晰的进阶路径
布置结课扩展任务

前言：从零件到系统​

11.1 项目包结构​

11.2 完善规划客户端​

11.2.1 接收 Topic 目标点的规划节点​

11.2.2 障碍物管理节点​

11.2.3 注册入口点​

11.3 创建 bringup 包​

11.3.1 完整系统 Launch 文件​

11.3.2 配置 CMakeLists.txt​

11.4 构建并运行完整系统​

11.5 逐步验证清单​

11.6 典型排障路径​

11.6.1 问题 1：系统启动后 RViz2 里没有模型​

11.6.2 问题 2：发送目标点后没有反应​

11.6.3 问题 3：规划失败​

11.6.4 问题 4：有障碍物但路径没有绕开​

11.7 练习题​

11.7.1 练习 1：发送多个目标点​

11.7.2 练习 2：发送不可达目标​

11.7.3 练习 3：添加第二个障碍物​

本讲核心总结​

参考代码​

下一讲预告​