Ubuntu20.04，ROS Neotic。Move It！（7）规划场景ROS API Planning Scene ROS API

最新推荐文章于 2024-07-31 21:59:31 发布

难受啊！马飞...

最新推荐文章于 2024-07-31 21:59:31 发布

阅读量385

点赞数

分类专栏： Moveit学习文章标签：机器学习

本文链接：https://blog.csdn.net/qq_33328642/article/details/116905315

版权

Moveit学习专栏收录该内容

9 篇文章 25 订阅

订阅专栏

Ubuntu20.04，ROS Neotic。Move It！（6）

在这里插入图片描述

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

向世界添加和删除对象
将对象附着和分离到机器人

运行代码

打开两个终端。在第一个外壳程序中，启动RViz并等待所有内容完成加载：

source ~/ws_moveit/devel/setup.bash
roslaunch panda_moveit_config demo.launch

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

source ~/ws_moveit/devel/setup.bash
roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch

注意：本教程使用RvizVisualToolsGui面板逐步演示。要将此面板添加到RViz中，请按照“可视化教程”中的说明进行操作。

之后，将出现RViz窗口，其外观类似于此页面顶部的窗口。要通过每一步演示或者按进度在接下来的按钮RvizVisualToolsGui在屏幕的底部面板或选择关键工具的工具，在屏幕的顶部面板，然后按ñ你的键盘上，而RViz集中。

可视化

软件包MoveItVisualTools提供了许多功能，可用于可视化RViz中的对象，机械手和轨迹以及调试工具，例如脚本的逐步自省。

moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();

ROS API

计划场景发布者的ROS API是通过使用“diffs”的主题界面来实现的。计划场景差异是当前计划场景（由move_group节点维护）与用户所需的新计划场景之间的差异。

Advertise the required topic

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

ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::WallDuration sleep_t(0.5);
while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
  sleep_t.sleep();
}
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

定义附加的对象消息

我们将使用此消息从世界中添加或减去对象，并将该对象附加到机器人。

moveit_msgs::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::Pose pose;
pose.position.z = 0.11;
pose.orientation.w = 1.0;

/* Define a box to be attached */
shape_msgs::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" };

将对象添加到环境中

将对象添加到环境中，方法是将其添加到规划场景的“世界”部分中的一组碰撞对象中。请注意，此处我们仅使用attached_object消息的“ object”字段。

ROS_INFO("Adding the object into the world at the location of the hand.");
moveit_msgs::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调用发送diff 并阻止，直到应用diff （同步更新）为止
通过主题发送diff ，即使diff 可能尚未应用也继续（异步更新）

尽管本教程的大部分内容都使用后一种机制（考虑到为可视化目的而插入的长时间睡眠，异步更新不会造成问题），但完全可以用以下服务客户端替换planning_scene_diff_publisher：

ros::ServiceClient planning_scene_diff_client =
    node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client.waitForExistence();

and send the diffs to the planning scene via a service call:（这边实在不知道该怎么用中文表达）

moveit_msgs::ApplyPlanningScene srv;
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);

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

将对象附加到机器人

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

附加对象需要两项操作

从环境中删除原始对象
将对象附着到机器人

/* First, define the REMOVE object message*/
moveit_msgs::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 */
ROS_INFO("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");

从碰撞世界中移除对象

从碰撞世界中删除对象仅需要使用前面定义的删除对象消息。请注意，同样如何通过首先清除那些字段来确保diff消息不包含其他任何附加对象或碰撞对象。

ROS_INFO("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);