moveit的3D Perception功能（环境感知）

ROS的moveit中，可以使用深度相机感知环境数据，生成一种被称为octomap的方格地图，感知后的环境可以作为环境障碍物，机械臂规划路径时会主动避开有octomap地图的地方。其中用到了moveit的组件 Occupancy Map Updater，Occupancy Map Updater 接受两种数据
1、pointcloud2点云
2、深度图 sensor_msgs/Image

下面以一个官方示例例程来展示moveit的3d 感知功能。
操作系统：ubuntu16.04
ROS版本：kinetic
3D感知方式：pointcloud2点云
示例机器人：Panda

1、下载并配置示例工程
完整过程：
https://ros-planning.github.io/moveit_tutorials/doc/getting_started/getting_started.html
简化过程如下：
首先，建立一个新空间

mkdir -p ~/ws_moveit/src
cd ..
catkin_make

之后，下载官方示例程序到src目录

cd ~/ws_moveit/src
git clone https://github.com/ros-planning/moveit_tutorials.git
git clone https://github.com/ros-planning/panda_moveit_config.git

如果担心却相关组件，可以执行下述命令

cd ~/ws_moveit/src
rosdep install -y --from-paths . --ignore-src --rosdistro kinetic

2、修改moveit配置文件
完整过程：
https://ros-planning.github.io/moveit_tutorials/doc/perception_pipeline/perception_pipeline_tutorial.html
简要过程如下：
(1)编写一个关于传感器的yaml配置文件：sensors_kinect_pointcloud.yaml，放在panda_moveit_config/config目录下

sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
point_cloud_topic: /camera/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

(2)填写一份launch文件。
在panda_moveit_config/launch目录下，有两个与sensor相关的launch文件
一个是sensor_manager.launch.xml,该文件是moveit生成配置文件后自动生成好的。
另一个是XXX_moveit_sensor_manager.launch.xml文件，这也是自动生成好的，只不过是个空的launch文件，需要填写以下内容。

                //这个参数是octomap所在坐标系，可以自己指定
                            //octomap的分辨率，越小分辨率越高，越消耗系统资源。
    
     //将之前的yaml文件里的参数传到ros参数服务器

到此，就可以启动示例程序观看效果了。

roslaunch moveit_tutorials obstacle_avoidance_demo.launch //如果source空间的命令没加入.bashrc文件，记得source工作空间

显示效果如下：

该示例程序的运行内容：
（1）回放了一个深度相机的数据包。
由于并没有真实或仿真环境下的深度相机传感器，所以数据来源是教程里提前录制好的。你可以通过添加pointcloud2视图来查看发布的点云数据。

（2）启动了panda的moveit配置程序，这里并没有用到gazebo等仿真环境。

可能遇到的问题：

问题1：octomap地图并没有立即显示出来。
由于octomap的生成比较耗费系统资源，不同电脑加载可能需要的时间不一样。
–>如果你启动了上述示例但是在3~5s内都没看见octomap地图，在稍等若干秒，就应该能看到。
–>若还是没看到，请查看命令行程序是否有红色报错提示，若看到下面的提示，就是有问题了，没发现太好的解决办法，重新启动示例程序多试几次。

–>若系统显示实在是太吃力，请将上文octomap_resolution参数修改的大一些，让地图显示的粗略一些。

正常的话启动后应该显示如下：

问题2：找不到depth_image_octomap_updater插件所需要的库。

[ERROR] [1528466140.174800759]: Exception while loading octomap updater 'occupancy_map_monitor/DepthImageOctomapUpdater':
Could not find library corresponding to plugin occupancy_map_monitor/DepthImageOctomapUpdater.
Make sure the plugin description XML file has the correct name of the library and that the library actually exists.

跑上述教程其实不会遇到这个问题，因为我们是通过点云来生成octomap的，但文章开头也说了，moveit还可以通过深度图来显示octomap，如果你正在尝试使用深度图来生成octomap地图，那么，上述报错是有可能产生的。

问题原因：
moveit的某一版本出了点小小状况，下载后确实不包含depth_image_octomap_updater相关动态链接库，在github最新的moveit版本中，已经不存在这个问题，下载新版本重新编译即可。
网上对此问题的讨论：https://github.com/ros-planning/moveit/issues/943

可以看出，这一版本的moveit更改导致少了4个与depth_image_octomap_updater相关的库。
解决办法：
下载moveit的最新源码，编译成功后，可以查看所有编译生成的文件，搜索depth_image_octomap_updater开头的所有.so文件，应该有4个，全部复制到/opt/ros/kinetic/lib目录下。