moveit2基础教程上手-使用xarm6演示

0、前置信息

开发环境：wsl。
ros版本：jazzy，ubuntu版本：24.04
xarm-ros2地址

1、启动Rviz，加载 Motion Planning Plugin，实现演示功能

Getting Started — MoveIt Documentation: Rolling documentation

ros2 launch xarm_moveit_config xarm6_moveit_fake.launch.py add_gripper:=True

2、first C++ moveit project

Your First C++ MoveIt Project — MoveIt Documentation: Rolling documentation

ros2 pkg create \
 --build-type ament_cmake \
 --dependencies moveit_ros_planning_interface rclcpp \
 --node-name hello_moveit hello_moveit

编写src/hello_moveit.cpp

#include <memory>

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>

int main(int argc, char ** argv)
{
  // Initialize ROS
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Create ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // log info
  RCLCPP_INFO(logger, "Hello MoveIt2!");
  
  // Shutdown ROS
  rclcpp::shutdown();
  return 0;
}

编译运行！

# build
colcon build --packages-select hello_moveit
# source env
source install/setup.bash
# Run node
ros2 run hello_moveit hello_moveit

3、增加功能，使用movegroupinterface进行规划和执行

// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
// node, group_name 注意这里替换为xarm6
auto move_group_interface = MoveGroupInterface(node, "xarm6");

// Set a target pose
auto const target_pose = []{
    geometry_msgs::msg::Pose msg;
    msg.position.x = 0.3;
    msg.position.y = -0.1;
    msg.position.z = 0.2;
    msg.orientation.x = 1;
    msg.orientation.y = 0;
    msg.orientation.z = 0;
    msg.orientation.w = 0;
    return msg;
  }();
move_group_interface.setPoseTarget(target_pose);

// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
auto const ok = static_cast<bool>(move_group_interface.plan(my_plan));
return std::make_pair(ok, my_plan);
}();

// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planning failed!");
}

（补充）使用lambda进行复杂初始化：IIFE for Complex Initialization - C++ Stories

// lambda 范式，[& / =] 按引用 / 值捕获所有使用的外部变量
[capture list] (parameter list) -> return type { function body }

// ex.
const auto var = [&] { 
    return /* some complex code here */; 
}(); // call it!

然后在 Displays 窗口的 MotionPlanning/Planning Request 下，取消选中 Query Goal State 框。

编译运行（报错记录，已解决）（只运行节点似乎缺少关键配置项robot_description_semantic，对比xarm_planner中的launch文件发现）

ros2 run hello_moveit hello_moveit

在类中使用参数（C++） — ROS 2 Documentation: Humble 文档

dod@qDoDp:~/dev_ws$ ros2 run hello_moveit hello_moveit 
[INFO] [1742572281.073652608] [hello_moveit]: Hello MoveIt2!
[ERROR] [1742572291.184066819] [hello_moveit]: Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds.
Error:   Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT ErrorID=13 (0xd) Line number=0
         at line 732 in ./src/model.cpp
[ERROR] [1742572291.194733210] [moveit_2235338666.moveit.ros.rdf_loader]: Unable to parse SRDF
[FATAL] [1742572291.195511210] [moveit_2235338666.moveit.ros.move_group_interface]: Unable to construct robot model. Please make sure all needed information is on the parameter server.
terminate called after throwing an instance of 'std::runtime_error'
  what():  Unable to construct robot model. Please make sure all needed information is on the parameter server.
[ros2run]: Aborted

4、编写launch文件，加载moveit配置

# create launch
mkdir launch

# update CMakeLists.txt
install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}/
)

launch文件内容：

# moveit_conf.launch.py
import yaml
import json
from launch import LaunchDescription
from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from uf_ros_lib.moveit_configs_builder import MoveItConfigsBuilder

