【MoveIt 2】您的第一个 C++ MoveIt 项目

第一个 C++ MoveIt 项目教程

最新推荐文章于 2025-06-20 15:08:59 发布

原创最新推荐文章于 2025-06-20 15:08:59 发布 · 1k 阅读

14 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #开发语言

您的第一个 C++ MoveIt 项目

本教程将指导你编写第一个使用 MoveIt 的 C++ 应用程序。

警告：由于需要额外的参数才能实现完整的 Move Group 功能，MoveIt 中的大多数功能将无法正常工作。要进行完整设置，请继续学习 Move Group C++ 接口教程。

先决条件

如果您还没有这样做，请确保您已完成入门中的步骤。

本教程假设您了解 ROS 2 的基础知识。为了为此做好准备，请完成官方 ROS 2 教程，直到“编写一个简单的发布者和订阅者（C++）”。

步骤

1 创建一个包

打开终端并设置你的 ROS 2 安装，以便 ros2 命令可以工作。

导航到你在“入门”教程中创建的 ws_moveit 目录。

进入 src 目录，因为我们将源代码放在那里。

使用 ROS 2 命令行工具创建一个新包：

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

此输出将显示它在新目录中创建了一些文件。

请注意，我们添加了 moveit_ros_planning_interface 和 rclcpp 作为依赖项。这将在 package.xml 和 CMakeLists.txt 文件中创建必要的更改，以便我们可以依赖这两个包。

在您喜欢的编辑器中打开为您在 ws_moveit/src/hello_moveit/src/hello_moveit.cpp 创建的新源文件。

2 创建一个 ROS 节点和执行器

这第一段代码有点样板，但你应该已经习惯在 ROS 2 教程中看到这个。

#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");


  // 下一步代码在这里


  // 关闭 ROS
  rclcpp::shutdown();
  return 0;
}

2.1 构建和运行

我们将在继续之前构建并运行程序以确保一切正常。

将目录更改回工作区目录 ws_moveit 并运行此命令：

colcon build --mixin debug

cxy@cxy-Ubuntu2404:~/ws_moveit$ colcon build --packages-select hello_moveit --mixin debug
Starting >>> hello_moveit
[Processing: hello_moveit]                             
Finished <<< hello_moveit [41.3s]                       


Summary: 1 package finished [46.0s]

成功后，打开一个新的终端，然后在该新的终端中 source 工作区环境脚本，以便我们可以运行我们的程序。

cd ~/ws_moveit
source install/setup.bash

运行你的程序并查看输出。

ros2 run hello_moveit hello_moveit

程序应运行并退出而不会出错。

2.2 检查代码

顶部包含的头文件只是一些标准的 C++头文件，以及我们稍后将使用的 ROS 和 MoveIt 的头文件。

之后，我们有正常的调用来初始化 rclcpp，然后我们创建我们的节点。

auto const node = std::make_shared<rclcpp::Node>(
  "hello_moveit",
  rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);

第一个参数是 ROS 将用于命名唯一节点的字符串。第二个参数是 MoveIt 所需的，因为我们使用 ROS 参数。

接下来，我们创建一个名为“hello_moveit”的日志记录器，以保持我们的日志输出有序且可配置。

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

最后，我们有关闭 ROS 的代码。

// Shutdown ROS
rclcpp::shutdown();
return 0;

3 使用 MoveGroupInterface 进行规划和执行

在注释“Next step goes here”处，添加以下代码：

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


// 设置目标姿态
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, "Planning failed!");
}

3.1 构建和运行

就像以前一样，我们需要先构建代码，然后才能运行它。

在工作区目录 ws_moveit 中运行此命令：

colcon build --mixin debug

cxy@cxy-Ubuntu2404:~/ws_moveit$ colcon build --packages-select hello_moveit --mixin debug
Starting >>> hello_moveit
Finished <<< hello_moveit [9.98s]                      


Summary: 1 package finished [10.3s]

成功后，我们需要重用上一个教程中的 demo 启动文件来启动 RViz 和 MoveGroup 节点。在一个单独的终端中，设置工作区，然后执行：

ros2 launch moveit2_tutorials demo.launch.py

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

在第三个终端中，配置工作区并运行您的程序。

ros2 run hello_moveit hello_moveit

这应该会导致 RViz 中的机器人移动并最终处于这个姿势：请注意，如果你在未先启动 demo 启动文件的情况下运行节点 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 启动文件正在启动提供机器人描述的 MoveGroup 节点。当构建 MoveGroupInterface 时，它会查找发布带有机器人描述的主题的节点。如果在 10 秒内未找到，它将打印此错误并终止程序。

3.2 检查代码

我们首先要做的是创建 MoveGroupInterface 。这个对象将用于与 move_group 交互，这使我们能够规划和执行轨迹。请注意，这是我们在此程序中创建的唯一可变对象。另一个需要注意的是我们在这里创建的 MoveGroupInterface 对象的第二个参数： "manipulator" 。这是在机器人描述中定义的我们将使用此 MoveGroupInterface 操作的关节组。

using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "manipulator");

然后，我们设置目标位姿并规划。请注意，仅设置目标姿势（通过 setPoseTarget ）。起始姿势隐含为关节状态发布器发布的位置，可以使用 MoveGroupInterface::setStartState* 系列函数进行更改（但在本教程中未进行更改）。

关于下一部分代码的另一件事是使用 lambda 构造消息类型 target_pose 和规划。这是你会在现代 C++ 代码库中找到的一种模式，允许以更声明的方式编写代码。有关此模式的更多信息，请参阅本教程末尾的几个链接。

// 设置目标姿态
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, "Planning failed!");
}

总结

您创建了一个 ROS 2 包，并使用 MoveIt 编写了您的第一个程序。
你学习了如何使用 MoveGroupInterface 进行规划和执行移动。
以下是本教程末尾的完整 hello_moveit.cpp 源代码副本。https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/tutorials/your_first_project/kinova_hello_moveit.cpp

进一步阅读

我们使用 lambda 来初始化对象为常量。这被称为 IIFE 技术。阅读更多关于此模式的信息，请参阅 C++ Stories。https://www.cppstories.com/2016/11/iife-for-complex-initialization/
我们还将所有可能的内容声明为 const。阅读更多关于 const 的有用性，请参阅这里。https://www.cppstories.com/2016/12/please-declare-your-variables-as-const/

Next Step

在下一个教程《在 RViz 中可视化》中https://moveit.picknik.ai/main/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.html ，您将扩展您在此处构建的程序，以创建visual markers视觉标记，使理解 MoveIt 的工作变得更容易。

附录：完整代码

#include <memory> // 引入内存管理库


#include <rclcpp/rclcpp.hpp> // 引入ROS2核心库
#include <moveit/move_group_interface/move_group_interface.h> // 引入MoveIt!库


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, "manipulator"); // 使用“manipulator”组


  // 设置目标位姿
  auto const target_pose = []{
    geometry_msgs::msg::Pose msg;
    msg.orientation.w = 1.0; // 设置四元数的w分量
    msg.position.x = 0.28; // 设置x坐标
    msg.position.y = -0.2; // 设置y坐标
    msg.position.z = 0.5; // 设置z坐标
    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, "Planning failed!"); // 规划失败，记录错误
  }


  // 关闭ROS
  rclcpp::shutdown();
  return 0;
}