四、使用MoveGroup C++接口——规划场景监视器（六）

目录

前言

1.Planning Scene Monitor 介绍

1.1 Planning Scene Monitor 架构

1.2 世界几何监视器

1.3 3D感知

1.4 八爪鱼图 

2.Planning Scene Monitor使用案例

3.总结

参考资料

---

前言

        这篇文档介绍了MoveIt中的规划场景监视器（Planning Scene Monitor）的概念、架构、使用案例以及其在机器人运动规划中的作用。

---

1.Planning Scene Monitor 介绍

        规划场景监视器是一个用于存储机器人周围世界表示以及机器人本身状态的对象。该对象的内部状态通常由一个组件维护，该组件能够以线程安全的方式读取和写入状态。规划场景是机器人路径规划和运动规划中非常重要的部分，它包含了机器人、障碍物以及其他环境元素的详细信息。

1.1 Planning Scene Monitor 架构

1.2 世界几何监视器

        世界几何监视器利用机器人上的传感器（如激光雷达或深度相机）以及用户输入的信息来构建世界几何。它使用1.3描述的方法来构建机器人周围环境的3D表示，并通过特定主题添加对象信息来增强这种表示。其中，占据图监视器（Occupancy Map Monitor）是这一过程的关键部分。

1.3 3D感知

        在MoveIt中，3D感知由占据图监视器处理。占据图监视器采用插件架构来处理不同类型的传感器输入，如架构图所示。MoveIt内置支持处理两种类型的输入：

• 点云：由PointCloudOccupancyMapUpdate插件处理
• 深度图像：由DepthImageOccupancyMapUpdate插件处理

需要注意的是，可以作为插件向占据图监视器添加自己类型的更新器。

1.4 八爪鱼图 

        在MoveIt框架中，占据图监视器利用八叉树地图这种数据结构来存储和更新机器人周围环境的占据情况。八叉树地图是一种高效的空间划分数据结构，用于表示三维空间中的占据（被物体占据）和空闲（未被物体占据）区域。每个八叉树的节点代表一个空间单元（或称为“单元格”），而八叉树地图能够对这些单元格进行概率编码，即每个单元格都有一个表示该位置被占据的概率值。然而，在当前的MoveIt实现中，这种概率信息并没有被直接利用来进行路径规划或碰撞检测。

        尽管如此，八叉树地图的结构和数据仍然非常有用，因为它可以直接与FCL（Flexible Collision Library）集成。FCL是一个开源的碰撞检测库，MoveIt使用它来执行机器人与其环境之间的碰撞检查。通过将八叉树地图传递给FCL，MoveIt能够高效地确定机器人在其工作空间中的哪些位置是安全的，哪些位置可能会与障碍物发生碰撞。这对于实现安全的机器人导航和路径规划至关重要。

2.Planning Scene Monitor使用案例

1. 初始化MoveGroupInterface：首先，你需要创建一个 MoveGroupInterface对象，该对象提供了与机器人规划组的接口。
2. 获取Planning Scene Monitor：通过 MoveGroupInterface的getPlanningSceneMonitor 方法，你可以访问到 PlanningSceneMonitor，它负责管理和更新规划场景。
3. 循环更新和检查：在实际应用中，你通常会在一个循环中持续获取规划场景的快照，并使用它来进行各种操作，如检查碰撞、添加或删除障碍物等。
4. 检查碰撞：在上面的示例中，我们使用 PlanningScene的 isStateColliding方法来检查机器人在当前状态下的碰撞情况。这个方法会考虑机器人的当前状态以及规划场景中的障碍物。
5. 实时性：Planning Scene Monitor会持续监听ROS话题，以获取有关机器人状态和环境变化的最新信息，并相应地更新规划场景。这意味着你的代码可以实时地获取到最新的规划场景数据。

#include <moveit/move_group_interface/move_group_interface.h>  
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>  
#include <moveit/planning_scene/planning_scene.h>  
  
int main() {  
    // 假设已经正确初始化了ROS节点  
  
    // 创建MoveGroupInterface对象  
    moveit::planning_interface::MoveGroupInterface move_group("manipulator");  
  
    // 获取Planning Scene Monitor  
    const moveit::planning_scene_monitor::PlanningSceneMonitor* planning_scene_monitor =  
        move_group.getPlanningSceneMonitor();  
  
    // 循环以持续更新和检查规划场景  
    ros::Rate loop_rate(1.0); // 设置循环频率为1Hz  
    while (ros::ok()) {  
        // 获取当前规划场景的快照  
        const moveit::planning_scene::PlanningSceneConstPtr current_scene =  
            planning_scene_monitor->getCurrentPlanningScene();  
  
        // 使用规划场景进行操作，例如检查碰撞  
        bool is_in_collision = current_scene->isStateColliding(current_scene->getCurrentRobotState(), "");  
  
        if (is_in_collision) {  
            ROS_INFO("Robot is in collision!");  
        } else {  
            ROS_INFO("Robot is not in collision.");  
        }  
  
        // 可以在这里添加其他逻辑，如更新障碍物位置、修改机器人状态等  
  
        // 等待一段时间，然后继续循环  
        loop_rate.sleep();  
    }  
  
    return 0;  
}

3.总结

        规划场景监视器是MoveIt中实现复杂运动规划任务的关键组件，它通过维护一个最新的规划场景来支持机器人的安全导航和操作。

---

参考资料

MoveIt2 官方教程