ROS2- Moveit2 -Planning Scene ROS API (规划场景 ROS API)

在本教程中，我们将研究如何使用规划场景差异来执行两项操作：

• 在世界中添加和移除物体

• 将物体安装到机器人上或从机器人上卸下

入门

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

运行代码

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

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行此演示的启动文件：

ros2 launch moveit2_tutorials planning_scene_ros_api_tutorial.launch.py

片刻之后，RViz 窗口应会出现，看起来类似于可视化教程中的此步骤。要完成每个演示步骤，请按屏幕底部RvizVisualToolsGui面板中的下一步按钮，或选择屏幕顶部工具面板中的Key Tool ，然后在 RViz 处于焦点时按下键盘上的0 。

预期输出

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

• 物体出现在规划场景中。

• 物体附着在机器人上。

• 物体与机器人分离。

• 对象已从规划场景中移除。

完整代码

完整代码可以在 MoveIt GitHub 项目中看到。

可视化

MoveItVisualTools 包提供了许多在 RViz 中可视化对象、机器人和轨迹的功能，以及调试工具。

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

ROS应用程序接口

ROS API 通过使用“diffs”的主题接口与规划场景发布者进行交互。规划场景差异是当前规划场景（由 move_group 节点维护）与用户所需的新规划场景之间的差异。

通告所需主题

我们创建一个发布者并等待订阅者。请注意，可能需要在启动文件中重新映射此主题。

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" };

将对象添加到环境中

通过将对象添加到规划场景的“世界”部分的碰撞对象集合中，将其添加到环境中。请注意，我们在这里仅使用 attachment_object 消息的“object”字段。

RCLCPP_INFO(LOGGER, "Adding the object into the world at the location of the world.");
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 RvizVisualToolGui window to continue the demo.");

插曲：同步更新与异步更新

有两种单独的机制可用于使用 diff 与 move_group 节点进行交互：

• 通过 rosservice 调用发送差异并阻止，直到应用差异（同步更新）

• 通过主题发送差异，即使差异可能尚未应用也继续（异步更新）

虽然本教程的大部分内容都使用后一种机制（考虑到为了可视化目的而插入的长睡眠，异步更新不会造成问题），但用以下服务客户端替换 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.");
  }
}

请注意，直到我们确定已应用差异后，此过程才会继续。

将物体附着到机器人上

当机器人从环境中拾取一个物体时，我们需要将该物体“附着”到机器人上，以便任何处理机器人模型的组件都知道要考虑附着的物体，例如进行碰撞检查。

附加对象需要两个操作

• 从环境中移除原始对象

• 将物体附着到机器人上

/* 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");

从碰撞世界中移除物体

从碰撞世界中移除对象只需使用先前定义的移除对象消息即可。请注意，我们如何通过首先清除这些字段来确保 diff 消息不包含其他附加对象或碰撞对象。'

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