１、Ｍoveit 安装
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相机对应的ｒｏｓ话题。
附：
-
kinect2_calibration：标定kinect彩色图和深度图程序。 Further details - 添加标定生成文件到
kinect2_bridge/data/<serialnumber>文件夹。Further details - 重新启动
kinect2_bridge，可通过rosrun kinect2_viewer kinect2_viewer kinect2 sd cloud`查看点云图像。
3、Moveit 3D感知结合Kinect点云数据生成栅格地图
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>
运行后效果如下：
在Ｒviz中添加
PointCloud2插件：可以查看点云数据Topic选择
/kinect2/sd/points:可以看到如下图部分, PointCloud2->OctoMap
生成TF PDF:rosrun tf view_frames
4、挖坑部分，待解决
１.相机坐标系相对于世界坐标系没有标定：ｌａｕｎｃｈ文件随意发送了一个静态ＴＦ.
2.静态TF,之前正常，现在附在world坐标系的Octomap会报xxx_link exits.（已解决）
坐标系位姿重复，讲上述
roslaunch obstacle_real_kinect.launch文件中两个静态TF位姿发不一样即可。注：urdf的各连杆不能头尾相连。
