moveIt!机械手真实世界运动规划避障实战记录1— —Kinect V2驱动安装

之前留下的坑，如果要使用Kinect V2，必须要安装OpenCV2.4.9版本（ 经测试，可不安装）。只能重新装一遍OpenCV，还好CUDA没有受影响。

1 ROS下打开Kinect V2

1.1 安装iai_kinect2

安装之前需要安装依赖NASM和依赖libjpeg-turbo
根据官方文档和教程来。

cd ~/ws_moveit/src/
git clone https://github.com/code-iai/iai_kinect2.git
cd iai_kinect2
rosdep install --from-paths ~/ws_moveit/src/iai_kinect2 --ignore-src -r
cd ~/ws_moveit
catkin_make -DCMAKE_BUILD_TYPE="Release"  
rospack profile
roslaunch kinect2_bridge kinect2_bridge.launch
rosrun kinect2_viewer kinect2_viewer sd cloud
rosrun image_view image_view image:=/kinect2/hd/image_color
rosrun rqt_image_view rqt_image_view

image_view一直无法显示图像，只能从这篇参考中使用rqt来查看Kinect2的图像了。
报错，

/usr/bin/ld: CMakeFiles/kinect2_bridge.dir/src/kinect2_bridge.cpp.o: undefined reference to symbol '_ZNSo9_M_insertIdEERSoT_@@GLIBCXX_3.4.9'
/usr/lib/x86_64-linux-gnu/libstdc++.so.6: error adding symbols: DSO missing from command line

按照建议
报错，按照OpenCV必须使用2.4.9的版本来改。
有两处进行注释，并改成：

#if(NOT "include;/opt/ros/kinetic/include/opencv-3.3.1;/opt/ros/kinetic/include/opencv-3.3.1/opencv" STREQUAL " ")
#set(cv_bridge_INCLUDE_DIRS " ")
#set(_include_dirs "include;/opt/ros/kinetic/include/opencv-3.3.1;/opt/ros/kinetic/include/opencv-3.3.1/opencv") 
if(NOT "include;/usr/local/include;/usr/local/include/opencv;" STREQUAL " ")
  set(cv_bridge_INCLUDE_DIRS "")

#set(libraries “cv_bridge;/opt/ros/kinetic/lib/libopencv_core3.so.3.3.1;/opt/ros/kinetic/lib/libopencv_imgproc3.so.3.3.1;/opt/ros/kinetic/lib/libopencv_imgcodecs3.so.3.3.1”)
set(libraries "cv_bridge;/usr/local/lib/libopencv_core.so.2.4.9;/usr/local/lib/libopencv_calib3d.so.2.4.9;/usr/local/lib/libopencv_highgui.so.2.4.9;/usr/local/lib/libopencv_imgproc.so.2.4.9")

可通过以下命令来查看配置的OpenCV的情况：

pkg-config --modversion opencv 
pkg-config --cflags opencv 
pkg-config --libs opencv 

将iai_kinect2里面的CmakeLists，在第一行增加以下语句：

set(CMAKE_PREFIX_PATH “/usr/local/share/OpenCV”)

报以下错误：

CMake Error at /opt/ros/kinetic/share/cv_bridge/cmake/cv_bridgeConfig.cmake:115 (message):
  Project 'cv_bridge' specifies '/opt/ros/kinetic/include/opencv-3.3.1' as an
  include dir, which is not found.  It does neither exist as an absolute
  directory nor in '/opt/ros/kinetic//opt/ros/kinetic/include/opencv-3.3.1'.
  Check the issue tracker
  'https://github.com/ros-perception/vision_opencv/issues' and consider
  creating a ticket if the problem has not been reported yet.
Call Stack (most recent call first):
  /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:76 (find_package)
  iai_kinect2/kinect2_bridge/CMakeLists.txt:25 (find_package)

将/opt/ros/kinetic/share/cv_bridge/cmake下的cv_bridgeConfig.cmake里面的命令改一下：

if(NOT "include;/opt/ros/kinetic/include/opencv-3.3.1;/opt/ros/kinetic/include/opencv-3.3.1/opencv" STREQUAL " ")
set(cv_bridge_INCLUDE_DIRS " ")
set(_include_dirs "include;/opt/ros/kinetic/include/opencv-3.3.1;/opt/ros/kinetic/include/opencv-3.3.1/opencv") 

改成：

