【MoveIt2-humble】入门教程（翻译自官方文档）一：第一个 C++ MoveIt 程序

0 前沿

本系列教程共四节，环境为：

• Ubuntu22.04
• ros2-humble
• MoveIt2-humble

官方文档上的教程，从moveit1的melodic到moveit2的foxy基本一致，但是从最新的humble开始有了很大的变化，其中之一便是 lambda表达式 的广泛使用。

本节为教程的第一节，前提是需要完成上一节安装教程包的内容，可以看我的这篇博文：https://blog.csdn.net/qq_43557907/article/details/125636298
四节教程会手把手带你写一个完整的 Moveit 控制程序，包括轨迹规划、RViz可视化、添加碰撞物体、抓取和放置。

更多信息可以去官方文档查看：https://moveit.picknik.ai/humble/index.html
本系列博客为我个人翻译，方便自己学习使用。

1 创建依赖包

进入到教程所在工作空间下的src目录，创建一个新的依赖包。

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

在新建的这个包中添加hello_moveit.cpp，准备开始编码

2 创建ROS节点和执行器

#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 logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // Next step goes here

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

2.1 编译和运行

我们可以尝试编译并运行一下，看看有无出现问题。

2.2 代码说明

最上面的是标准C++头文件，随后是为使用 ROS 和 Moveit 所添加的头文件。

在这之后，我们初始化 rclcpp，并创建了节点。

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

第一个参数是节点名称。第二个参数对于 Moveit 很重要，因为我们要使用 ROS 参数。

最后，关闭这个节点。

3 使用MoveGroupInterface来规划和执行

在代码中“Next step goes here,”的后面添加以下节点：

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

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

3.1 编译和运行

打开教程中的launch文件去启动rviz和 MoveGroup 节点，在另一个终端中source这个工作空间并执行：

ros2 launch moveit2_tutorials demo.launch.py

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

在第三个终端中 source 并允许我们本节的程序：

ros2 run hello_moveit hello_moveit

注：

如果你没有运行launch文件就运行了 hello_moveit 节点，等待10s后将会报如下错误：

[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 的第二个接口 "panda_arm"，这是在机器人描述中定义的关节组，我们将通过 MoveGroupInterface 去操作它。

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

之后，设置目标位姿和规划。起始点位姿通过 joint state publisher 来发布，它能够使用MoveGroupInterface::setStartState*中的函数来被改变（本教程无）

通过使用 lambda 表达式来构建信息类型 target_pose与规划。

// 利用 lambda 函数来创建目标位姿
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);

// 利用 lambda 函数来创建到目标位姿的规划
// 注：这里使用了 C++ 20标准的新特性
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) {
  move_group_interface.execute(plan);
} else {
  RCLCPP_ERROR(logger, "Planning failed!");
}

扩展知识

我们使用了 lambda 表达式将对象初始化为常量，这被称为 IIFE 技术。Read more about this pattern from C++ Stories.

我们同时知道一切都能作为常量去使用Read more about the usefulness of const here.

下一步

在下一章Visualizing in RViz中，会丰富本章的程序，创建可视化的标记来让我们更容易知道 MoveIt 在做什么。