规划场景 PlanningScene 的 ROS API¶

在本教程中，我们将研究如何执行两个操作来变更 Planning Scene：

向 world 添加或移除对象；

将物体附加到 world 里或从 world 里分离。

开始¶

请先确保已经完成了入门里的步骤。

运行代码¶

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

roslaunch panda_moveit_config demo.launch

在另一个 shell 里使用为本示例启动 launch 文件：

roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch

注意: 本教程使用 RvizVisualToolsGui 面板来逐步运行此 demo。想要在 RViz里添加这个面板，请按照可视化指南中的说明。

RViz 窗口过一会儿就会出现，看起来和本页面顶部的窗口差不多。想要依次查看每个演示步骤，要么按下窗口底部 RvizVisualToolsGui 面板里的 Next 按钮，或者在 RViz 窗口聚焦状态下，选择窗口顶部 Tools 面板下的 Key Tool ，然后按下键盘上的 N 。

预期输出¶

在 RViz 里，我们应该能看到以下效果：

物体出现在了规划场景里；

物体固连到了机器人上；

物体从机器人上脱离；

物体被从规划场景里移除。

注意: 您可能会看到一条显示 Found empty JointState message 的错误信息，这是一个已知的错误，将尽快修复。

整个代码¶

全部代码可以在 MoveIt GitHub project 里找到。

可视化¶

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

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

ROS API¶

planning scene 发布者的 ROS API 是通过使用 “diffs” 的话题接口实现的。 planning scene diff 是当前规划场景（由 move_group 节点维护）与用户所需的新规划场景之间的差异。

发布所需的话题¶

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

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";
/* 头必须包含有效的 TF 坐标系 */
attached_object.object.header.frame_id = "panda_hand";
/* 对象的id */
attached_object.object.id = "box";

/* 一个默认位姿 */
geometry_msgs::Pose pose;
pose.position.z = 0.11;
pose.orientation.w = 1.0;

/* 定义一个要附加的 box */
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" };

将一个对象添加到环境里¶

将对象添加到环境中的方法是将其添加到规划场景的 “world” 部分中的一组碰撞对象中。注意，这里我们仅使用 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");

插曲：同步与异步更新¶

有两种单独的机制可使用变更信息与 move_group 节点进行交互：

通过 rosservice 调用发送变更信息并阻塞，直到变更信息被接受并应用（同步更新）
通过话题发送变更信息，即使变更信息可能尚未生效也继续（异步更新）

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

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

并通过 service 调用 call() 方法来将变更信息发送到规划场景：

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

请注意，直到我们确定变更信息已被接受并应用后，此操作才会继续。

将对象固连到机器人¶

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

添加对象需要两项操作

从环境中删除原始对象；

将对象固连到机器人。

/* 首先, 定义 REMOVE 物体的信息 */
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "panda_hand";
remove_object.operation = remove_object.REMOVE;

请注意我们是如何通过首先清除 collision_objects 来确保变更消息不包含其他附加对象或碰撞对象的。

/* 执行 REMOVE + ATTACH 操作 */
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");

从机器人上取走物体¶

从机器人上取走物体需要两项操作：

从机器人上取走物体

将该物体重新引入环境

/* 首先, 定义 DETACH 物体的信息 */
moveit_msgs::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "panda_hand";
detach_object.object.operation = attached_object.object.REMOVE;

请注意我们是如何通过首先清除 collision_objects 来确保变更消息不包含其他附加对象或碰撞对象的。

/* 执行 DETACH + ADD 操作 */
ROS_INFO("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");

从碰撞体世界中移除物体¶

从碰撞体世界中删除对象仅需要使用前面定义的删除对象消息。请注意我们是如何通过首先清除 collision_objects 来确保变更消息不包含其他附加对象或碰撞对象的。

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

launch 文件¶

整个 launch 文件在 GitHub 上的这里可见。本教程中的所有代码都可从 moveit_tutorials 包中运行，这个包是 MoveIt 安装的一部分。

调试和规划场景监视器¶

为了帮助调试物体分离和固连，以下命令行工具可以帮助你检查：

rosrun moveit_ros_planning moveit_print_planning_scene_info

开源反馈

有地方需要改进？请在 GitHub page 新开一个 pull request。