ROS下　Ｍoveit 3D感知功能与Kinect联合使用

１、Ｍoveit 安装

Moveit官网教程

2、Kinect驱动安装

参考网址：
https://blog.csdn.net/sunbibei/article/details/51594824
https://blog.csdn.net/qq_33835307/article/details/81272377

2.1、KinectＬinux驱动安装：libfreenect2

git clone https://github.com/OpenKinect/libfreenect2.git
cd libfreenect2
sudo apt-get install build-essential cmake pkg-config
sudo apt-get install libusb-1.0-0-dev
sudo apt-get install libturbojpeg libjpeg-turbo8-dev
sudo apt-get install libglfw3-dev
sudo apt-get install libopenni2-dev

cd ..
mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX=$HOME/freenect2
make
make install
#针对上面cmake命令的说明， 第一个参数， 指定安装的位置; 第二个参数,增加对C++11的支持。

• 设定udev rules: sudo cp ../platform/linux/udev/90-kinect2.rules /etc/udev/rules.d/, 然后重新插拔Kinect2.
• 运行build/bin 路径下运行./Protonect，如下效果：
  Screenshot from 2019-09-20 15-02-46.png

2.2、Kinect ROS驱动安装：iai-kinect2

cd ~/rosLib_ws/src/
git clone https://github.com/code-iai/iai_kinect2.git
cd iai_kinect2
rosdep install -r --from-paths .
cd ~/rosLib_ws
catkin_make -DCMAKE_BUILD_TYPE="Release"
#Note: rosdep will output errors on not being able to locate [kinect2_bridge] and [depth_registration].

运行Kinect ROS驱动：
roslaunch kinect2_bridge kinect2_bridge.launch
rostopic list:查看kinect相机对应的ｒｏｓ话题。

附：

1. kinect2_calibration：标定kinect彩色图和深度图程序。 Further details
2. 添加标定生成文件到 kinect2_bridge/data/<serialnumber> 文件夹。Further details
3. 重新启动kinect2_bridge ，可通过rosrun kinect2_viewer kinect2_viewer kinect2 sd cloud`查看点云图像。

3、Moveit 3D感知结合Kinect点云数据生成栅格地图

Perception Pipeline Tutorial

3.1 Updater使用插件架构来处理不同类型的输入:

• PointCloud Occupancy Map Updater : 点云（sensor_msgs/PointCloud2）
• Depth Image Occupancy Map Updater : 深度图像（sensor_msgs/Image）

roslaunch moveit_tutorials obstacle_avoidance_demo.launch //官方点云数据包回放功能演示

YAML Configuration file (Point Cloud):
我使用Kinect的点云topic:/kinect2/sd/points

#“sensors_kinect_pointcloud.yaml”:
sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater    #指定要监听点云的主题。
    point_cloud_topic: /camera/depth_registered/points　　# 将topic改成： /kinect2/sd/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  #将发布过滤后的云的主题（主要用于调试）。过滤云是执行自我过滤后的结果云。
#注：＃注释使用请删除

YAML Configuration file (Depth Map):

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  #octomap表示将以小于或等于此值的速率更新
    filtered_cloud_topic: filtered_cloud

3.2 更新YAML文件：

您现在需要使用此传感器信息更新panda_moveit_config目录的“launch”目录中的sensor_manager.launch文件（此文件由Setup Assistant自动生成但为空）。您需要将以下行添加到该文件中，以配置要使用的MoveIt的传感器源集：

< rosparam  command = “load”  file = “$（find panda_moveit_config）/config/sensors_kinect_pointcloud.yaml”  />

如果您使用的是depthmap，请将yaml文件的名称更改为sensors_kinect_depthmap.yaml。请注意，您需要输入上面创建的正确文件的路径。

3.3 Octomap配置

您还需要通过在sensor_manager.launch中添加以下行来配置Octomap：

< 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”  />

MoveIt使用基于八叉树的框架来表示它周围的世界。上面的Octomap参数是此表示的配置参数：

• octomap_frame：指定将存储此表示的坐标系。如果您正在使用移动机器人，则此框架应该是世界上固定的框架。
• octomap_resolution：指定维护此表示的分辨率（以米为单位）。
• max_range：指定要为此节点输入的任何传感器应用的最大范围值。

3.4 运行launch文件：(修改过后的）

先运行Ｋinect Ros驱动：
roslaunch kinect2_bridge kinect2_bridge.launch
在运行Moveit 3D环境感知launch文件:(obstacle_real_kinect.launch详细文件如下)
roslaunch obstacle_real_kinect.launch

<launch>
  <include file="$(find panda_moveit_config)/launch/demo.launch" />
  <!-- broadcast static tf for panda to world -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="to_world" args="0 0.4 -0.6 0 0 0  panda_link0 world" />
  <!-- broadcast static tf for kinect to world -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="kinect_to_world" args="0 0.3 -0.5 0 0 0  kinect2_ir_optical_frame world" />

</launch>

运行后效果如下：

生成的OctoMap
在Ｒviz中添加PointCloud2插件：可以查看点云数据
Topic选择/kinect2/sd/points:可以看到如下图部分, PointCloud2->OctoMap
PointCloud2->OctoMap

生成TF PDF:rosrun tf view_frames

4、挖坑部分，待解决

１.相机坐标系相对于世界坐标系没有标定：ｌａｕｎｃｈ文件随意发送了一个静态ＴＦ.

2.静态TF,之前正常，现在附在world坐标系的Octomap会报xxx_link exits.（已解决）

坐标系位姿重复，讲上述roslaunch obstacle_real_kinect.launch文件中两个静态TF位姿发不一样即可。注：urdf的各连杆不能头尾相连。

Ps:IntelSR300 linux与ros驱动——小强ROS机器人教程