ur5+moveit 3D perception功能应用

moveit中自带3D perception(3D感知)功能，简单来说，就是支持外部传感器（如kinect, realsense等），能够将其采集的点云或深度数据，转换为octomap（八叉树），并添加到仿真环境中，以进行运动规划和避障。

本文借鉴了moveit官方教程，把教程中的panda机械臂换成了ur5，而且在gazebo中使用kinect录制了点云数据集。

• 操作系统：ubuntu 16.04
• ROS版本：kinetic
• 3D感知方式：pointcloud2点云
• 示例机器人：ur5

moveit官方教程：
https://ros-planning.github.io/moveit_tutorials/doc/perception_pipeline/perception_pipeline_tutorial.html

中文参考：
https://blog.csdn.net/lingchen2348/article/details/82917457
https://blog.csdn.net/coldplayplay/article/details/79105688
https://www.jianshu.com/p/8ce4cf21a244

1 修改moveit配置文件

找到ur5_moveit_config包，通常路径为：/opt/ros/kinetic/share/ur5_moveit_config，如果没有的话，请先安装moveit和ur5的相关官方包。在ur5_moveit_config/config路径下，添加传感器相关配置文件。moveit接受两种数据：

• pointcloud2（点云）
• sensor_msgs/Image（深度图）
  因此需要两个配置文件。

ur5_moveit_config/config/sensors_kinect_pointcloud.yaml：

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater    #指定要监听点云的主题
    point_cloud_topic: /camera/depth/points  #此处为发布点云的话题名称
    max_range: 5.0   #(以m为单位）不会使用比此更远的点
    point_subsample: 1  #选择每个point_subsample点中的一个
    padding_offset: 0.1 #填充的大小（以cm为单位）
    padding_scale: 1.0   #填充的比例。
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud    #将发布过滤后的云的主题（主要用于调试）。过滤云是执行自我过滤后的结果云

ur5_moveit_config/config/sensors_kinect_depthmap.yaml：

sensors:
  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
    image_topic: /camera/depth_registered/image_raw
    queue_size: 5
    near_clipping_plane_distance: 0.3
    far_clipping_plane_distance: 5.0
    shadow_threshold: 0.2
    padding_scale: 4.0
    padding_offset: 0.03
    max_update_rate