Moveit2使用说明（C++）

boss-dog2025-12-10 8:59

目录

  • 0.MoveIt2的核心架构
  • [1.Tutorials------Getting Started](#1.Tutorials——Getting Started)
  • [2.Tutorials------MoveIt Quickstart in RViz](#2.Tutorials——MoveIt Quickstart in RViz)
  • [3.Tutorials------Your First C++ MoveIt Project](#3.Tutorials——Your First C++ MoveIt Project)
    • [3.1 创建功能包（Package）](#3.1 创建功能包（Package）)
    • [3.2 创建 ROS 节点（Node）和执行器（Executor）](#3.2 创建 ROS 节点（Node）和执行器（Executor）)
    • [3.4 构建与运行](#3.4 构建与运行)
  • [4.Tutorials------Visualizing In RViz](#4.Tutorials——Visualizing In RViz)
    • [4.1 添加 moveit_visual_tools 依赖](#4.1 添加 moveit_visual_tools 依赖)
    • [4.2 创建 ROS 执行器并在线程中运行节点](#4.2 创建 ROS 执行器并在线程中运行节点)
    • [4.3 创建并初始化 MoveItVisualTools](#4.3 创建并初始化 MoveItVisualTools)
    • [4.4 编写可视化相关的闭包](#4.4 编写可视化相关的闭包)
    • [4.5 可视化程序的执行步骤](#4.5 可视化程序的执行步骤)
    • [4.6 在 RViz 中启用可视化功能](#4.6 在 RViz 中启用可视化功能)
    • [4.7 程序运行效果](#4.7 程序运行效果)
    • 源码
  • [5.Tutorials------Planning Around Objects](#5.Tutorials——Planning Around Objects)
    • [5.1 添加规划场景接口的头文件](#5.1 添加规划场景接口的头文件)
    • [5.2 修改目标位姿](#5.2 修改目标位姿)
    • [5.3 创建碰撞对象](#5.3 创建碰撞对象)
    • [5.4 将对象添加到规划场景](#5.4 将对象添加到规划场景)
    • [5.5 运行程序并观察效果](#5.5 运行程序并观察效果)
    • 源码
  • [6.Tutorials------Pick and Place with MoveIt Task Constructor](#6.Tutorials——Pick and Place with MoveIt Task Constructor)

0.MoveIt2的核心架构

  • Planning Scene（规划场景）：相当于城市地图，记录所有道路和障碍物信息
  • MoveGroup Interface（运动组接口）：类似交通指挥中心，接收指令并协调各个组件
  • Trajectory Execution（轨迹执行）：如同交通信号灯和车辆调度，确保运动按计划进行

1.Tutorials------Getting Started

https://moveit.picknik.ai/main/doc/tutorials/getting_started/getting_started.html

  • 关于moveit的安装我没参考官网的方式，而是用的源码编译安装的，具体的可以参考：
    https://blog.csdn.net/qq_45445740/article/details/154125527?spm=1001.2014.3001.5501
  • 官网最后这里说了需要设置你的 Colcon 工作区

    因为我是通过源码编译安装的，所有的包都在install里面，所以如果需要随时使用moveit，可以考虑将下面的工作空间添加到.bashrc中，这里我怕污染环境就先没设置。

2.Tutorials------MoveIt Quickstart in RViz

https://moveit.picknik.ai/main/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html

MoveIt 在 RViz 中的快速入门指南：本教程将教如何使用 RViz 和 MoveIt 显示插件在 MoveIt 中创建运动规划。RViz 是 ROS 中的主要可视化工具，也是机器人调试的实用工具。MoveIt 显示插件支持你搭建虚拟环境（规划场景）、交互式设置机器人的起始状态和目标状态、测试各类运动规划器并可视化输出结果。

步骤 1：启动演示并配置插件

启动演示程序：

bash 复制代码 
source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

若为首次操作，你会看到 RViz 中显示一个空世界，需手动添加运动规划插件：

  • 1.在 RViz 的 Displays 选项卡中，点击 Add。
  • 2.在 moveit_ros_visualization 文件夹中，选择 "Motion Planning" 作为显示类型，点击 OK。
  • 3.此时你应能在 RViz 中看到 Kinova 机器人。

加载运动规划插件后，进行如下配置：

  • 在 Displays 子窗口的 Global Options 标签页中，将 Fixed Frame 字段设为 /base_link。
  • 在 Displays 列表中点击 Motion Planning，进入机器人专属配置：
    • 确保 Robot Description 字段设为 robot_description。
    • 确保 Planning Scene Topic 字段设为 /monitored_planning_scene（点击话题名称可展开下拉菜单选择）。
    • 确保 Planned Path 下的 Trajectory Topic 设为 /display_planned_path。
    • 在 Planning Request 中，将 Planning Group 改为 manipulator（也可在左下角的 Motion Planning 面板中设置）。

实际因为本地加载的机器人（当前是 panda 熊猫机器人）和官方文档中的不是同一个机器人Kinova Gen 3 机器人，所以有些参数不一样。

1.将 Fixed Frame 字段设为 /panda_link0

2.在 Planning Request 中，将 Planning Group 改为 panda_arm


步骤 2：体验机器人可视化效果

RViz 中包含四种重叠的可视化效果：

  • 规划场景（/monitored_planning_scene）中机器人的配置（默认启用）。
  • 机器人的规划路径（默认启用）。
  • 绿色：运动规划的起始状态（默认禁用）。
  • 橙色：运动规划的目标状态（默认启用）。

可通过复选框切换各可视化效果的显示 / 隐藏：

  • 场景机器人：勾选 Scene Robot 树形菜单中的 Show Robot Visual。
  • 规划路径：勾选 Planned Path 树形菜单中的 Show Robot Visual。
  • 起始状态：勾选 Planning Request 树形菜单中的 Query Start State。
  • 目标状态：勾选 Planning Request 树形菜单中的 Query Goal State。
    尝试操作这些复选框，体验不同可视化效果的切换。

步骤 3：与 Kinova Gen 3 机器人交互

实际本地加载的是 panda 熊猫机器人

后续步骤需仅显示场景机器人、起始状态和目标状态：

  • 1.勾选 Planned Path 树形菜单中的 Show Robot Visual。
  • 2.取消勾选 Scene Robot 树形菜单中的 Show Robot Visual。
  • 3.勾选 Planning Request 树形菜单中的 Query Goal State。
  • 4.勾选 Planning Request 树形菜单中的 Query Start State。

此时会出现两个交互式标记（Interactive Markers）：橙色机械臂对应的标记用于设置运动规划的 "目标状态"，绿色机械臂对应的标记用于设置 "起始状态"。若未看到交互式标记，点击 RViz 顶部菜单中的 Interact（注：部分工具可能隐藏，可点击顶部菜单中的 "+" 添加 Interact 工具）。

现在你可以通过这些标记拖动机械臂，调整其姿态，动手尝试吧！

移动至碰撞状态

本小节需隐藏规划路径和目标状态：

  • 1.取消勾选 Planned Path 树形菜单中的 Show Robot Visual。
  • 2.取消勾选 Planning Request 树形菜单中的 Query Goal State。

此时仅显示起始状态（绿色机械臂）。尝试将机械臂调整到两连杆（links）相互碰撞的姿态（若操作困难，确保 Motion Planning 插件 Planning 标签页下的 Use Collision-Aware IK 复选框已取消勾选）。碰撞发生后，相撞的连杆会变为红色。

勾选 Use Collision-Aware IK 复选框后再次尝试：勾选状态下，IK 求解器会持续为期望的末端执行器（end-effector）姿态寻找无碰撞解；未勾选时，求解器允许解中存在碰撞。无论复选框状态如何，相撞的连杆始终会以红色可视化显示。

移动至工作空间可达范围外

注意观察当末端执行器（end-effector）被移至可达工作空间（reachable workspace）外时的现象。

进入下一小节前，重新启用规划路径和目标状态：

  • 1.勾选 Planned Path 树形菜单中的 Show Robot Visual。
  • 2.勾选 Planning Request 树形菜单中的 Query Goal State。

关节运动或零空间运动

你可以通过 Joints 选项卡移动单个关节，或 7 自由度（7-DOF）机器人的冗余关节（redundant joints）。尝试拖动如下动画中的 "Null Space Exploration" 滑块，观察末端执行器保持静止时关节的运动情况。

步骤 4：使用 Kinova Gen 3 进行运动规划

现在可以通过 MoveIt RViz 插件为 Kinova Gen 3 机器人进行运动规划：

  • 1.将起始状态移动到期望位置。
  • 2.将目标状态移动到另一个期望位置。
  • 3.确保两个状态均未与机器人自身发生碰撞。
  • 4.取消勾选 Planned Path 树形菜单中的 Show Trail。
  • 5.在 Motion Planning 窗口的 Planning 标签页下，点击 Plan 按钮。
  • 6.勾选 Planned Path 树形菜单中的 Show Trail，你会看到机械臂的路径以一系列机械臂姿态（manipulator poses）呈现。

查看轨迹路径点

你可以在 RViz 中逐点可视化查看轨迹（trajectories）：

  • 1.从 Panels 菜单中，选择 Trajectory - Trajectory Slider，RViz 会新增一个滑块面板。
  • 2.设置目标姿态（goal pose），然后运行 Plan。
  • 3.操作滑块面板，例如拖动滑块、点击 "Play" 按钮。
    注：将末端执行器移动到新目标后，务必先运行 Plan 再点击 Play------ 否则会显示上一个目标的路径点（若存在）。

规划笛卡尔运动

若勾选 Use Cartesian Path 复选框，机器人会尝试让末端执行器在笛卡尔空间（Cartesian space）中线性移动。

  • 不勾选 Use Cartesian Path 复选框（关节运动）
  • 勾选 Use Cartesian Path 复选框（直线运动）

执行轨迹与调整速度

规划成功后，点击 "Plan & Execute" 或 "Execute" 会将轨迹发送给机器人 ------ 本教程中，由于使用的是 kinova_demo.launch，机器人仅为仿真模式。

初始状态下，速度（velocity）和加速度（acceleration）的默认缩放比例为机器人最大值的 10%（0.1）。你可以在下方的 Planning 标签页中修改这些缩放系数，或在机器人的 moveit_config 配置文件（joint_limits.yaml 中）修改默认值。

后续步骤

RViz 可视化工具

许多教程会使用 moveit_visual_tools 逐步演示。继续学习下一个教程前，建议启用 RViz Visual Tools GUI ：

从 Panels 菜单中，选择 Add New Panels。

在菜单中选择 RViz Visual Tools GUI，点击 OK，新面板会添加到 RViz 中。

保存配置

RViz 支持通过 File -> Save Config 保存配置，建议在继续下一个教程前执行此操作。若需自定义配置文件名，可通过 File -> Save Config As 保存，之后通过以下命令调用该配置文件：

bash 复制代码 
ros2 launch moveit2_tutorials demo.launch.py rviz_config:=your_rviz_config.rviz

将 your_rviz_config.rviz 替换为你保存的文件名，并存放到 moveit2_tutorials/doc/tutorials/quickstart_in_rviz/launch/ 目录下，然后构建工作空间（workspace）使其可被识别。

3.Tutorials------Your First C++ MoveIt Project

https://moveit.picknik.ai/main/doc/tutorials/your_first_project/your_first_project.html

3.1 创建功能包（Package）

打开终端，配置 ROS2 环境，确保 ros2 命令可正常使用。导航至 "入门指南" 中创建的 ws_moveit 目录。进入 src 目录（源代码存放位置）。通过 ROS 2 命令行工具创建新功能包：

bash 复制代码 
ros2 pkg create \
  --build-type ament_cmake \
  --dependencies moveit_ros_planning_interface rclcpp \
  --node-name hello_moveit_node hello_moveit

命令执行后，会在新目录中生成相关文件。

注：我们添加了 moveit_ros_planning_interface 和 rclcpp 作为依赖项。这会自动在 package.xml 和 CMakeLists.txt 文件中添加必要配置，使功能包可依赖这两个包。

用你常用的编辑器打开新生成的源代码文件：ws_moveit/src/hello_moveit/src/hello_moveit.cpp。

3.2 创建 ROS 节点（Node）和执行器（Executor）

这里的代码和官方给的源代码中有几处不同：

1.move_group_interface.h，而不是move_group_interface.hpp

2.机器人已经变成了panda，所以规划组要改：panda_arm

CMakeLists.txt

xml 复制代码 
# 1. 声明 C++ 标准（MoveIt 要求 C++17 及以上）
cmake_minimum_required(VERSION 3.8)
project(hello_moveit)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 2. 查找依赖包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(geometry_msgs REQUIRED)  # 用到 Pose 消息时需要

# 3. 编译节点（替换为你的节点名）
add_executable(hello_moveit_node src/hello_moveit_node.cpp)
ament_target_dependencies(
  hello_moveit_node
  rclcpp
  moveit_ros_planning_interface
  geometry_msgs
)

# 4. 安装节点
install(TARGETS
  hello_moveit_node
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

hello_moveit_node.cpp

cpp 复制代码 
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char* argv[])
{
  // 初始化 ROS 并创建节点
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // 创建 ROS 日志器
  auto const logger = rclcpp::get_logger("hello_moveit !");

  // 创建 MoveIt MoveGroup 接口
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "panda_arm");

  // 设置目标姿态（Target Pose）
  auto const target_pose = []{
    geometry_msgs::msg::Pose msg;
    msg.orientation.w = 1.0;
    msg.position.x = 0.28;
    msg.position.y = -0.2;
    msg.position.z = 0.5;
    return msg;
  }();
  move_group_interface.setPoseTarget(target_pose);

  // 规划到目标姿态的路径
  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);
  }();

  // 执行规划路径
  if (success)
  {
    move_group_interface.execute(plan);
  }
  else
  {
    RCLCPP_ERROR(logger, "规划失败！");
  }

  // 关闭 ROS
  rclcpp::shutdown();
  return 0;
}
  • 代码解析

3.4 构建与运行

与之前相同，先构建代码（在 ws_moveit 目录下）：

bash 复制代码 
colcon build

构建成功后，在新终端配置环境并启动上一教程中的演示启动文件（用于启动 RViz 和 MoveGroup 节点）：

bash 复制代码 
# 加载你之前保存的rviz文件，如果没保存会打开后显示默认的
ros2 launch moveit2_tutorials demo.launch.py rviz_config:=your_rviz_config.rviz

在 RViz 的 Displays 窗口中，展开 MotionPlanning/Planning Request，取消勾选 Query Goal State。

再打开一个终端，配置环境并运行程序：

bash 复制代码 
ros2 run hello_moveit hello_moveit_node

此时 RViz 中的机器人会移动，最终到达目标姿态（效果如下）。

PS：若未先启动演示文件就运行 hello_moveit 节点，程序会等待 10 秒后输出以下错误并退出：

ERROR\] \[1644181704.350825487\] \[hello_moveit\]: Could not find parameter robot_description and did not receive robot_description via std_msgs::msg::String subscription within 10.000000 seconds. 原因是 demo.launch.py 会启动提供机器人描述（robot description）的 MoveGroup 节点。MoveGroupInterface 构建时会查找发布机器人描述话题的节点，若 10 秒内未找到，就会输出错误并终止程序。

  • 代码解析

4.Tutorials------Visualizing In RViz

https://moveit.picknik.ai/main/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.html

4.1 添加 moveit_visual_tools 依赖

  • 在 hello_moveit 项目的package.xml文件中，在其他<depend>语句之后添加以下内容：
xml 复制代码 
<depend>moveit_visual_tools</depend>
  • 然后在CMakeLists.txt文件的find_package语句部分添加以下内容：
bash 复制代码 
find_package(moveit_visual_tools REQUIRED)
  • 在该文件的后续部分，扩展ament_target_dependencies宏调用，将新的依赖项包含其中，如下所示：
bash 复制代码 
ament_target_dependencies(
  hello_moveit
  "moveit_ros_planning_interface"
  "moveit_visual_tools"
  "rclcpp"
)
  • 为验证依赖项添加是否正确，请在源文件hello_moveit.cpp中添加所需的头文件引用：
cpp#include 复制代码 
#include <moveit_visual_tools/moveit_visual_tools.h>
  • 要测试上述配置是否生效，可以回到初始的moveit2_ws空间，单独编译hello_moveit试下有没有问题
bash 复制代码 
cd /moveit_ws
colcon build --packages-select hello_moveit

4.2 创建 ROS 执行器并在线程中运行节点

  • 在初始化 MoveItVisualTools 之前，我们需要让 ROS 节点上的执行器保持运行状态。这是因为 MoveItVisualTools 与 ROS 服务和话题的交互方式决定了这一步是必需的。首先，在文件顶部的头文件引用部分添加线程库：
cpp 复制代码 
#include <thread>  // <---- 添加到顶部的头文件引用集合中
  • 通过创建并命名日志记录器，我们可以让程序日志更有条理：
cpp 复制代码 
// 创建ROS日志记录器
auto const logger = rclcpp::get_logger("hello_moveit");
  • 接下来，在创建 MoveIt 的 MoveGroup 接口之前添加执行器相关代码：
cpp 复制代码 
// 启动单线程执行器，使MoveItVisualTools能够与ROS进行交互
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto spinner = std::thread([&executor]() { executor.spin(); });
// 创建MoveIt的MoveGroup接口
  • 最后，确保在程序退出前合并线程：
cpp 复制代码 
// 关闭ROS
rclcpp::shutdown();  // <--- 这将使线程中的spin函数返回
spinner.join();  // <--- 退出前合并线程
return 0;

完成上述修改后，重新构建工作空间，确保没有语法错误。

4.3 创建并初始化 MoveItVisualTools

  • 在构建完 MoveGroupInterface 之后，我们将构建并初始化 MoveItVisualTools：

注意这里因为默认是panda机器人，所以将manipulator改为panda_arm

cpp 复制代码 
// 创建MoveIt的MoveGroup接口
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm"); 

// 构建并初始化MoveItVisualTools
auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
  node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
  move_group_interface.getRobotModel()
};
moveit_visual_tools.deleteAllMarkers();
moveit_visual_tools.loadRemoteControl();

我们向构造函数传入以下参数：ROS 节点、机器人的基础连杆（base link）、要使用的标记话题（后续会详细说明）以及机器人模型（通过 move_group_interface 获取）。接下来，调用删除所有标记的方法，这会清除 RViz 中之前运行留下的任何渲染状态。最后，加载远程控制功能。远程控制是一个非常简单的插件，它能让我们在 RViz 中通过一个按钮与程序进行交互。

4.4 编写可视化相关的闭包

  • 构建并初始化完成后，我们创建一些闭包（能够访问当前作用域中变量的函数对象），后续可在程序中使用这些闭包在 RViz 中渲染可视化效果：

注意这里因为默认是panda机器人，所以将manipulator改为panda_arm

cpp 复制代码 
// 创建可视化相关的闭包
auto const draw_title = [&moveit_visual_tools](auto text) {
  auto const text_pose = [] {
    auto msg = Eigen::Isometry3d::Identity();
    msg.translation().z() = 1.0;  // 将文本放置在基础连杆上方1米处
    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(
     "panda_arm")](auto const trajectory) {
    moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
  };
  • 这三个闭包均通过引用捕获moveit_visual_tools，最后一个闭包还捕获了我们用于规划的关节模型组对象的指针。每个闭包都会调用moveit_visual_tools上的一个函数，以改变 RViz 中的显示内容。
    第一个闭包 draw_title会在机器人基础上方 1 米处添加文本，这是一种从宏观层面展示程序状态的有效方式。
    第二个闭包 调用prompt函数，该函数会阻塞程序，直到用户在 RViz 中按下 "下一步" 按钮，这在调试时逐步执行程序非常有帮助。
    最后一个闭包会绘制规划轨迹的工具路径，通常有助于从工具的角度理解规划的轨迹。

你可能会疑惑为什么要创建这样的 lambda 表达式，原因很简单，就是为了让后续的代码更易于阅读和理解。在编写软件时，将功能拆分为命名函数通常很有帮助，这些函数可以方便地重复使用并单独进行测试。在下一部分中，你将看到如何使用这些创建好的函数。

4.5 可视化程序的执行步骤

  • 现在，我们来完善程序中间部分的代码。更新规划和执行相关的代码，加入这些新功能：
cpp 复制代码 
// 设置目标姿态
auto const target_pose = [] {
  geometry_msgs::msg::Pose msg;
  msg.orientation.w = 1.0;
  msg.position.x = 0.28;
  msg.position.y = -0.2;
  msg.position.z = 0.5;
  return msg;
}();
move_group_interface.setPoseTarget(target_pose);

// 创建到达该目标姿态的规划
prompt("在RvizVisualToolsGui窗口中按下'下一步'开始规划");
draw_title("规划中");
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);
}();

// 执行规划
if (success) {
  draw_trajectory_tool_path(plan.trajectory_);
  moveit_visual_tools.trigger();
  prompt("在RvizVisualToolsGui窗口中按下'下一步'开始执行");
  draw_title("执行中");
  moveit_visual_tools.trigger();
  move_group_interface.execute(plan);
} else {
  draw_title("规划失败！");
  moveit_visual_tools.trigger();
  RCLCPP_ERROR(logger, "规划失败！");
}

你会很快发现，每次调用函数改变 RViz 中的渲染内容后，都需要调用moveit_visual_tools的trigger方法。这是因为发送到 RViz 的消息会被批量处理，只有调用trigger时才会发送，以此减少标记话题的带宽占用。

  • 最后，再次构建项目，确保所有新增代码都正确无误：
bash 复制代码 
cd /moveit_ws
colcon build --packages-select hello_moveit

4.6 在 RViz 中启用可视化功能

  • 打开一个新终端，获取工作区的源代码，然后启动打开RViz的演示启动文件
bash 复制代码 
cd /moveit_ws
source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py
  • 在 "显示"（Displays）选项卡中取消勾选 "运动规划"（MotionPlanning）以隐藏该功能，接下来的部分我们不会使用 "运动规划" 插件。------后面的规划是通过代码实现的，不是界面上的功能
  • 要添加用于与程序中提示交互的按钮，请通过 左上角"面板"（Panels）/"添加新面板"（Add New Panel）菜单打开对话框：
  • 然后选择RvizVisualToolsGui并点击 "确定"（OK），这将在左下角创建一个新面板，其中包含我们后续会使用的 "下一步"（Next）按钮。
  • 最后，我们需要添加 "标记数组"（Marker Array）来渲染已添加的可视化内容。在 "显示"（Displays）面板中点击 "添加"（Add）按钮：

    选择 "标记数组"（MarkerArray）并点击 "确定"（OK）。
  • 在 "显示"（Displays）面板中向下滚动到该新增项，将其使用的话题修改为/rviz_visual_tools。但我这里没有这个话题

    现在，你已准备好运行带有可视化功能的新程序。

4.7 程序运行效果

  • 在新的终端中，进入工作空间，设置环境变量，然后运行hello_moveit：
bash 复制代码 
source install/setup.bash 
ros2 run hello_moveit hello_moveit_node
  • 你会发现程序已暂停，并显示如下日志提醒你点击 RvizVisualToolsGui 中的 Next
    点击 RViz 中的 "下一步"（Next）按钮，观察应用程序的运行进度。
    点击 "下一步" 按钮后，你会看到应用程序开始规划，在机器人上方添加了标题，并绘制了一条代表工具路径的线条。要继续执行，请再次按下 "下一步"（Next）按钮，观察机器人执行规划的过程。

源码

  • package.xml
xml 复制代码 
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>hello_moveit</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="zw.zhou@duoduo.ai">zzw</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>moveit_ros_planning_interface</depend>
  <depend>rclcpp</depend>
  <depend>moveit_visual_tools</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
  • CMakeLists.txt
xml 复制代码 
# 1. 声明 C++ 标准（MoveIt 要求 C++17 及以上）
cmake_minimum_required(VERSION 3.8)
project(hello_moveit)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 2. 查找依赖包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(geometry_msgs REQUIRED)  # 用到 Pose 消息时需要
find_package(moveit_visual_tools REQUIRED)

# 3. 编译节点（替换为你的节点名）
add_executable(hello_moveit_node src/hello_moveit_node.cpp)
ament_target_dependencies(
  hello_moveit_node
  "rclcpp"
  "moveit_ros_planning_interface"
  "moveit_visual_tools"
  "geometry_msgs"
)

# 4. 安装节点
install(TARGETS
  hello_moveit_node
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
  • hello_moveit_node.cpp
cpp 复制代码 
#include <memory>
#include <thread>

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

int main(int argc, char *argv[])
{
    // 初始化 ROS 并创建节点
    rclcpp::init(argc, argv);
    auto const node =
        std::make_shared<rclcpp::Node>("hello_moveit",
                                       rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    // 创建 ROS 日志器
    auto const logger = rclcpp::get_logger("hello_moveit !");

    // 为当前状态监视器启动一个SingleThreadedExecutor，以获取有关机器人状态的信息
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]() { executor.spin(); });

    // 创建 MoveIt MoveGroup 接口
    using moveit::planning_interface::MoveGroupInterface;
    auto move_group_interface = MoveGroupInterface(node, "panda_arm");

    // 构造并初始化MoveItVisualTools
    auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{ node,
                                                                       "base_link",
                                                                       rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                                                       move_group_interface.getRobotModel() };
    moveit_visual_tools.deleteAllMarkers();
    moveit_visual_tools.loadRemoteControl();

    // Create a closure for updating the text in rviz
    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("panda_arm")](
            auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); };

    // 设置目标姿态（Target Pose）
    auto const target_pose = [] {
        geometry_msgs::msg::Pose msg;
        msg.orientation.w = 1.0;
        msg.position.x    = 0.28;
        msg.position.y    = -0.2;
        msg.position.z    = 0.5;
        return msg;
    }();
    move_group_interface.setPoseTarget(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);
    }();

    // 执行规划
    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!");
    }

    // 关闭 ros
    rclcpp::shutdown();
    spinner.join();
    return 0;
}

5.Tutorials------Planning Around Objects

https://moveit.picknik.ai/main/doc/tutorials/planning_around_objects/planning_around_objects.html

本教程将向你介绍如何在规划场景中添加障碍物，并实现绕障碍物规划。

5.1 添加规划场景接口的头文件

在源文件顶部的头文件列表中添加以下内容：

cpp 复制代码 
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>

5.2 修改目标位姿

首先，通过以下修改更新目标位姿，使机器人规划到新的位置：

cpp 复制代码 
// 设置更新后的目标位姿
auto const target_pose = [] {
  geometry_msgs::msg::Pose msg;
  msg.orientation.y = 0.8;
  msg.orientation.w = 0.6;
  msg.position.x = 0.1;
  msg.position.y = 0.4;
  msg.position.z = 0.4;
  return msg;
}();
move_group_interface.setPoseTarget(target_pose);

5.3 创建碰撞对象

在接下来的代码块中，我们将创建一个碰撞对象。需要注意的是，该对象将被放置在机器人的坐标系中。

**如果我们有感知系统可报告场景中障碍物的位置，其生成的消息格式与此类似。**本示例中我们将手动创建该对象。代码块末尾设置消息的操作类型为 ADD，这将使该对象被添加到碰撞场景中。将此代码块插入到 "设置目标位姿"（步骤 2）和 "创建规划" 的代码之间。

cpp 复制代码 
// 创建机器人需要避开的碰撞对象
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;
  // 定义立方体的尺寸（单位：米）
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[primitive.BOX_X] = 0.5;  // X轴方向长度
  primitive.dimensions[primitive.BOX_Y] = 0.1;  // Y轴方向长度
  primitive.dimensions[primitive.BOX_Z] = 0.5;  // Z轴方向长度
  
  // 定义立方体的位姿（相对于frame_id坐标系）
  geometry_msgs::msg::Pose box_pose;
  box_pose.orientation.w = 1.0;  // 四元数的x、y、z分量默认初始化为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;
}();

5.4 将对象添加到规划场景

最后，我们需要将该碰撞对象添加到规划场景中。通过 PlanningSceneInterface 类实现这一操作，该类利用 ROS 接口将规划场景的变更传递给 MoveGroup。此代码块应紧跟在 "创建碰撞对象" 的代码之后。

cpp 复制代码 
// 将碰撞对象添加到场景中
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);

5.5 运行程序并观察效果

按照上一教程的操作方式，使用 demo.launch.py 脚本启动 RViz，然后运行我们的程序。

  • 终端一：
bash 复制代码 
source install/setup.bash 
ros2 launch moveit2_tutorials demo.launch.py

同样的关闭 MotionPlanning ，打开RivzVisualToolsGui

  • 终端二：
bash 复制代码 
colcon build --packages-select hello_moveit
source install/setup.bash 
ros2 run hello_moveit hello_moveit_node

终端二运行之后然后根据终端的提示点击RivzVisualToolsGui 的Next 按钮，可以看到确实避开了障碍物进行规划。


源码

  • package.xml
xml 复制代码 
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>hello_moveit</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="zw.zhou@duoduo.ai">zzw</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>moveit_ros_planning_interface</depend>
  <depend>rclcpp</depend>
  <depend>moveit_visual_tools</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
  • CMakeLists.txt
xml 复制代码 
# 1. 声明 C++ 标准（MoveIt 要求 C++17 及以上）
cmake_minimum_required(VERSION 3.8)
project(hello_moveit)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 2. 查找依赖包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(geometry_msgs REQUIRED)  # 用到 Pose 消息时需要
find_package(moveit_visual_tools REQUIRED)

# 3. 编译节点（替换为你的节点名）
add_executable(hello_moveit_node src/hello_moveit_node.cpp)
ament_target_dependencies(
  hello_moveit_node
  "rclcpp"
  "moveit_ros_planning_interface"
  "moveit_visual_tools"
  "geometry_msgs"
)

# 4. 安装节点
install(TARGETS
  hello_moveit_node
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
  • hello_moveit_node.cpp
cpp 复制代码 
#include <memory>
#include <thread>

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

int main(int argc, char *argv[])
{
    // 初始化 ROS 并创建节点
    rclcpp::init(argc, argv);
    auto const node =
        std::make_shared<rclcpp::Node>("hello_moveit",
                                       rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    // 创建 ROS 日志器
    auto const logger = rclcpp::get_logger("hello_moveit !");

    // 为当前状态监视器启动一个SingleThreadedExecutor，以获取有关机器人状态的信息
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]() { executor.spin(); });

    // 创建 MoveIt MoveGroup 接口
    using moveit::planning_interface::MoveGroupInterface;
    auto move_group_interface = MoveGroupInterface(node, "panda_arm");

    // 构造并初始化MoveItVisualTools
    auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{ node,
                                                                       "base_link",
                                                                       rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                                                       move_group_interface.getRobotModel() };
    moveit_visual_tools.deleteAllMarkers();
    moveit_visual_tools.loadRemoteControl();

    // Create a closure for updating the text in rviz
    auto const draw_title = [&moveit_visual_tools](auto text) {
        auto const text_pose = [] {
            auto msg              = Eigen::Isometry3d::Identity();
            msg.translation().z() = 1.0;
            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("panda_arm")](
            auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); };

    // 设置更新后的目标位姿
    auto const target_pose = [] {
        geometry_msgs::msg::Pose msg;
        msg.orientation.y = 0.8;
        msg.orientation.w = 0.6;
        msg.position.x    = 0.1;
        msg.position.y    = 0.4;
        msg.position.z    = 0.4;
        return msg;
    }();
    move_group_interface.setPoseTarget(target_pose);

    // 创建机器人需要避开的碰撞对象
    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;

        // 定义立方体的尺寸（单位：米）
        primitive.type = primitive.BOX;
        primitive.dimensions.resize(3);
        primitive.dimensions[primitive.BOX_X] = 0.5; // X轴方向长度
        primitive.dimensions[primitive.BOX_Y] = 0.1; // Y轴方向长度
        primitive.dimensions[primitive.BOX_Z] = 0.5; // Z轴方向长度

        // 定义立方体的位姿（相对于frame_id坐标系）
        geometry_msgs::msg::Pose box_pose;
        box_pose.orientation.w = 1.0; // 四元数的x、y、z分量默认初始化为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;
    }();

    // 将碰撞对象添加到场景中
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    planning_scene_interface.applyCollisionObject(collision_object);

    // 创建规划器去目标位姿态
    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);
    }();

    // 执行规划
    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!");
    }

    rclcpp::shutdown();
    spinner.join();
    return 0;
}
  • 使用规划场景进行碰撞和约束检查的示例。
  • 使用规划场景ROS API的示例。
  • 可视化碰撞对象的示例。
  • 用于规划对象的子帧示例。

6.Tutorials------Pick and Place with MoveIt Task Constructor

https://moveit.picknik.ai/main/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.html

相关推荐
fpcc15 小时前
C++编程实践——链式调用的实践
c++
bkspiderx17 小时前
C++中的volatile：从原理到实践的全面解析
开发语言·c++·volatile
君义_noip18 小时前
信息学奥赛一本通 2134：【25CSPS提高组】道路修复 | 洛谷 P14362 [CSP-S 2025] 道路修复
c++·算法·图论·信息学奥赛·csp-s
liulilittle18 小时前
OPENPPP2 Code Analysis One
网络·c++·网络协议·信息与通信·通信
Morwit19 小时前
*【力扣hot100】 647. 回文子串
c++·算法·leetcode
天赐学c语言19 小时前
1.7 - 删除排序链表中的重要元素II && 哈希冲突常用解决冲突方法
数据结构·c++·链表·哈希算法·leecode
w陆压19 小时前
12.STL容器基础
c++·c++基础知识
龚礼鹏20 小时前
Android应用程序 c/c++ 崩溃排查流程二——AddressSanitizer工具使用
android·c语言·c++
qq_4017004120 小时前
QT C++ 好看的连击动画组件
开发语言·c++·qt
额呃呃21 小时前
STL内存分配器
开发语言·c++
热门推荐
01GitHub 镜像站点02安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）03Labelme从安装到标注：零基础完整指南04Linux下V2Ray安装配置指南05【踩坑笔记】50系显卡适配的 PyTorch 安装06手把手教你通过Gemini3 pro 学生认证，白用一年，手慢无！07jdk21下载、安装（Windows、Linux、macOS）08GitLab 零基础入门指南：从安装到项目管理全流程09Opencode CLI 安装成功,但是启动失败10UV安装并设置国内源