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 ROSrclcpp::init(argc, argv);auto const node = std::make_shared<rclcpp::Node>("hello_moveit",rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));// Create ROS loggerauto const logger = rclcpp::get_logger("hello_moveit");// log infoRCLCPP_INFO(logger, "Hello MoveIt2!");// Shutdown ROSrclcpp::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=0at 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(DIRECTORYlaunchDESTINATION 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 MoveItConfigsBuilderdef 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 nodesreturn [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 FindPackageSharedef 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.pyrobot_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 linkreturn 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 metersprimitive.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 0box_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:=Trueros2 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/srcgit clone -b $ROS_DISTRO https://github.com/moveit/moveit_task_constructor.gitrosdepc 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 面板，其中概述了任务的层次结构阶段结构。当您选择特定阶段时，成功和失败的解决方案列表将显示在最右侧的窗口中。选择其中一个解决方案将开始其可视化。