def launch_setup(context, *args, **kwargs):
    dof = LaunchConfiguration('dof', default=7)
    robot_type = LaunchConfiguration('robot_type', default='xarm')
    prefix = LaunchConfiguration('prefix', default='')
    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
    limited = LaunchConfiguration('limited', default=True)
    effort_control = LaunchConfiguration('effort_control', default=False)
    velocity_control = LaunchConfiguration('velocity_control', default=False)
    model1300 = LaunchConfiguration('model1300', default=False)
    robot_sn = LaunchConfiguration('robot_sn', default='')
    attach_to = LaunchConfiguration('attach_to', default='world')
    attach_xyz = LaunchConfiguration('attach_xyz', default='"0 0 0"')
    attach_rpy = LaunchConfiguration('attach_rpy', default='"0 0 0"')
    mesh_suffix = LaunchConfiguration('mesh_suffix', default='stl')
    kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='')

    add_gripper = LaunchConfiguration('add_gripper', default=False)
    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
    add_bio_gripper = LaunchConfiguration('add_bio_gripper', default=False)
    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
    add_d435i_links = LaunchConfiguration('add_d435i_links', default=True)
    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
    geometry_type = LaunchConfiguration('geometry_type', default='box')
    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
    geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')

    moveit_config_dump = LaunchConfiguration('moveit_config_dump', default='')
    moveit_config_dump = moveit_config_dump.perform(context)
    moveit_config_dict = yaml.load(moveit_config_dump, Loader=yaml.FullLoader) if moveit_config_dump else {}

    if not moveit_config_dict:
        moveit_config = MoveItConfigsBuilder(
            context=context,
            # controllers_name=controllers_name,
            dof=dof,
            robot_type=robot_type,
            prefix=prefix,
            hw_ns=hw_ns,
            limited=limited,
            effort_control=effort_control,
            velocity_control=velocity_control,
            model1300=model1300,
            robot_sn=robot_sn,
            attach_to=attach_to,
            attach_xyz=attach_xyz,
            attach_rpy=attach_rpy,
            mesh_suffix=mesh_suffix,
            kinematics_suffix=kinematics_suffix,
            # ros2_control_plugin=ros2_control_plugin,
            # ros2_control_params=ros2_control_params,
            add_gripper=add_gripper,
            add_vacuum_gripper=add_vacuum_gripper,
            add_bio_gripper=add_bio_gripper,
            add_realsense_d435i=add_realsense_d435i,
            add_d435i_links=add_d435i_links,
            add_other_geometry=add_other_geometry,
            geometry_type=geometry_type,
            geometry_mass=geometry_mass,
            geometry_height=geometry_height,
            geometry_radius=geometry_radius,
            geometry_length=geometry_length,
            geometry_width=geometry_width,
            geometry_mesh_filename=geometry_mesh_filename,
            geometry_mesh_origin_xyz=geometry_mesh_origin_xyz,
            geometry_mesh_origin_rpy=geometry_mesh_origin_rpy,
            geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz,
            geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy,
        ).to_moveit_configs()
        moveit_config_dict = moveit_config.to_dict()
    
    move_group_interface_params = {
        'robot_description': moveit_config_dict['robot_description'],
        'robot_description_semantic': moveit_config_dict['robot_description_semantic'],
        'robot_description_kinematics': moveit_config_dict['robot_description_kinematics'],
    }

    node_executable = LaunchConfiguration('node_executable', default='hello_moveit_planner')
    node_name = LaunchConfiguration('node_name', default=node_executable)
    node_parameters = LaunchConfiguration('node_parameters', default={})

    try:
        xarm_planner_parameters = json.loads(node_parameters.perform(context))
    except:
        xarm_planner_parameters = {}
    
    xarm_planner_node = Node(
        name=node_name,
        package='hello_moveit',
        executable=node_executable,
        output='screen',
        parameters=[
            move_group_interface_params,
            {
                'robot_type': robot_type,
                'dof': dof,
                'prefix': prefix
            },
            xarm_planner_parameters,
        ],
    )

    # should be a list of nodes
    return [xarm_planner_node]

def generate_launch_description():
    return LaunchDescription([
        OpaqueFunction(function=launch_setup)
    ])
  
