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: 1.0
    filtered_cloud_topic: filtered_cloud

2 修改ur5_moveit_config中的launch文件

在/opt/ros/kinetic/share/ur5_moveit_config/launch中，找到sensor_manager.launch.xml文件，修改为如下：

<launch>

    

  
  
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="5.0" />

  
  <arg name="moveit_sensor_manager" default="ur5" />
  <include file="$(find ur5_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
  
launch>

找到ur5_moveit_sensor_manager.launch.xml文件，修改为如下：

<launch>
    
    <param name="octomap_frame" type="string" value="odom_combined" />            
    
    <param name="octomap_resolution" type="double" value="0.05" />                        
    <param name="max_range" type="double" value="5.0" />
    
    <rosparam command="load" file="$(find ur5_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
launch>

3 录制包含点云信息的ros包

这里我用了turtlebot教程中自带的一个launch文件，用来启动gazebo和带有kinect的turtlebot：
$ roslaunch turtlebot_gazebo turtlebot_world.launch

然后录制数据集：

$ mkdir bagfiles
$ cd bagfiles
$ rosbag record -o subset /camera/depth/points

4 启动moveit + ur5 并播放rosbag

写了2个launch文件。都放在了~/study_ws/src/robot_marm/marm_planning/launch路径下。

ur5_octomap_demo.launch：

<launch>
  <arg name="bag_path" default="/home/tianbot/bagfiles/subset.bag" />
  
  <param name="use_sim_time" value="true" />

  
  <include file="$(find marm_planning)/launch/start_moveit_gazebo_rviz_ur5.launch" />
  
  
  <node pkg="rosbag" type="play" name="bag_play" args="$(arg bag_path)"/>

  
  <node pkg="tf" type="static_transform_publisher" name="to_world" args="0 0 0 0 0 0 base_link camera_depth_optical_frame 10" />
launch>

start_moveit_gazebo_rviz_ur5.launch:

<launch>
  <arg name="sim" default="true" doc="use sim time in moveit"/>
  <arg name="config" default="true" doc="use config in rviz"/>

  
  <include file="$(find ur_gazebo)/launch/ur5.launch"/>

  
  <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
    <arg name="sim" default="$(arg sim)"/>
  include>

  
  <include file="$(find ur5_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" default="$(arg config)"/>
  include>

launch>

5 测试

只需要启动一个launch即可：

roscore
roslaunch marm_planning ur5_octomap_demo.launch

在rviz中订阅/move_group/filtered_cloud话题，即可看到环境中的octomap：