规划场景 ROS API

在本教程中，我们将研究如何使用规划场景差异（planning scene diff）执行两种操作：

• 在世界（world）中添加和移除物体
• 将物体附加（attach）到机器人上，以及从机器人上分离（detach）物体

开始之前

如果尚未完成，请先完成 Getting Started 中的步骤。

运行代码

打开两个终端（shell）。在第一个终端中启动 RViz 并等待所有内容加载完成：

ros2 launch moveit2_tutorials move_group.launch.py

在第二个终端中，运行本演示的启动文件（launch file）：

ros2 launch moveit2_tutorials planning_scene_ros_api_tutorial.launch.py

片刻之后，RViz 窗口应该会出现，其外观与可视化教程中的这一步类似。要逐步完成每个演示步骤，请点击屏幕底部 RvizVisualToolsGui 面板中的 Next 按钮，或者在屏幕顶部的 Tools 面板中选择 Key Tool，然后在 RViz 获得焦点时按下键盘上的 0。

预期输出

在 RViz 中，你应该能够看到以下内容：

• 物体出现在规划场景（planning scene）中。
• 物体被附加到机器人上。
• 物体从机器人上分离。
• 物体从规划场景中移除。

完整代码

完整代码可以在 MoveIt GitHub 项目的此处查看。

可视化

MoveItVisualTools 软件包提供了许多在 RViz 中可视化物体、机器人和轨迹的功能，以及诸如逐步内省（step-by-step introspection）脚本之类的调试工具。

rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "planning_scene_ros_api_tutorial", node);
visual_tools.loadRemoteControl();
visual_tools.deleteAllMarkers();

ROS API

规划场景发布器（planning scene publisher）的 ROS API 通过使用“差异”（diff）的话题接口提供。规划场景差异（planning scene diff）是当前规划场景（由 move_group 节点维护）与用户期望的新规划场景之间的差异。

发布所需的话题

我们创建一个发布器（publisher）并等待订阅者（subscriber）。注意，该话题可能需要在启动文件（launch file）中重新映射（remap）。

rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher =
    node->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 1);
while (planning_scene_diff_publisher->get_subscription_count() < 1)
{
  rclcpp::sleep_for(std::chrono::milliseconds(500));
}
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

定义附着物体消息

我们将使用该消息在世界中添加或移除物体，并将物体附加到机器人上。

moveit_msgs::msg::AttachedCollisionObject attached_object;
attached_object.link_name = "panda_hand";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "panda_hand";
/* The id of the object */
attached_object.object.id = "box";

/* A default pose */
geometry_msgs::msg::Pose pose;
pose.position.z = 0.11;
pose.orientation.w = 1.0;

/* Define a box to be attached */
shape_msgs::msg::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.075;
primitive.dimensions[1] = 0.075;
primitive.dimensions[2] = 0.075;

attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);

注意，将物体附加到机器人需要将相应的操作指定为 ADD 操作。

attached_object.object.operation = attached_object.object.ADD;

由于我们将物体附加到机器人的手部以模拟拾取物体，我们希望碰撞检查器忽略物体与机器人手部之间的碰撞。

attached_object.touch_links = std::vector<std::string>{ "panda_hand", "panda_leftfinger", "panda_rightfinger" };

向环境中添加物体

通过将物体添加到规划场景“世界（world）”部分的碰撞物体（collision object）集合中，即可将物体添加到环境中。注意，这里我们只使用了 attached_object 消息的 “object” 字段。

RCLCPP_INFO(LOGGER, "Adding the object into the world at the location of the hand.");
moveit_msgs::msg::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher->publish(planning_scene);
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

插曲：同步与异步更新

有两种不同的机制可以使用差异（diff）与 move_group 节点交互：

• 通过 rosservice 调用发送差异并阻塞，直到差异被应用（同步更新）
• 通过话题发送差异，然后继续执行，即使差异可能尚未被应用（异步更新）

虽然本教程大部分内容使用后一种机制（考虑到为可视化目的而插入的较长休眠时间，异步更新不会造成问题），但用下面的服务客户端（service client）替换 planning_scene_diff_publisher 也完全合理：

rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>::SharedPtr planning_scene_diff_client =
    node->create_client<moveit_msgs::srv::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client->wait_for_service();

并通过服务调用将差异发送到规划场景：

auto request = std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>();
request->scene = planning_scene;
std::shared_future<std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response>> response_future;
response_future = planning_scene_diff_client->async_send_request(request).future.share();

等待服务响应。

std::chrono::seconds wait_time(1);
std::future_status fs = response_future.wait_for(wait_time);
if (fs == std::future_status::timeout)
{
  RCLCPP_ERROR(LOGGER, "Service timed out.");
}
else
{
  std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response> planning_response;
  planning_response = response_future.get();
  if (planning_response->success)
  {
    RCLCPP_INFO(LOGGER, "Service successfully added object.");
  }
  else
  {
    RCLCPP_ERROR(LOGGER, "Service failed to add object.");
  }
}

注意，在确认差异已被应用之前，程序不会继续执行。

将物体附加到机器人上

当机器人从环境中拾取物体时，我们需要将物体“附加（attach）”到机器人上，这样任何处理机器人模型的组件（例如用于碰撞检测的组件）都会考虑到附着物体。

附加物体需要两个操作：

• 从环境中移除原始物体
• 将物体附加到机器人上

/* First, define the REMOVE object message*/
moveit_msgs::msg::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "panda_hand";
remove_object.operation = remove_object.REMOVE;

注意，我们通过先清空这些字段来确保差异消息中不包含其他附着物体或碰撞物体。

/* Carry out the REMOVE + ATTACH operation */
RCLCPP_INFO(LOGGER, "Attaching the object to the hand and removing it from the world.");
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene.robot_state.is_diff = true;
planning_scene_diff_publisher->publish(planning_scene);
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

从机器人上分离物体

从机器人上分离物体需要两个操作：

• 将物体从机器人上分离
• 将物体重新引入环境中

/* First, define the DETACH object message*/
moveit_msgs::msg::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "panda_hand";
detach_object.object.operation = attached_object.object.REMOVE;

注意，我们通过先清空这些字段来确保差异消息中不包含其他附着物体或碰撞物体。

/* Carry out the DETACH + ADD operation */
RCLCPP_INFO(LOGGER, "Detaching the object from the robot and returning it to the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.robot_state.is_diff = true;
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher->publish(planning_scene);
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

从碰撞世界中移除物体

从碰撞世界中移除物体只需要使用前面定义的移除物体消息。同时请注意，我们通过先清空这些字段来确保差异消息中不包含其他附着物体或碰撞物体。

RCLCPP_INFO(LOGGER, "Removing the object from the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene_diff_publisher->publish(planning_scene);