你的第一个 C++ MoveIt 项目

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

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

先决条件

如果您尚未完成此操作，请确保您已完成入门中的步骤。

本教程假设您了解 ROS 2 的基础知识。

为了做好准备，请完成官方 ROS 2 教程直到“编写简单的发布者和订阅者 (C++)”。

步骤

1 创建包

打开终端并“获取 ROS 2 安装 <https://docs.ros.org/en/rolling/Tutorials/Configuring-ROS2-Environment.html>”以便“ros2”命令可以正常工作。

导航到您在 :doc:“入门教程 </doc/tutorials/getting_started/getting_started>”中创建的“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[])
{
  // 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

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

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”的记录器<https://docs.ros.org/en/humble/Tutorials/Demos/Logging-and-logger-configuration.html>`_，以使我们的日志输出井然有序且可配置。

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

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

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

3 使用 MoveGroupInterface 进行计划和执行

在“下一步从这里开始”的注释处添加以下代码：

// 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 节点。在单独的终端中，获取工作区，然后执行以下命令：

ros2 launch moveit2_tutorials demo.launch.py

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

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

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 来规划和执行移动。
Here is a copy of the full hello_moveit.cpp source at the end of this tutorial.

进一步阅读

我们使用 lambda 来将对象初始化为常量。

这被称为 IIFE 技术。从 C++ Stories 中了解有关此模式的更多信息。 - 我们还将我们可以声明的所有内容声明为 const。在此处阅读有关 const 的更多实用性。

下一步

在下一个教程在 RViz 中可视化中，您将扩展此处构建的程序以创建可视化标记，以便更轻松地理解 MoveIt 正在做什么。