ROS下Kinect2的驱动安装及简单应用
目录
- 
目录
 - 
Kinect2
 - 
ROS
 - 
驱动节点配置
- libfreenect2
 - iai-kinect2
 
 - 
简单运用
- 保存图片
 - 保存图片序列
 - 点击点云图获取坐标
 
 
Kinect2
对于这个话题感兴趣的同学们来说呢 对于这个话题也比较熟悉吧 这个设备价格也不算高 现在某东上大概两千元左右就能买到一个 还是可以搭配一个三脚架使用 值得推荐的是 无论是成像效果还是骨骼捕捉质量都较一代有明显提升 如需更多详细信息 请参考Google或其他官方渠道查阅
ROS
对机器人技术领域工作及技术学习者而言,
机器人操作系统(ROS)因其广泛应用而备受关注。
官方提供了丰富的开源工具包,
这些包均可方便使用。
同时,
主流传感器设备均支持ROS平台。
值得注意的是Kinect II同样支持。
此外,
基于Ubuntu的操作系统运行着ROS系统,
我所使用的版本是Ubuntu 14.04 + ROS Indigo版本。
关于安装的具体指导,
建议参考 ROS官方文档 中的详细说明。
按照以下步骤依次执行命令:
首先启动rosindigo节点服务;
接着配置ros参数;
最后运行rosbag初始化节点以创建ros bag文件。
    sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
    
    sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net --recv-key 0xB01FA116
    
    sudo apt-get update
    
    sudo apt-get install ros-indigo-desktop-full
    
    sudo rosdep init
    
    rosdep update
    
    echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
    
    source ~/.bashrc
    
    sudo apt-get install python-rosinstall
        当上述指令已全部执行完毕时,
ROS也随之完成了安装过程。
随后还需建立自己的工作空间。
执行下述代码:
    mkdir -p ~/catkin_ws/src
    cd ~/catkin_ws/src
    catkin_init_workspace
    cd ..
    catkin_make
        驱动节点配置