if(NOT "include;/opt/ros/kinetic/include/opencv-3.3.1-dev;/opt/ros/kinetic/include/opencv-3.3.1-dev/opencv" STREQUAL " ")
set(cv_bridge_INCLUDE_DIRS " ")
set(_include_dirs "include;/opt/ros/kinetic/include/opencv-3.3.1-dev;/opt/ros/kinetic/include/opencv-3.3.1-dev/opencv") 

1.1 安装pcl(点云库，经测试可不安装)

先安装依赖，由于安装pcl1.7.2需要安装libboost1.55，因此需要很多依赖文件才能安装libboost1.58
解压后直接使用命令安装：

sudo ./bootstrap.sh

完了记得添加一下环境变量。
然后再通过pcl安装教程安装1.7.2版本pcl

至此完成了安装，安装CUDA7.5是个大坑，因为CUDA7.5的gcc版本不能超过4.8，但是目前ROS提供支持的版本Kinetic采用gcc5以上的版本编译，这使得在编译联结Kinect V2与ROS的iai_kinect包时出现了错误，如果使用gcc4.8编译，则会出现ros中的函数未定义的引用的错误;如果使用gcc5.4.0编译，则会出现未定义CUDA中的函数的错误。

1.3 使用MoveIt!进行panda机械手结合Kinect的运动规划

这里需要自己写一段从Kinect V2将点云图转换成深度图的函数。
参考链接：https://blog.csdn.net/BetterEthan/article/details/81660049通过直接发布Piontcloud2的信息，利用MoveIt!直接获取Piontcloud2节点的信息来读取Kinect V2的点云数据。

roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true 
rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link kinect2_link 100  //设定摄像头的位姿

根据文章moveit的3D Perception功能（环境感知）

接上Kinect V2后，输入以下命令：

roslaunch kinect2_bridge kinect2_bridge.launch reg_method:=opencl depth_method:=opencl publish_tf:=true
roslaunch moveit_tutorials obstacle_avoidance_demo.launch

设置Global Options中的Fixed Frame为kinect2_link，添加Pointcloud2，topic为/kinect2/sd/points，添加MotionPlanning，进行运动规划。
一看，坐标有误，三维点云都显示在上方了。

出现以上的结果，标定（为了保证准确度，除了相机内参标定外，还需要进行手眼标定）。

1.3.1 kinect2内参标定

自己用的标定板是chess9x6x0.021的所以命令如下：

rosrun kinect2_bridge kinect2_bridge _fps_limit:=2

rosrun kinect2_calibration kinect2_calibration chess9x6x0.021 record color
rosrun kinect2_calibration kinect2_calibration chess9x6x0.021 calibrate color

rosrun kinect2_calibration kinect2_calibration chess9x6x0.021 record ir
rosrun kinect2_calibration kinect2_calibration chess9x6x0.021 calibrate ir

rosrun kinect2_calibration kinect2_calibration chess9x6x0.021 record sync
rosrun kinect2_calibration kinect2_calibration chess9x6x0.021 calibrate sync

rosrun kinect2_calibration kinect2_calibration chess9x6x0.021 calibrate depth
roscd kinect2_bridge/data; mkdir 017240764847

用另一种方法标定Kinect_Tutorial：

rosrun camera_calibration cameracalibrator.py --size 6x9 --square 0.0105 image:=/kinect2/hd/image_color

rosrun camera_calibration cameracalibrator.py --size 6x9 --square 0.0105 image:=/kinect2/hd/image_

1.3.2 手眼标定

上面做了内参标定是为了提高手眼标定的精度，
而手眼标定是为了确定相机相对于机械臂基坐标系的变换矩阵，通过发布该变换矩阵的tf参数，
才能够把相机的画面正确校正到以机械臂为基础的场景上。
但是自己标定的结果总是不太准确，有多种方法可用于手眼标定，在另一篇文章再另行讨论吧。

关于标定的解释让机械臂自动进行手眼标定—以Dobot Magician和Realsense为例（附示例代码）

利用pcl的库将PCD文件转化为点云文件给rviz的方式利用三维建模的方法：ros下使用Kinect传感器对移动机器人环境进行三维建模（rgbdslam方法），并使用rviz打开：

rosrun pcl_ros pcd_to_pointcloud 0000_cloud.pcd

其他参考链接：
1 ubuntu16.04安装kinect2的驱动及其在ROS中使用
2 iai_Kinect2
3 完整安装kinect2驱动libfreenect2和ros包iai_kinect2
4 ROS下Kinect2的驱动安装及简单应用
5 关于相机标定讲得比较清楚的文章：相机参数标定（camera calibration）及标定结果如何使用