MoveIt2——7. 场景规划ROS API

最新推荐文章于 2023-09-05 22:10:12 发布

babyrtsh .

最新推荐文章于 2023-09-05 22:10:12 发布

阅读量298

点赞数 1

分类专栏： ROS Moveit2 文章标签： ubuntu c++

本文链接：https://blog.csdn.net/Avada_C_L/article/details/125675683

版权

Moveit2 同时被 2 个专栏收录

10 篇文章 11 订阅

订阅专栏

ROS

2 篇文章 0 订阅

订阅专栏

场景规划ROS API

介绍使用规划场景差分来执行，在机器人世界中添加和删除物体，在机器人上连接和分离物体

运行代码

打开两个终端窗口，在第一个终端窗口中启动RViz并等待加载完所有项目：

ros2 launch moveit2_tutorials move_group.launch.py

在第二个终端窗口中运行本演示的启动文件：

ros2 launch moveit2_tutorials planning_scene_ros_api_tutorial.launch.py

片刻之后，RViz窗口应该会出现，看起来类似于“在RViz中快速上手MoveIt2”教程中“RViz可视化工具”部分的结果图。要完成每个演示步骤，在RViz处于活动状态（聚焦）时按屏幕底部RvizVisualToolsGui面板中的“Next”按钮，或选择屏幕顶部“Tools”面板中的“Key Tool”后按键盘上的“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 API（应用程序接口）

至规划场景发布者节点的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" };

将物体添加到环境中

通过将某个物体添加到规划场景“world”部分中的碰撞物体集合来将该物体添加到环境中。注意，这里只使用了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");

插曲：同步与异步更新

有以下两种不同方法可以使用“差分（diffs）”与move_group节点进行交互：

通过一个rosservice调用发送一个“差分（diffs）”并阻塞直到应用该“差分”（同步更新）
通过某个话题发送一个“差分（diffs）”，即使该“差分”可能尚未应用也继续执行后续进程（异步更新）

虽然本教程大部分时间都使用后一种方法（考虑到为可视化目的插入的长时间休眠，异步更新不会造成问题），但将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);

然后等待该服务响应：

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;

请注意这里是如何通过首先清除这些字段来确保diff消息不包含其他的已连接物体或碰撞物体。

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

请注意这里是如何通过首先清除这些字段来确保diff消息不包含其他已连接到物体或碰撞物体。

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