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, includingapt-get install cuda
which installs Nvidia graphics driver. - (Jetson TK1) It is preloaded.
- (Nvidia/Intel dual GPUs) After
apt-get install cuda
, usesudo prime-select intel
to use Intel GPU for desktop. - (Other) Follow Nvidia website’s instructions.
- (Ubuntu 14.04 only) Download
- 安裝 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
, 不出意外, 應該能夠看到如下效果:
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. 效果如下圖所示, 圖中紅色座標位置, 實際是滑鼠所在位置, 截圖時, 滑鼠截不下來.