MoveIt2-Trajectory-Planning

MoveIt2 轨迹规划

MoveIt2 官方文档 | MoveGroupInterface API

---

概述

MoveIt2 是 ROS2 中用于机器人运动规划的框架。通过 MoveGroupInterface 或直接调用服务可以设置目标位姿，调用 OMPL 等规划器计算关节轨迹，实现机械臂的运动控制。

---

核心架构

通讯机制

MoveGroupInterface 是客户端，通过 ROS2 通讯与 move_group 节点交互：

通讯类型	用途	示例
Action	执行运动	/move_action, /execute_trajectory
Service	规划和查询	/compute_ik, /plan_kinematic_path
Topic	状态监控	/joint_states, /display_planned_path

你的节点 (MoveGroupInterface 或 Service Client)
        |
        | Action/Service/Topic
        v
   move_group 节点
        |
        | 调用规划器 (OMPL等)
        | 调用 IK 求解器
        v
   返回规划结果

---

使用方法

依赖配置

package.xml:

<depend>moveit_ros_planning_interface</depend>  <!-- C++ -->
<depend>moveit_msgs</depend>  <!-- Python 直接调用服务 -->

CMakeLists.txt (C++):

find_package(moveit_ros_planning_interface REQUIRED)
ament_target_dependencies(your_node moveit_ros_planning_interface)

C++ 实现（推荐）

#include <moveit/move_group_interface/move_group_interface.h>

auto move_group = moveit::planning_interface::MoveGroupInterface(node, "arm");

geometry_msgs::msg::Pose target_pose;
target_pose.position.x = 0.5;
target_pose.orientation.w = 1.0;

move_group.setPoseTarget(target_pose);

moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);

if (success) {
    move_group.execute(plan);  // 执行
    // 或只获取路径: plan.trajectory_.joint_trajectory
}

Python 实现（直接调用服务）

MoveIt2 的 Python binding (moveit.planning) 可能不可用，推荐直接调用 /plan_kinematic_path 服务：

from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from moveit_msgs.srv import GetMotionPlan
from moveit_msgs.msg import (
    Constraints, PositionConstraint, OrientationConstraint,
    BoundingVolume, RobotState,
)
from sensor_msgs.msg import JointState
from shape_msgs.msg import SolidPrimitive

class TrajectoryServer(Node):
    def __init__(self):
        super().__init__("trajectory_server")
        self.callback_group = ReentrantCallbackGroup()

        # 订阅 joint_states 获取当前状态
        self.current_joint_state = None
        self.joint_sub = self.create_subscription(
            JointState, "/joint_states", self.joint_state_callback, 10
        )

        self.client = self.create_client(
            GetMotionPlan, "/plan_kinematic_path",
            callback_group=self.callback_group
        )

    def plan(self, target_pose, group_name="arm", ee_link="link6", base_frame="base_link"):
        req = GetMotionPlan.Request()
        req.motion_plan_request.group_name = group_name
        req.motion_plan_request.num_planning_attempts = 10
        req.motion_plan_request.allowed_planning_time = 5.0

        # 设置起始状态（重要！）
        if self.current_joint_state:
            req.motion_plan_request.start_state = RobotState()
            req.motion_plan_request.start_state.joint_state = self.current_joint_state

        # 位置约束
        pos = PositionConstraint()
        pos.header.frame_id = base_frame
        pos.link_name = ee_link
        pos.constraint_region = BoundingVolume()
        sphere = SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])
        pos.constraint_region.primitives.append(sphere)
        pos.constraint_region.primitive_poses.append(target_pose)
        pos.weight = 1.0

        # 姿态约束
        ori = OrientationConstraint()
        ori.header.frame_id = base_frame
        ori.link_name = ee_link
        ori.orientation = target_pose.orientation
        ori.absolute_x_axis_tolerance = 0.1
        ori.absolute_y_axis_tolerance = 0.1
        ori.absolute_z_axis_tolerance = 0.1
        ori.weight = 1.0

        goal = Constraints()
        goal.position_constraints.append(pos)
        goal.orientation_constraints.append(ori)
        req.motion_plan_request.goal_constraints.append(goal)

        future = self.client.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        result = future.result()

        if result.motion_plan_response.error_code.val == 1:  # SUCCESS
            return result.motion_plan_response.trajectory.joint_trajectory
        return None

# 必须使用 MultiThreadedExecutor
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()

常用配置

move_group.setPlanningTime(5.0);                    // 规划时间限制（默认5秒）
move_group.setNumPlanningAttempts(10);              // 规划尝试次数
move_group.setMaxVelocityScalingFactor(0.5);        // 速度缩放
move_group.setMaxAccelerationScalingFactor(0.5);    // 加速度缩放

---

避障规划

方式一：Octomap（推荐）

MoveIt2 内置支持将点云转换为 Octomap 进行碰撞检测。

配置文件 sensors_3d.yaml:

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera/depth/points
    max_range: 2.5
    point_subsample: 1
    padding_offset: 0.03
    padding_scale: 1.0

在 launch 文件中加载：

moveit_config = (
    MoveItConfigsBuilder("your_robot")
    .sensors_3d(file_path="config/sensors_3d.yaml")
    .to_moveit_configs()
)

方式二：手动添加碰撞物体

from moveit_msgs.msg import PlanningScene, CollisionObject
from shape_msgs.msg import SolidPrimitive

collision_object = CollisionObject()
collision_object.header.frame_id = 'base_link'
collision_object.id = 'obstacle'

box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = [0.1, 0.1, 0.5]

collision_object.primitives.append(box)
collision_object.primitive_poses.append(pose)
collision_object.operation = CollisionObject.ADD

scene = PlanningScene()
scene.is_diff = True
scene.world.collision_objects.append(collision_object)

planning_scene_publisher.publish(scene)

---

常见问题

Found empty JointState message

原因：没有设置规划的起始状态。

解决：订阅 /joint_states 获取当前关节状态，设置到 motion_plan_request.start_state。

服务回调中调用其他服务导致死锁

原因：默认单线程执行器无法在处理一个回调时同时处理另一个回调。

解决：

from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor

self.callback_group = ReentrantCallbackGroup()
self.client = self.create_client(srv_type, srv_name, callback_group=self.callback_group)
self.srv = self.create_service(srv_type, srv_name, callback, callback_group=self.callback_group)

executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()

Python binding 不可用

错误：ModuleNotFoundError: No module named 'moveit.planning'

解决：直接调用 /plan_kinematic_path 服务，不依赖 Python binding。

---

启动顺序

# 1. 启动 move_group 节点
ros2 launch your_robot_moveit_config move_group.launch.py

# 2. 启动规划节点
ros2 run your_package trajectory_server

---

注意事项

• planning_group 名称在 MoveIt Setup Assistant 生成的 SRDF 文件中定义
• move_group 节点必须先启动
• 默认规划时间为 5 秒，复杂场景可能需要增加
• 点云数据量大时建议先进行体素降采样
• Python 实现需要使用 MultiThreadedExecutor 避免死锁