Skip to content

规划场景监视器

PlanningSceneMonitor(规划场景监视器)是维护最新规划场景(planning scene)的推荐接口。RobotState、CurrentStateMonitor、PlanningScene、PlanningSceneMonitor 与 PlanningSceneInterface 之间的关系一开始可能会让人非常困惑。本教程旨在阐明这些关键概念。

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

CurrentStateMonitor(CSM)可以看作是 RobotState 的一个 ROS 封装。它订阅指定主题,接收 JointState 消息,这些消息提供单个自由度(degree of freedom)执行器(如转动关节或移动关节)的最新传感器值,并用这些关节值更新其内部的 RobotState。除了单个自由度的关节外,机器人还可以拥有多个自由度的关节,例如浮动关节(floating joint)和平面关节(planar joint)。为了维护与通过多自由度关节连接的连杆(link)及其他坐标系相关的变换信息并保持更新,CSM 存储了一个 TF2 Buffer,该 Buffer 使用一个 TF2 TransformListener 在其内部数据中设置这些变换。

PlanningScene 是整个世界的一个快照,其中既包含 RobotState,也包含任意数量的碰撞物体(collision object)。规划场景(Planning Scene)可用于碰撞检测(collision checking),以及获取有关环境的信息。

PlanningSceneMonitor 用 ROS 接口包装一个 PlanningScene,以保持其更新。要访问 PlanningSceneMonitor 底层的 PlanningScene,请使用提供的 LockedPlanningSceneRW 和 LockedPlanningSceneRO 类。

PlanningSceneMonitor 拥有以下对象,这些对象各自具备 ROS 接口,用于保持规划场景各子组件的更新:

  • 一个 CurrentStateMonitor,用于通过 robot_state_subscriber_ 和 tf_buffer_ 跟踪 RobotState 的更新,以及一个规划场景订阅器,用于监听其他发布者发布的规划场景差异(planning scene diff)。
  • 一个 OccupancyMapMonitor,用于通过 ROS 主题和服务跟踪 OccupancyMap 的更新。

PlanningSceneMonitor 具有以下订阅器(subscriber):

  • collision_object_subscriber_:监听指定主题上的 CollisionObject 消息,这些消息可能会添加、移除或修改规划场景中的碰撞物体,并将其传入它所监视的规划场景
  • planning_scene_world_subscriber_:监听指定主题上的 PlanningSceneWorld 消息,这些消息可能包含碰撞物体信息和/或 octomap 信息。这对于保持规划场景监视器之间的同步非常有用
  • attached_collision_object_subscriber_:监听指定主题上的 AttachedCollisionObject 消息,这些消息决定物体在机器人状态中与连杆的附着/脱离

PlanningSceneMonitor 具有以下服务(service):

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

PlanningSceneMonitor 通过以下方式初始化:

  • startSceneMonitor:启动 planning_scene_subscriber_;
  • startWorldGeometryMonitor:启动 collision_object_subscriber_、planning_scene_world_subscriber_ 和 OccupancyMapMonitor;
  • startStateMonitor:启动 CurrentStateMonitor 和 attached_collision_object_subscriber_;
  • startPublishingPlanningScene:启动另一个线程,在指定主题上发布整个规划场景,供其他 PlanningSceneMonitor 订阅;
  • providePlanningSceneService:启动 get_scene_service_。

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