你的第一个 C++ MoveIt 项目

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

警告： MoveIt 中的大多数功能在单独运行时无法正常工作，因为完整的 Move Group 功能需要额外的参数。要完成完整设置，请继续阅读 Move Group 接口教程。

先决条件

如果你还没有完成入门指南中的步骤，请务必先完成。

本教程假定你已掌握 ROS 2 的基础知识。为此，请先完成官方 ROS 2 教程中直到”编写简单的发布者和订阅者（C++）“的部分。

步骤

1 创建软件包

打开终端并 source 你的 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.hpp>

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

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

  // Next step goes here

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

2.1 构建并运行

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

将目录切换回工作区 ws_moveit，运行以下命令：

colcon build --mixin debug

构建成功后，打开一个新终端，在新终端中 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” 注释的位置，添加以下代码：

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

// Set a 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);

// Create a plan to that 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);
}();

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

3.1 构建并运行

和之前一样，需要先构建代码才能运行。

在工作区目录 ws_moveit 中，运行以下命令：

colcon build --mixin debug

构建成功后，我们需要复用上一个教程中的演示启动文件来启动 RViz 和 MoveGroup 节点。在单独的终端中，source 工作区，然后执行：

ros2 launch moveit2_tutorials demo.launch.py

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

在第三个终端中，source 工作区并运行你的程序：

ros2 run hello_moveit hello_moveit

这将使 RViz 中的机器人移动，最终停留在以下姿态：

注意：如果你在未先启动演示启动文件的情况下直接运行 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++ 代码库中常见的模式，允许你以更具声明性的风格编写代码。关于此模式的更多信息，本教程末尾提供了几个链接。

// Set a 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);

// Create a plan to that 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);
}();

最后，如果规划成功就执行轨迹，否则记录一条错误日志：

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

总结

你创建了一个 ROS 2 软件包，并使用 MoveIt 编写了你的第一个程序。
你了解了如何使用 MoveGroupInterface 来规划和执行运动。
这是本教程结尾处完整的 hello_moveit.cpp 源码副本。

进一步阅读

我们使用 lambda 以便将对象初始化为常量，这种技术称为 IIFE（Immediately Invoked Function Expression）。从 C++ Stories 阅读更多关于此模式的内容。
我们还尽可能将所有变量声明为 const。在此阅读更多关于 const 的作用。

下一步

在下一个教程在 RViz 中可视化中，你将在当前程序的基础上扩展，创建可视化标记，让你更直观地理解 MoveIt 的运行过程。