【MoveIt 2】直接通过 C++ API 使用 MoveIt : 规划场景监视器

规划场景监视器 

PlanningSceneMonitor 是维护最新规划场景的推荐接口。RobotState、CurrentStateMonitor、PlanningScene、PlanningSceneMonitor 和 PlanningSceneInterface 之间的关系一开始可能会非常混乱。本教程旨在阐明这些关键概念。

机器人状态 RobotState

RobotState 是机器人的快照。它包含 RobotModel 和一组关节值。

 当前状态监视器 CurrentStateMonitor

CurrentStateMonitor (CSM) 可以被认为是 RobotState 的 ROS 包装器。它订阅提供的 JointState 消息主题，这些消息为单自由度执行器（如旋转或棱柱关节）提供最新的传感器值，并使用这些关节值更新其内部 RobotState。除了单自由度关节外，机器人还可以具有多自由度关节，如浮动关节和平面关节。为了维护附加有多自由度关节的连杆和其他框架的最新变换信息，CSM 存储了一个 TF2 缓冲区，该缓冲区使用 TF2 TransformListener 在其内部数据中设置它们的变换。

 规划场景 PlanningScene

PlanningScene 是世界的快照，包括 RobotState 和任意数量的碰撞对象。Planning Scene 可用于碰撞检查以及获取有关环境的信息。

PlanningSceneMonitor

PlanningSceneMonitor 使用 ROS 接口包装了一个 PlanningScene，以保持 PlanningScene 的最新状态。要访问 PlanningSceneMonitor 的底层 PlanningScene，请使用提供的 LockedPlanningSceneRW 和 LockedPlanningSceneRO 类。

PlanningSceneMonitor 具有以下对象，这些对象具有自己的 ROS 接口，用于保持规划场景的子组件的最新状态

• 用于通过 robot_state_subscriber_ 和 tf_buffer_ 跟踪 RobotState 更新的 CurrentStateMonitor，以及用于监听其他发布者的规划场景差异的规划场景订阅者。

• 用于通过 ROS 主题和服务跟踪 OccupancyMap 更新的 OccupancyMapMonitor。

PlanningSceneMonitor 有以下订阅者：

• collision_object_subscriber_ - 监听提供的主题以获取可能在规划场景中添加、删除或修改碰撞对象的 CollisionObject 消息，并将它们传递到其监控的规划场景中

• planning_scene_world_subscriber_ - 监听提供的主题以获取可能包含碰撞对象信息和/或八叉树图信息的 PlanningSceneWorld 消息。这对于保持规划场景监视器同步非常有用。

• attached_collision_object_subscriber_ - 监听提供的主题以获取 AttachedCollisionObject 消息，这些消息决定了对象在机器人状态下与链接的附加/分离。

PlanningSceneMonitor 具有以下服务：

• get_scene_service_ - 这是一个可选服务，用于获取完整的规划场景状态。

PlanningSceneMonitor 初始化为：

• startSceneMonitor - 开始于 planning_scene_subscriber_ ，

• startWorldGeometryMonitor - 启动 collision_object_subscriber_ 、 planning_scene_world_subscriber_ 和 OccupancyMapMonitor，

• startStateMonitor - 启动 CurrentStateMonitor 和 attached_collision_object_subscriber_ ，

• startPublishingPlanningScene - 这将启动另一个线程，用于在提供的主题上发布整个计划场景，以供其他 PlanningSceneMonitors 订阅，并且

• providePlanningSceneService - 开始于 get_scene_service_ 。

PlanningSceneInterface

PlanningSceneInterface 是一个有用的类，用于通过 C++ API 向 MoveGroup 的 PlanningSceneMonitor 发布更新，而无需创建自己的订阅者和服务客户端。没有 MoveGroup 或 MoveItCpp，它可能无法工作。

规划场景监视器

示例代码

以下是一个简单的示例代码，展示了如何使用 PlanningSceneMonitor 进行碰撞检测：

#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/planning_scene/planning_scene.h>


int main(int argc, char** argv)
{
  ros::init(argc, argv, "planning_scene_monitor_example");
  ros::AsyncSpinner spinner(1);
  spinner.start();


  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(
      new planning_scene_monitor::PlanningSceneMonitor("robot_description"));


  planning_scene_monitor->startSceneMonitor();
  planning_scene_monitor->startWorldGeometryMonitor();
  planning_scene_monitor->startStateMonitor();


  planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor);
  collision_detection::CollisionRequest collision_request;
  collision_detection::CollisionResult collision_result;
  scene->checkCollision(collision_request, collision_result);


  if (collision_result.collision)
  {
    ROS_INFO("Collision detected!");
  }
  else
  {
    ROS_INFO("No collision detected.");
  }


  ros::shutdown();
  return 0;
}