# moveit_api_pose.launch.py
import json
from launch import LaunchDescription
from launch.actions import OpaqueFunction
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def launch_setup(context, *args, **kwargs):
    prefix = LaunchConfiguration('prefix', default='')
    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
    limited = LaunchConfiguration('limited', default=False)
    effort_control = LaunchConfiguration('effort_control', default=False)
    velocity_control = LaunchConfiguration('velocity_control', default=False)
    add_gripper = LaunchConfiguration('add_gripper', default=False)
    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
    dof = LaunchConfiguration('dof', default=6)
    robot_type = LaunchConfiguration('robot_type', default='xarm')

    node_executable = 'hello_moveit'
    node_parameters = {}

    # moveit conf launch
    # hello_moveit/launch/moveit_conf.launch.py
    robot_planner_node_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('hello_moveit'), 'launch', 'moveit_conf.launch.py'])),
        launch_arguments={
            'prefix': prefix,
            'hw_ns': hw_ns,
            'limited': limited,
            'effort_control': effort_control,
            'velocity_control': velocity_control,
            'add_gripper': 'false',
            'add_vacuum_gripper': add_vacuum_gripper,
            'dof': dof,
            'robot_type': robot_type,
            'node_executable': node_executable,
            'node_parameters': json.dumps(node_parameters)
        }.items(),
    )

    return [
        robot_planner_node_launch
    ]


def generate_launch_description():
    return LaunchDescription([
        OpaqueFunction(function=launch_setup)
    ])

接下来重新编译运行：

# launch rviz
ros2 launch xarm_moveit_config xarm6_moveit_fake.launch.py

# launch hello_moveit
ros2 launch hello_moveit moveit_api_pose.launch.py

group name的问题：

# 通过ros2 param查看
ros2 param get /move_group robot_description_semantic

5、add dep: moveit_visual_tools

Visualizing In RViz — MoveIt Documentation: Rolling documentation

在 package.xml 中添加依赖：

# add to package.xml
<depend>moveit_visual_tools</depend>

在CMakeLists.txt中，补充find_package，扩展 ament_target_dependencies 宏调用以包含新的依赖项：

find_package(moveit_visual_tools REQUIRED)

ament_target_dependencies(
  hello_moveit
  "moveit_ros_planning_interface"
  "moveit_visual_tools"
  "rclcpp"
)

在源码中引入头文件：

#include <moveit_visual_tools/moveit_visual_tools.h>

编译验证，发现缺少这个依赖

# 安装依赖
sudo apt-get install ros-$ROS_DISTRO-moveit-visual-tools

# 也可使用rosdep，因为已经配置了package.xml
rosdep install --from-paths src --ignore-src -r -y

6、Create a ROS executor and spin the node on a thread

在初始化 MoveItVisualTools 之前，我们需要在 ROS 节点上启动一个执行程序。

（创建一个 ROS 2 执行器，允许在一个线程中处理多个节点的回调。它通过调用 spin() 方法来开始处理来自 ROS 话题、服务、定时器等的消息<阻塞调用>。）

#include <thread>  // <---- add this to the set of includes at the top

// Spin up a SingleThreadedExecutor for MoveItVisualTools to interact with ROS
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto spinner = std::thread([&executor]() { executor.spin(); });

// Create the MoveIt MoveGroup Interface

// Shutdown ROS
rclcpp::shutdown();  // <--- This will cause the spin function in the thread to return
spinner.join();  // 等待子线程结束主线程才可结束
return 0;

7、创建并初始化 moveit_visual_tools

接下来，我们将在构造 MoveGroupInterface 之后构造并初始化MoveItVisualTools。构造函数中传入节点、基坐标系、marker topic、机器人模型。

// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "xarm6");

// Construct and initialize MoveItVisualTools
// modify base_link to link_base
auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
    node, "link_base", rviz_visual_tools::RVIZ_MARKER_TOPIC,
    move_group_interface.getRobotModel()};
moveit_visual_tools.deleteAllMarkers();
moveit_visual_tools.loadRemoteControl();

构建和初始化后，我们现在创建一些闭包（可以访问当前范围内的变量的函数对象），我们稍后可以在程序中使用它们来帮助在 RViz 中呈现可视化。

• 第一个draw_title在机器人底座上方一米处添加文本。这是从高级别显示程序状态的有用方法。
• 第二个调用名为 prompt 的函数。此功能会阻止您的程序，直到用户按下 RViz 中的Next按钮。这对于在调试时单步调试程序很有帮助。
• 第三个绘制轨迹路径。

