深度传感器消息（pointcloud2）导入moveit!机械臂环境当中

1、深度数据发布

传感器lovix-mid70通过SLAM算法建好障碍物点云map，并发布数据
这里因为没有传感器，搭建仿真模型有点麻烦，直接使用别人给的pcd文件作为传感器发布的消息

#include<ros/ros.h>
#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<sensor_msgs/PointCloud2.h>
#include<pcl/io/pcd_io.h>//which contains the required definitions to load and store point clouds to PCD and other file formats.
 
main (int argc, char **argv)
{
  ros::init (argc, argv, "UandBdetect");
  ros::NodeHandle nh;
  ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
  pcl::PointCloud<pcl::PointXYZ> cloud;
  sensor_msgs::PointCloud2 output;
  pcl::io::loadPCDFile ("/home/xieguanhao/map/map2_all_points.pcd", cloud);   
// 修改自己pcd文件所在路径
  //Convert the cloud to ROS message
  pcl::toROSMsg(cloud, output);
  output.header.frame_id = "odom";
  
  //this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer    
    //！！！这一步需要注意，是后面rviz的 fixed_frame 重点！！
  ros::Rate loop_rate(1);
  while (ros::ok())
  {
    pcl_pub.publish(output);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

订阅该消息后输出如图所示

b、验证点云数据转为octomap数据的情况
利用octomap-server服务器输入点云数据输出octomap-msgs
安装octomap如下所示：

sudo apt-get install ros-kinetic-octomap ros-kinetic-octomap-mapping ros-kinetic-octomap-msgs ros-kinetic-octomap-ros ros-kinetic-octomap-rviz-plugins ros-kinetic-octomap-server

编写roslauch文件启动服务器

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.05" />

    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <param name="frame_id" type="string" value="base" />

    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="100.0" />
    <param name="latch" value="true" />

    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="1000" />
    <param name="pointcloud_min_z" value="0" />

    <!-- topic from where pointcloud2 messages are subscribed -->
    <remap from="/cloud_in" to="/pcl_output" />
 
  </node>
</launch>

输出如下：

验证完毕

2、把pointcloud2数据加入到moveit!环境当中

由于moveit！自带octomap相关包，我们输入点云数据后自动转为octomap模型。
首先，最好在universal_robot里创建新的功能包sensor_moveit_config包

catkin_create_pkg sensor_moveit_config roscpp rospy std_msgs moveit_ros_move_group moveit_ros_visualization

然后，进入该功能包新建config和launch文件，并在前者新建sensor_rgbd.yaml文件，后者新建livox_moveit_sensor_manager.launch.xml和sensor_manager.launch.xml（这里的livox因为本人使用的是这个系列的激光雷达），代码如下所示：

cd sensor_moveit_config
mkdir config
mkdir launch
cd config
touch sensor_rgbd.yaml
cd ..
cd launch
touch livox_moveit_sensor_manager.launch.xml
touch sensor_manager.launch.xml

其中，sensor_moveit_config文件内容如下所示:

# /universal_robot/sensor_moveit_config/config/sensor_rgbd.yaml
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: /pcl_output
  max_range: 5.0
  frame_subsample: 1
  point_subsample: 1
  padding_offset: 0.1
  padding_scale: 1.0
  max_update_rate: 1.0
  filtered_cloud_topic: output_cloud

livox_moveit_sensor_manager.launch.xml文件内容如下：

<launch>
   <rosparam command = "load" file="$(find sensor_moveit_config)/config/sensor_rgbd.yaml" />
</launch>

sensor_manager.launch.xml代码如下所示：

<launch>

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

  <!-- Params for the octomap monitor -->
  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
  <param name="base" type="string" value="odom_combined" /> 
  <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="livox" />
  <include file="$(find sensor_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

回到catkin_ws下catkin_make一下
最后，在ur{what you want to use}_moveit_config/launch/move_group.launch文件环境感知部分增加如下代码：

<include ns="move_group" file="$(find sensor_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
   <arg name="moveit_sensor_manager" value="livox" />
</include>

以上便完成了将点云转换为八叉树输入到moveit的过程，其中move_group节点通过订阅/universal_robot/sensor_moveit_config/config/sensor_rgbd.yaml文件中的point_cloud_topic话题获取点云信息，该点云信息的消息结构为sensor_msgs/PointCloud2。

最后按如下命令启动机械臂和点云数据节点

rosrun load_pcd load_pcd
roslaunch ur5_moveit_config demo.launch
 

运行结果如下图所示：