moveit perception

介绍

 moveit包含感知（perception）模块，其可以将深度相机采集的点云或深度图转换成OctoMap（八叉树地图），用rviz可以显示Octomap，并且可以采用Octomap进行避障轨迹规划。感知周围环境对于机械臂来说非常重要，具有广泛的应用场景。本文简要介绍moveit中的perception模块。

实现步骤

本文的实现步骤参考moveit官方教程Perception Pipeline Tutorial — moveit_tutorials Noetic documentation

ros版本：melodic

1、首先需要在机械臂腕部安装深度相机或RGB_D相机，如果没有相机实物，我们可以利用gazebo仿真一个相机，本文就是利用gazebo仿真了一个kinect_v2相机，如何利用gazebo仿真相机不在本文的讨论范围内。齐次还需要有机械臂的moveit配置文件，采用moveit setup asststent对机械臂进行配置可参考MoveIt Setup Assistant — moveit_tutorials Noetic documentation

2、准备工作做好之后，打开moveit配置文件夹中config/sensors_3d.yaml文件，该文件默认是空的，需要填入如下的信息

# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /kinect_V2/depth_registered/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

其中

• sensor_plugin：加载用于更新Octomap的插件，如果输入是点云，则插件是occupancy_map_monitor/PointCloudOctomapUpdater， 如果输入是深度图，插件是

occupancy_map_monitor/DepthImageOctomapUpdater

• point_cloud_topic: 相机发布的点云话题
• max_range: 生成的Octomap距离原点的最大范围
• point_subsample: 如果是1，则不进行下采样，如果设为2，则删除掉一般的点
• max_update_rate: Octomap更新速率，Octomap不能更新太快，否则会导致系统卡顿
• filtered_cloud_topic: 上述加载的插件会对输入的点云进行过滤，其实就是删除位于机器人内部的点，保留剩余的点

3、接下来打开moveit配置文件夹中launch/sensor_manager.launch.xml文件，添加如下信息

<rosparam command="load" file="$(find arm_moveit_config)/config/sensors_3d.yaml" />

该条语句就是加载刚才配置的sensors_3d.yaml文件。

sensor_manager.launch.xml文件完成信息如下

<launch>

  <!-- This file makes it easy to include the settings for sensor managers -->

  <!-- Params for 3D sensors config -->
  <rosparam command="load" file="$(find arm_moveit_config)/config/sensors_3d.yaml" />

  <!-- Params for the octomap monitor -->
  <param name="octomap_frame" type="string" value="world" />
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="arm" />
  <include file="$(find arm_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />

</launch>

其中

• octomap_frame: Octomap的坐标系
• octomap_resolution: Octomap最小方块的尺寸

4、启动机械臂和相机，因为本文是利用gazebo仿真的机械臂和相机，所以这里运行gazebo，如下如所示

然后运行moveit配置文件夹中launch/demo.launch，rviz会打开并显示机械臂和Octomap，如下如所示

可以看出Octomap大致可以显示出环境，但是存在一些误差，比如地面显示了两层，桌子上方也出现一些Octomap。

分析

 下面简要分析一下moveit中的感知模块的执行流程

1. 运行moveit配置文件中launch/demo.launch，会运行move_group节点，move_group会创建PlanningSceneMonitor类对象，并执行函数startWorldGeometryMonitor。该函数原型如下所示

   void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
                                                        const std::string& planning_scene_world_topic,
                                                        const bool load_octomap_monitor)

   函数参数load_octomap_monitor决定是否运行Octomap。如果为true，则在该函数内部会启动Octomap。

2. 所谓启动Octomap就是运行类OccupancyMapMonitor的startMonitor函数，startMonitor会执行类OccupancyMapUpdater的start函数。

3. OccupancyMapMonitor类会从参数服务器中读取我们配置好的sensors_3d.yaml文件中的参数。

4. OccupancyMapUpdater类是我们设置的插件的基类，会订阅我们在sensors_3d.yaml文件中设置的点云话题，并对点云信息进行处理。

5. 拿到点云信息后，会判断每个点云是位于机器人内部还是外部，然后过滤掉位于机器人内部的点，得到过滤后的点云并发布，然后根据点云更新Octomap，Octomap保存着被点云占据的空间以及没有被点云占据的空间。