// Create closures for visualization
auto const draw_title = [&moveit_visual_tools](auto text) {
  auto const text_pose = [] {
    auto msg = Eigen::Isometry3d::Identity();
    msg.translation().z() = 1.0;  // Place text 1m above the base link
    return msg;
  }();
  moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
                                  rviz_visual_tools::XLARGE);
};
auto const prompt = [&moveit_visual_tools](auto text) {
  moveit_visual_tools.prompt(text);
};
auto const draw_trajectory_tool_path =
    [&moveit_visual_tools,
     jmg = move_group_interface.getRobotModel()->getJointModelGroup(
         "manipulator")](auto const trajectory) {
      moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
    };

接下来在plan、excute部分都添加触发器。

// Create a plan to that target pose
prompt("Press 'Next' in the RvizVisualToolsGui window to plan");
draw_title("Planning");
moveit_visual_tools.trigger();
auto const [success, plan] = [&move_group_interface] {
  moveit::planning_interface::MoveGroupInterface::Plan msg;
  auto const ok = static_cast<bool>(move_group_interface.plan(msg));
  return std::make_pair(ok, msg);
}();

// Execute the plan
if (success) {
  draw_trajectory_tool_path(plan.trajectory);
  moveit_visual_tools.trigger();
  prompt("Press 'Next' in the RvizVisualToolsGui window to execute");
  draw_title("Executing");
  moveit_visual_tools.trigger();
  move_group_interface.execute(plan);
} else {
  draw_title("Planning Failed!");
  moveit_visual_tools.trigger();
  RCLCPP_ERROR(logger, "Planning failed!");
}

8、在rviz中启用可视化

启用rviz，并取消勾选“Motion Planning”，不需要在这部分使用。

# launch rviz
ros2 launch xarm_moveit_config xarm6_moveit_fake.launch.py add_gripper:=True

使用 “Panels/Add New Panel” 添加RvizVisualToolsGui。

最后，我们需要添加一个 Marker Array 来渲染我们添加的可视化效果。单击 “Displays” 面板中的 “Add” 按钮，并将新 Marker Array /rviz_visual_tools 的主题编辑。

运行验证

9、围绕对象进行规划

​ 创建一个 collision 对象。首先要注意的是它被放置在机器人的坐标系中。如果我们有一个感知系统来报告场景中障碍物的位置，那么它就会构建这种信息。因为这只是一个示例，所以我们是手动创建的。在此代码块的末尾需要注意的一点是，通过设置collision_object.ADD，这会导致对象被添加到碰撞场景中。将此代码块放在上一步中设置目标姿势和创建Plan之间。

// 添加头文件
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>

move_group_interface.setPoseTarget(target_pose);

// Create collision object for the robot to avoid
auto const collision_object = [frame_id =
                               move_group_interface.getPlanningFrame()] {
    moveit_msgs::msg::CollisionObject collision_object;
    collision_object.header.frame_id = frame_id;
    collision_object.id = "box1";
    shape_msgs::msg::SolidPrimitive primitive;

    // Define the size of the box in meters
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[primitive.BOX_X] = 0.5;
    primitive.dimensions[primitive.BOX_Y] = 0.1;
    primitive.dimensions[primitive.BOX_Z] = 0.5;

    // Define the pose of the box (relative to the frame_id)
    geometry_msgs::msg::Pose box_pose;
    box_pose.orientation.w = 1.0;  // We can leave out the x, y, and z components of the quaternion since they are initialized to 0
    box_pose.position.x = 0.2;
    box_pose.position.y = 0.2;
    box_pose.position.z = 0.25;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    return collision_object;
}();

prompt("Press 'Next' in the RvizVisualToolsGui window to plan");
draw_title("Planning");
moveit_visual_tools.trigger();

// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    auto const ok = static_cast<bool>(move_group_interface.plan(my_plan));
    return std::make_pair(ok, my_plan);
}();

最后，我们需要将此对象添加到碰撞场景中。为此，我们使用一个名为 PlanningSceneInterface 的对象，它使用 ROS 接口将规划场景的更改传达给 MoveGroup。此代码块应直接位于创建碰撞对象的代码块之后。

// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);

编译运行

# launch rviz2
ros2 launch xarm_moveit_config xarm6_moveit_fake.launch.py add_gripper:=True

ros2 launch hello_moveit moveit_api_pose.launch.py

使用moveit任务构造函数实现拾取和放置

Pick and Place with MoveIt Task Constructor — MoveIt Documentation: Rolling documentation

