1. 程式人生 > >ROS下Kinect2的驅動安裝及簡單應用

ROS下Kinect2的驅動安裝及簡單應用

目錄

Kinect2

相信對這個話題感興趣的同學, 對Kinect2應該也是很熟悉的吧。 這個裝置現在也不貴, 某東上面大概兩千左右就能買到, 並且還能配置一個三腳架。 Kinect2的效果, 確實會比1代要好很多, 無論是骨骼點還是影象質量等等。 更詳細的介紹, 感興趣的同學可以自行Google, 在MS官網上面檢視。

ROS

做機器人相關的工作的同學, 同時對該部分也不會陌生吧。 機器人作業系統(ROS), 應用非常廣泛, 並且有很多開源庫, 包可以使用。 同時, 主流的感測器在ROS中也都有支援。 當然, Kinect2也是能夠支援的。 ROS安裝於Ubuntu之上, 我使用的版本是Ubuntu14.04 + ROS indigo。 關於ROS的安裝問題, 可以檢視

官網相關指導 。 很簡單的步驟。 依次輸入下列命令:

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

驅動節點配置

首先, 需要安裝其libfreenect2, 步驟如下(以下預設以ubuntu14.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 do 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 only) Download cuda-repo-ubuntu1404...*.deb (“deb (network)”) from Nvidia website, follow their installation instructions, including apt-get install cuda which installs Nvidia graphics driver.
    • (Jetson TK1) It is preloaded.
    • (Nvidia/Intel dual GPUs) After apt-get install cuda, use sudo prime-select intel to use Intel GPU for desktop.
    • (Other) Follow Nvidia website’s instructions.
  • 安裝 VAAPI (可選, Intel only)
    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 rules: sudo cp ../platform/linux/udev/90-kinect2.rules /etc/udev/rules.d/, 然後重新插拔Kinect2.
  • 一切搞定, 現在可以嘗試執行Demo程式: ./bin/Protonect, 不出意外, 應該能夠看到如下效果:

    DEMO

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對應的目錄。應該先執行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, 所以試著弄了一下, 將其整理下來形成這篇部落格的.

今天突然有一個同學在站內給我私信, 問我這篇部落格的內容. 分享出來的東西幫助到了別人, 確實很高興! 關於這位同學問到的問題, 其實在前面的工作中, 我也實現過. 下面試著回憶整理一下.

儲存圖片

其實目的就一個, 將Kinect2的RGB圖儲存下來. 在前面的敘述中, 輸入rosrun kinect2_viewer kinect2_viewer sd cloud來檢視顯示效果. 這句命令實質就是執行kinect2_viewer包中的kinect2_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鍵或者空格鍵就OK, 儲存的圖片就在當前目錄.

如果有一些特別的需求, 在上面原始碼的基礎上來進行實現, 將會事半功倍. 下面就是一個小小的例子.

儲存圖片序列

如果想要儲存一個圖片序列, 僅僅控制開始結束, 例如, 按鍵B(Begin)開始儲存, 按鍵E(End)結束儲存.

完成這樣一個功能, 在原始碼的基礎上進行適當更改就可以滿足要求. 首選, 需要每一次對按鍵B和E的判斷, 需要新增到上面摘錄的switch(key & 0xFF)塊中. 需要連續儲存的話, 簡單的設定一個標誌位即可.

首先, 複製vewer.cpp檔案, 命名為save_seq.cpp. 修改save_seq.cpp檔案, 在Receiver類中bool save變數下面新增一個新的成員變數, bool save_seq. 在類的建構函式的初始化列表中新增該變數的初始化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}
)

返回到catkin主目錄, 執行catkin_make, 編譯, 連結沒有問題的話, 就可以檢視效果了. 當然了, 首先是需要啟動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);
}

最後, 稍微改寫一下主函式就OK了, 整個主函式摘錄如下, 其中去掉了多餘的引數解析, 讓使用更加固定, 簡單.

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}
)

執行的話, 輸入rosrun kinect2_viewer click_rgb就OK. 效果如下圖所示, 圖中紅色座標位置, 實際是滑鼠所在位置, 截圖時, 滑鼠截不下來.


這裡寫圖片描述