基于ROS平台的环境下采用Kinect2技术,并主要依赖iai-kinect2这个软件包进行开发工作。Github官方仓库地址为:https://github.com/code-iai/iai_kinect2
第一步,请确保您已经配置好了环境变量,并下载相应的软件包:$libfreenect2 (可通过GitHub仓库获取)。
具体操作步骤如下(其中,默认情况下,默认情况下使用的是Ubuntu 14.04或更高版本,默认情况下)。
对于其他系统,请参考原始网站说明。
libfreenect2
- 下载 libfreenect2 源码
 
    git clone https://github.com/OpenKinect/libfreenect2.git
    cd libfreenect2
        - 下载upgrade deb 文件
 
    cd depends; ./download_debs_trusty.sh
        - 安装编译工具
 
    sudo apt-get install build-essential cmake pkg-config
        - 安装 libusb. 版本需求 >= 1.0.20.
 
    sudo dpkg -i debs/libusb*deb
        - 安装 TurboJPEG
 
    sudo apt-get install libturbojpeg libjpeg-turbo8-dev
        - 安装 OpenGL
 
    sudo dpkg -i debs/libglfw3*deb
    sudo apt-get install -f
    sudo apt-get install libgl1-mesa-dri-lts-vivid
        (If the last command conflicts with other packages, don't execute it.) As per the instruction, during installation, the third command indeed encountered an issue, so we proceeded to skip it.
- 
安装 OpenCL (可选)
- Intel GPU:
 
 
    sudo apt-add-repository ppa:floe/beignet
    sudo apt-get update
    sudo apt-get install beignet-dev
    sudo dpkg -i debs/ocl-icd*deb
        - 
AMD GPU:
apt-get install opencl-headers- 验证安装: 
clinfo - 安装 CUDA (可选, Nvidia only):
 
 - 验证安装: 
 - 
(Ubuntu 14.04仅限)从Nvidia官方网站下载.deb包'cuda-repo-ubuntu1404...*.deb'(其中'deb (network)'为选项),然后按照其提供的安装指南进行操作包括运行命令'apt-get install cuda'以安装NVIDIA显卡驱动程序。
 - 
(Jetson TK1)该功能已预置于系统中。
 - 
(Nvidia/Intel双GPU)在完成'apt-get install cuda'后,请使用命令'sudo prime-select intel'以选择Intel显卡用于桌面环境配置。
 - 
(其他情况)请访问NVIDIA官方网站并按照其指引进行操作。
 - 
安装VAAPI(仅适用于Intel处理器)
sudo dpkg -i debs/{libva,i965}*deb; sudo apt-get install- 安装 OpenNI2 (可选)
 
 
    sudo apt-add-repository ppa:deb-rob/ros-trusty && sudo apt-get update
    sudo apt-get install libopenni2-dev
        - Build
 
    cd ..
    mkdir build && cd build
    cmake .. -DCMAKE_INSTALL_PREFIX=$HOME/freenect2 -DENABLE_CXX11=ON
    make
    make install
        用于上述cmake命令的说明中,第一个参数可选指定安装路径的位置,默认为示例路径或/usr/local。第二个参数则用来增加C++11的支持。
- 配置udev规则:通过
sudo cp命令将路径下的kinect2.rules文件复制到etc/udev/rules.d目录下,并完成相关设置以允许重新插拔Kinect2设备。- 已完成所有设置,请启动
./bin/Protonect程序进行测试。- 在正常情况下(不出意外),您应该能够看到以下效果:
- 项目界面的显示
 - 相关功能模块的基本交互
 - 数据同步与同步日志记录
 
 
 - 在正常情况下(不出意外),您应该能够看到以下效果:
 
 - 已完成所有设置,请启动
 

iai-kinect2
利用命令行从Github上面下载工程源码到工作空间内src文件夹内:
    cd ~/catkin_ws/src/
    git clone https://github.com/code-iai/iai_kinect2.git
    cd iai_kinect2
    rosdep install -r --from-paths .
    cd ~/catkin_ws
    catkin_make -DCMAKE_BUILD_TYPE="Release"
        关于上述命令中的最后一条指令,请特别说明以下几点:如果前面所述的libfreenect2安装位置不符合标准两个路径的要求,则请明确指定libfreenect2的实际运行路径:
    catkin_make -Dfreenect2_DIR=path_to_freenect2/lib/cmake/freenect2 -DCMAKE_BUILD_TYPE="Release"
        编译结束, 一切OK的话, 会看到如下提示:

在ROS框架下实现Kinect2数据捕获即将成为可能。PS:大多数同学在执行子进程时经常会遇到不可行的情况,主要原因在于缺少对应的源目录配置。建议首先运行 source ~/catkin_ws/devel/setup.bash, 若已经将相关配置完成于 ~/.bashrc文件中,则无需过多担心。
    roslaunch kinect2_bridge kinect2_bridge.launch
        
通过roslaunch执行kinect2相关节点的运行,在另一个终端窗口中键入rostopic list即可观察到该节点所发布的主题(Topic)。此外,在另一个终端窗口中键入rosrun kinect2_viewer kinect2_viewer sd cloud能够启动一个Viewer以查看数据。运行结果如图所示

简单运用
很久未关注这篇博客, 文章内容是之前工作中需要使用Kinect2时尝试整理后汇编而成的。
今天一个偶然的同学在我站内发送了一封来信,请我阅读这篇博客内容。分享出去后帮到他人确实让我感到很开心!关于这位同学提出的问题,在我之前的开发工作中也已实现过。准备回顾一下具体情况以便更好地协助他完成后续工作
保存图片
主要目的是实现RGB图像的保存。在之前的描述中,默认输入bashrosrun kinect--viewer kinect--viewer sd cloud即可观察显示效果。这一指令本质上是启动kinect--viewer包中的kinect--viewer节点,并传递两个参数sd和cloud。进入该包目录后,则可直观地查看该节点的具体代码。在源码中核心函数代码如下:
    int main(int argc, char **argv) {
      ... ... // 省略了部分代码
      topicColor = "/" + ns + topicColor;
      topicDepth = "/" + ns + topicDepth;
      OUT_INFO("topic color: " FG_CYAN << topicColor << NO_COLOR);
      OUT_INFO("topic depth: " FG_CYAN << topicDepth << NO_COLOR);
      // Receiver是一个类, 也定义在该文件中.useExact(true), useCompressed(false)
      Receiver receiver(topicColor, topicDepth, useExact, useCompressed);
    
      OUT_INFO("starting receiver...");
      // 运行时给出参数"cloud", 则mode = Receiver::CLOUD
      // 运行时给出参数"image", 则mode = Receiver::IMAGE
      // 运行时给出参数"both", 则mode = Receiver::BOTH
      receiver.run(mode);
    
      ros::shutdown();
      return 0;
    }
        Receiver类的功能模块也不算太复杂。其中用于显示的主要循环位于imageViewer()或cloudViewer()中;根据提供的不同参数设置, 该系统会调用不同的功能模块;其对应的映射关系及详细内容请参考下文。
    switch(mode) {
    case CLOUD:
      cloudViewer();
      break;
    case IMAGE:
      imageViewer();
      break;
    case BOTH:
      imageViewerThread = std::thread(&Receiver::imageViewer, this);
      cloudViewer();
      break;
    }
        BOTH选项设置将导致以展示图像的形式出现两个窗口. 上述两个函数均已实现了对图像的保存功能.代码摘录如下所示, 两者的功能相似, 因此仅对imageViewer()进行了展示
    int key = cv::waitKey(1);
    switch(key & 0xFF) {
    case 27:
    case 'q':
      running = false;
      break;
    case ' ':
    case 's':
      if(mode == IMAGE) {
    createCloud(depth, color, cloud);
    saveCloudAndImages(cloud, color, depth, depthDisp);
      } else {
    save = true;
      }
      break;
    }
        其中关键函数saveCloudAndImages()的实现如下:
    void saveCloudAndImages(
    const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud,
    const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depthColored) {
    
      oss.str("");
      oss << "./" << std::setfill('0') << std::setw(4) << frame;
      // 所有文件都保存在当前路径下
      const std::string baseName = oss.str();
      const std::string cloudName = baseName + "_cloud.pcd";
      const std::string colorName = baseName + "_color.jpg";
      const std::string depthName = baseName + "_depth.png";
      const std::string depthColoredName = baseName + "_depth_colored.png";
    
      OUT_INFO("saving cloud: " << cloudName);
      // writer是该类的pcl::PCDWriter类型的成员变量
      writer.writeBinary(cloudName, *cloud);
      OUT_INFO("saving color: " << colorName);
      cv::imwrite(colorName, color, params);
      OUT_INFO("saving depth: " << depthName);
      cv::imwrite(depthName, depth, params);
      OUT_INFO("saving depth: " << depthColoredName);
      cv::imwrite(depthColoredName, depthColored, params);
      OUT_INFO("saving complete!");
      ++frame;
    }
        通过查看上述代码可以看出, 如果要将图片保存, 请先选择显示的图像窗口, 然后按下键盘'S'键或按下空格键即可完成, 即可将保存的图像放置在当前目录下。
如果有一些特殊需求,在现有源码的基础上进行开发将会效率倍增。举个例子来说,在原有功能的基础上进行扩展将会达到事半功倍的效果。
保存图片序列
为了存储一连串的图片, 只需调节启动与结束. 举个例子, 按下B键(Begin)启动存储功能, 再按下E键(End)完成存储过程.
为了实现这一功能,在现有源码的基础上进行必要的修改即可达到目标。优先考虑的情况下,每次对按键B和E进行判断,并建议将该判断逻辑新增至上述switch语句块中。如果需要连续保存,则只需简单设置一个标志位即可。
首先, 复制vewer.cpp文件并将其命名为save_seq.cpp. 接着, 对save_seq.cpp文件中的Receiver类进行操作, 在其bool save成员变量基础上增加一个新的成员变量bool save_seq. 在该类的构造函数参数列表中新增这一变量的初始化设置为false
- 定位到
void imageViewer()函数, 修改对应的switch(key & 0xFF)块如下: 
    int key = cv::waitKey(1);
    switch(key & 0xFF) {
    case 27:
    case 'q':
      running = false;
      break;
    case 'b': save_seq = true; save = true; break;
    case 'e': save_seq = false; save = false; break;
    case ' ':
    case 's':
      if (save_seq) break;
      if(mode == IMAGE) {
    createCloud(depth, color, cloud);
    saveCloudAndImages(cloud, color, depth, depthDisp);
      } else {
    save = true;
      }
      break;
    }
    if (save_seq) {
      createCloud(depth, color, cloud);
      saveCloudAndImages(cloud, color, depth, depthDisp);
    }
        - 定位到
void cloudViewer()函数, 修改对应的if (save)块如下: 
    if(save || save_seq) {
      save = false;
      cv::Mat depthDisp;
      dispDepth(depth, depthDisp, 12000.0f);
      saveCloudAndImages(cloud, color, depth, depthDisp);
    }
        将焦点移至void keyboardEvent(const pcl::visualization::KeyboardEvent &event, void *)函数, 按照以下方式对源码进行调整
    if(event.keyUp()) {
      switch(event.getKeyCode()) {
      case 27:
      case 'q':
    running = false;
    break;
      case ' ':
      case 's':
    save = true;
    break;
      case 'b':
    save_seq = true;
    break;
      case 'e':
    save_seq = false;
    break;
      }
    }
        在CMakeList.txt的最后, 添加如下指令:
    add_executable(save_seq src/save_seq.cpp)
    target_link_libraries(save_seq
      ${catkin_LIBRARIES}
      ${OpenCV_LIBRARIES}
      ${PCL_LIBRARIES}
      ${kinect2_bridge_LIBRARIES}
    )
        前往\texttt{catkin}根目录, 执行catkin_make, 进行构建操作. 如果编译过程顺利无异常, 即可进入下一步查看效果. 其中首先必须启动\texttt{kinect_bride}服务. 依次运行以下命令:
    roslaunch kinect2_bridge kinect2_bridge.launch
    rosrun kinect2_viewer save_seq hd cloud
        选中弹出的窗口, 按键B 开始, 按键E结束保存. Terminal输出结果如下:

点击点云图获取坐标
核心思路是,在操作过程中,在点云图中动态更新当前点击位置的三维坐标,并在点击触发后捕获这些坐标并发送出去。
这样操作的话, 首先需要对鼠标设置一个回调函数, 当鼠标状态发生变化时, 需要做相应的处理. 我们可以将其分为三种基本的情况:
- 鼠标移动
 - 鼠标左键点击
 - 鼠标右键点击
 
由于采用了回调机制, 虽然只是一个小型功能, 但其代码实现过程较为简单. 在具体实现中, 我们采用了三个全局变量来表示这三个状态(该代码需满足C++11版本的要求; 如果不想过多增加开发复杂度, 建议采用volatile int类型即可), 并同时设计了一个全局性的基础函数:
    std::atomic_int mouseX;
    std::atomic_int mouseY;
    std::atomic_int mouseBtnType;
    
    void onMouse(int event, int x, int y, int flags, void* ustc) {
    // std::cout << "onMouse: " << x << " " << y << std::endl;
    mouseX  = x;
    mouseY  = y;
    mouseBtnType = event;
    }
        在初始化阶段配置两个ros::Publisher节点。每个节点分别由按下鼠标左键和右键触发,并使得按下相应的按键时能够执行发布话题的行为。例如,在代码实现中可以通过条件判断来实现这一功能
    ros::Publisher leftBtnPointPub =
    nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/left", 1);
    ros::Publisher rightBtnPointPub =
    nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/right", 1);
        其中消息格式为geometry_msgs/PointStamped的ROS自带消息,在源节点中应包含该头文件#include <geometry_msgs/PointStamped.h>的具体定义如下:
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Point point
      float64 x
      float64 y
      float64 z
        然后再重写cloudViewer()函数如下:
    void cloudViewer() {
      cv::Mat color, depth;
      std::chrono::time_point<std::chrono::high_resolution_clock> start, now;
      double fps = 0;
      size_t frameCount = 0;
      std::ostringstream oss;
      std::ostringstream ossXYZ; // 新增一个string流
      const cv::Point pos(5, 15);
      const cv::Scalar colorText = CV_RGB(255, 0, 0);
      const double sizeText = 0.5;
      const int lineText = 1;
      const int font = cv::FONT_HERSHEY_SIMPLEX;
      // 从全局变量获取当前鼠标坐标
      int img_x = mouseX;
      int img_y = mouseY;
      geometry_msgs::PointStamped ptMsg;
      ptMsg.header.frame_id = "kinect_link";
    
      lock.lock();
      color = this->color;
      depth = this->depth;
      updateCloud = false;
      lock.unlock();
    
      const std::string window_name = "color viewer";
      cv::namedWindow(window_name);
      // 注册鼠标回调函数, 第三个参数是C++11中的关键字, 若不支持C++11, 替换成NULL
      cv::setMouseCallback(window_name, onMouse, nullptr);
      createCloud(depth, color, cloud);
    
      for(; running && ros::ok();) {
    if(updateCloud) {
      lock.lock();
      color = this->color;
      depth = this->depth;
      updateCloud = false;
      lock.unlock();
    
      createCloud(depth, color, cloud);
      img_x = mouseX;
      img_y = mouseY;
      const pcl::PointXYZRGBA& pt = cloud->points[img_y * depth.cols + img_x];
      ptMsg.point.x = pt.x;
      ptMsg.point.y = pt.y;
      ptMsg.point.z = pt.z;
    
      // 根据鼠标左键压下或右键压下, 分别发布三维坐标到不同的话题上去
      switch (mouseBtnType) {
      case cv::EVENT_LBUTTONUP:
          ptMsg.header.stamp = ros::Time::now();
          leftBtnPointPub.publish(ptMsg);
          ros::spinOnce();
          break;
      case cv::EVENT_RBUTTONUP:
          ptMsg.header.stamp = ros::Time::now();
          rightBtnPointPub.publish(ptMsg);
          ros::spinOnce();
          break;
      default:
          break;
      }
      mouseBtnType = cv::EVENT_MOUSEMOVE;
    
      ++frameCount;
      now = std::chrono::high_resolution_clock::now();
      double elapsed =
          std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() / 1000.0;
      if(elapsed >= 1.0) {
        fps = frameCount / elapsed;
        oss.str("");
        oss << "fps: " << fps << " ( " << elapsed / frameCount * 1000.0 << " ms)";
        start = now;
        frameCount = 0;
      }
    
      cv::putText(color, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
      ossXYZ.str("");
      ossXYZ << "( " << ptMsg.point.x << ", " << ptMsg.point.y
                                  << ", " << ptMsg.point.z << " )";
      cv::putText(color, ossXYZ.str(), cv::Point(img_x, img_y), font, 1, colorText, 3, CV_AA);
      // cv::circle(color, cv::Point(mouseX, mouseY), 5, cv::Scalar(0, 0, 255), -1);
      cv::imshow(window_name, color);
      // cv::imshow(window_name, depth);
      cv::waitKey(1);
    }
    
      }
      cv::destroyAllWindows();
      cv::waitKey(100);
    }
        最后一步稍作改动即可完成任务。整个主函数的具体内容如下:其中去掉了不必要的参数解析部分,使得使用更加固定且简单。
    int main(int argc, char **argv) {
    #if EXTENDED_OUTPUT
      ROSCONSOLE_AUTOINIT;
      if(!getenv("ROSCONSOLE_FORMAT"))
      {
    ros::console::g_formatter.tokens_.clear();
    ros::console::g_formatter.init("[${severity}] ${message}");
      }
    #endif
    
      ros::init(argc, argv, "kinect2_viewer", ros::init_options::AnonymousName);
    
      if(!ros::ok()) {
    return 0;
      }
    
      std::string ns = K2_DEFAULT_NS;
      std::string topicColor = K2_TOPIC_HD K2_TOPIC_IMAGE_COLOR K2_TOPIC_IMAGE_RECT;
      std::string topicDepth = K2_TOPIC_HD K2_TOPIC_IMAGE_DEPTH K2_TOPIC_IMAGE_RECT;
      bool useExact = true;
      bool useCompressed = false;
      Receiver::Mode mode = Receiver::CLOUD;
    
      topicColor = "/" + ns + topicColor;
      topicDepth = "/" + ns + topicDepth;
      OUT_INFO("topic color: " FG_CYAN << topicColor << NO_COLOR);
      OUT_INFO("topic depth: " FG_CYAN << topicDepth << NO_COLOR);
    
      Receiver receiver(topicColor, topicDepth, useExact, useCompressed);
    
      OUT_INFO("starting receiver...");
      OUT_INFO("Please click mouse in color viewer...");
      receiver.run(mode);
    
      ros::shutdown();
      return 0;
    }
        在CMakeList.txt中加入下面两段话, 然后make就OK.
    add_executable(click_rgb src/click_rgb.cpp)
    target_link_libraries(click_rgb
      ${catkin_LIBRARIES}
      ${OpenCV_LIBRARIES}
      ${PCL_LIBRARIES}
      ${kinect2_bridge_LIBRARIES}
    )
    
    install(TARGETS click_rgb
      RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
    )
        执行该命令后会启动 Kinect2 viewer 并显示结果。如图所示,在红圈标记的位置标注了当前摄像头捕捉到的目标物体。然而,在尝试截图时发现无法将鼠标位置捕获下来。