MoveIt任务构造器（MTC）的核心思想是，复杂的运动规划问题可以被分解为一组更简单的子问题。顶层的规划问题被定义为任务，而所有子问题则由阶段来定义。阶段可以按任意顺序和层次结构排列，仅受限于各阶段类型的约束。阶段的排列顺序受结果传递方向的限制。根据结果流的传递方向，存在三种可能的阶段类型：

• 生成器（Generator）：独立于相邻阶段计算结果，并向前后两个方向传递结果。例如，用于几何位姿的逆运动学（IK）采样器，其相邻阶段（如接近或离开动作）的解决方案依赖于该阶段的结果。
• 传播器（Propagator）：接收一个相邻阶段的结果，求解子问题后，将结果传递给另一侧的相邻阶段。根据具体实现，传播器可以向前、向后或双向传递结果。例如，基于起始状态或目标状态计算笛卡尔路径的阶段。
• 连接器（Connector）：不传播结果，而是尝试弥合两个相邻阶段结果状态之间的间隙。例如，计算从给定状态到另一状态的自由运动规划。

除了阶段顺序类型，还存在不同的层次类型，允许封装子阶段：

• 基础阶段（Primitive Stage）：不包含子阶段的阶段。
• 容器阶段（Container Stage）：包含子阶段的高层阶段。容器阶段分为三种类型：
  • 封装容器（Wrapper）：封装单个子阶段并修改或过滤其结果。例如，仅接受子阶段满足特定约束解的过滤器阶段。另一种典型应用是逆运动学封装阶段，它基于标注了位姿目标属性的规划场景生成逆运动学解。
  • 串行容器（Serial Container）：包含一系列子阶段，仅将端到端解决方案视为结果。例如，由多个连贯步骤组成的抓取动作。
  • 并行容器（Parallel Container）：组合多个子阶段，可用于传递最佳替代结果、运行备用求解器或合并多个独立解。例如，为自由运动规划运行替代规划器、优先用右手抓取物体并设置左手作为备用方案，或同时移动手臂和打开夹爪。

阶段不仅支持解决运动规划问题，还可用于处理各种状态转换（例如修改规划场景）。结合类继承的灵活性，仅需依赖一组结构良好的基础阶段即可构建非常复杂的行为。

1、start

拉取moveit_task_constructor包，安装相关依赖

cd ~/dev_ws/src

git clone -b $ROS_DISTRO https://github.com/moveit/moveit_task_constructor.git

rosdepc install --from-paths . --ignore-src --rosdistro $ROS_DISTRO

# 编译测试
colcon build --mixin release

报错信息

# mixin作用：使用colcon进行编译的时候有时会传入很多cmake-args，或者其他的args，并且还是固定每次都要传入，因此通过mixin提供一个快捷输入这一大串args的方式
dod@qDoDp:~/dev_ws$ colcon build --mixin release
usage: colcon [-h] [--log-base LOG_BASE] [--log-level LOG_LEVEL]
              {build,extension-points,extensions,graph,info,list,metadata,mixin,test,test-result,version-check}
              ...
colcon: error: Mixin 'release' is not available for 'build'

需要配置mixin

# 已有
sudo apt install python3-colcon-mixin
# 添加mixin源
colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
# 更新源
colcon mixin update default

# re build
--- stderr: moveit_task_constructor_core                                             
lto-wrapper: warning: using serial compilation of 12 LTRANS jobs
lto-wrapper: note: see the ‘-flto’ option documentation for more information
---
Finished <<< moveit_task_constructor_core [55.0s]
1 packages had stderr output: moveit_task_constructor_core

2、try demos

MoveIt Task Constructor 包包含几个基本示例和一个拾取和放置演示。对于所有演示，您都应该启动基本环境：

ros2 launch moveit_task_constructor_demo demo.launch.py

随后，您可以运行各个演示：

ros2 launch moveit_task_constructor_demo run.launch.py exe:=cartesian
ros2 launch moveit_task_constructor_demo run.launch.py exe:=modular
ros2 launch moveit_task_constructor_demo run.launch.py exe:=pick_place_demo

在右侧，您应该会看到 Motion Planning Tasks 面板，其中概述了任务的层次结构阶段结构。当您选择特定阶段时，成功和失败的解决方案列表将显示在最右侧的窗口中。选择其中一个解决方案将开始其可视化。