機器人操作系統(tǒng)（ROS2）入門與實踐 課件 第9章 ROS2中的圖像視覺應用

機器人操作系統(tǒng)（ROS2）入門與實踐機器人操作系統(tǒng)（ROS2）入門與實踐第1章LinuxUbuntu入門基礎第2章ROS2安裝與系統(tǒng)架構第3章ROS2編程基礎第4章ROS2機器人運動控制第5章激光雷達在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM環(huán)境建圖第8章ROS2中的NAV2自主導航第9章ROS2中的圖像視覺應用第10章ROS2的三維視覺應用第11章ROS2的機械臂應用第12章基于ROS2的綜合應用第9章9.3基于圖像視覺的目標追蹤實現(xiàn)

第9章ROS2中的圖像視覺應用9.2OpenCV顏色特征提取和目標定位9.1視覺圖像數(shù)據(jù)的獲取9.4基于圖像視覺的人臉檢測實現(xiàn)9.5本章小結9.1視覺圖像數(shù)據(jù)的獲取

詳細操作步驟見教材P260-P272頁

視覺圖像數(shù)據(jù)的獲取是通過訂閱相機驅動節(jié)點發(fā)布的話題，從話題中獲取相機發(fā)出的消息包來實現(xiàn)的。機器人頭部安裝的是KinectV2RGB-D相機，對應的話題名稱為“/kinect2/qhd/image_raw”，話題中的消息包格式為sensor_msgs::Image。9.1視覺圖像數(shù)據(jù)的獲取本實驗將會實現(xiàn)一個訂閱者節(jié)點，訂閱相機發(fā)布的話題"/kinect2/qhd/image_raw"。從此話題中接收sensor_msgs::Image類型的消息包，并將其中的圖像數(shù)據(jù)轉換成OpenCV的格式。最后使用OpenCV的圖形顯示接口，將圖像顯示在圖形窗口中。9.1.1編寫圖像數(shù)據(jù)獲取程序cd~/ros2_ws/srcros2pkgcreatecv_pkg打開一個新的終端窗口，在工作空間中創(chuàng)建一個名為“cv_pkg”的軟件包。1、編寫節(jié)點代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_image.cpp”。下面編寫這個代碼文件內容：9.1視覺圖像數(shù)據(jù)的獲取#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>

std::shared_ptr<rclcpp::Node>node;

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){cv_bridge::CvImagePtrcv_ptr;cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

cv::MatimgOriginal=cv_ptr->image;cv::imshow("RGB",imgOriginal);cv::waitKey(1);}9.1視覺圖像數(shù)據(jù)的獲取intmain(intargc,char**argv){rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_image_node");

autorgb_sub=node->create_subscription<sensor_msgs::msg::Image>("/kinect2/qhd/image_raw",1,CamRGBCallback);

cv::namedWindow("RGB");

rclcpp::spin(node);

cv::destroyAllWindows();

rclcpp::shutdown();

return0;}9.1視覺圖像數(shù)據(jù)的獲取2、設置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(cv_bridgeREQUIRED)find_package(OpenCVREQUIRED)add_executable(cv_imagesrc/cv_image.cpp)ament_target_dependencies(cv_image"rclcpp""sensor_msgs""cv_bridge""OpenCV")install(TARGETScv_imageDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>cv_bridge</depend><depend>OpenCV</depend>4、編譯軟件包cd~/ros2_wscolconbuild9.1視覺圖像數(shù)據(jù)的獲取9.1.2仿真運行圖像數(shù)據(jù)獲取程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_balls.launch.py9.1視覺圖像數(shù)據(jù)的獲取sourceinstall/setup.bashros2runcv_pkgcv_image打開第2個子窗口。

節(jié)點運行起來之后，會彈出一個名為“RGB”窗口程序，顯示機器人頭部相機所看到的四個顏色球的圖像。9.1視覺圖像數(shù)據(jù)的獲取sourceinstall/setup.bashros2runwpr_simulation2ball_random_move

為了測試這個圖像是不是實時獲取的，可以借助wpr_simulation2附帶的程序讓中間的桔色球動起來，以便進行對比觀察。打開第3個子窗口。

執(zhí)行之后，可以看到仿真窗口里的桔色球開始隨機運動。此時再切換到“RGB”窗口，可以看到圖像中的桔色球也跟著運動，說明這個采集到的圖像是實時更新的。9.2OpenCV顏色特征提取和目標定位

詳細操作步驟見教材P272-P291頁

在9.1節(jié)的實驗里，實現(xiàn)了從機器人的頭部相機獲取機器人的視覺圖像。這一次將繼續(xù)深入，使用OpenCV實現(xiàn)機器人視覺中的顏色特征提取和目標定位功能。1）對機器人視覺圖像進行顏色空間轉換，從RGB空間轉換到HSV空間，排除光照影響。2）對轉換后的圖像進行二值化處理，將目標物體分割提取出來。3）對提取到的目標像素進行計算統(tǒng)計，得出目標物的質心坐標。1、編寫節(jié)點代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_hsv.cpp”。下面編寫這個代碼文件內容：9.2OpenCV顏色特征提取和目標定位9.2.1編寫特征提取和目標定位程序#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>

std::shared_ptr<rclcpp::Node>node;

usingnamespacecv;usingnamespacestd;staticintiLowH=10;staticintiHighH=40;

staticintiLowS=90;staticintiHighS=255;

staticintiLowV=1;staticintiHighV=255;

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){cv_bridge::CvImagePtrcv_ptr;cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

MatimgOriginal=cv_ptr->image;

MatimgHSV;cvtColor(imgOriginal,imgHSV,COLOR_BGR2HSV);

9.2OpenCV顏色特征提取和目標定位vector<Mat>hsvSplit;split(imgHSV,hsvSplit);equalizeHist(hsvSplit[2],hsvSplit[2]);merge(hsvSplit,imgHSV);

MatimgThresholded;inRange(imgHSV,Scalar(iLowH,iLowS,iLowV),Scalar(iHighH,iHighS,iHighV),imgThresholded);

Matelement=getStructuringElement(MORPH_RECT,Size(5,5));morphologyEx(imgThresholded,imgThresholded,MORPH_OPEN,element);morphologyEx(imgThresholded,imgThresholded,MORPH_CLOSE,element);9.2OpenCV顏色特征提取和目標定位intnTargetX=0;intnTargetY=0;intnPixCount=0;intnImgWidth=imgThresholded.cols;intnImgHeight=imgThresholded.rows;for(inty=0;y<nImgHeight;y++){for(intx=0;x<nImgWidth;x++){if(imgThresholded.data[y*nImgWidth+x]==255){nTargetX+=x;nTargetY+=y;nPixCount++;}}}9.2OpenCV顏色特征提取和目標定位if(nPixCount>0){nTargetX/=nPixCount;nTargetY/=nPixCount;printf("Target(%d,%d)PixelCount=%d\n",nTargetX,nTargetY,nPixCount);Pointline_begin=Point(nTargetX-10,nTargetY);Pointline_end=Point(nTargetX+10,nTargetY);line(imgOriginal,line_begin,line_end,Scalar(255,0,0));line_begin.x=nTargetX;line_begin.y=nTargetY-10;line_end.x=nTargetX;line_end.y=nTargetY+10;line(imgOriginal,line_begin,line_end,Scalar(255,0,0));}else{printf("Targetdisappeared...\n");}

imshow("RGB",imgOriginal);imshow("HSV",imgHSV);imshow("Result",imgThresholded);cv::waitKey(5);}9.2OpenCV顏色特征提取和目標定位intmain(intargc,char**argv){rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_hsv_node");

autorgb_sub=node->create_subscription<sensor_msgs::msg::Image>("/kinect2/qhd/image_raw",1,CamRGBCallback);

namedWindow("Threshold",WINDOW_AUTOSIZE);

createTrackbar("LowH","Threshold",&iLowH,179);createTrackbar("HighH","Threshold",&iHighH,179);

createTrackbar("LowS","Threshold",&iLowS,255);createTrackbar("HighS","Threshold",&iHighS,255);

createTrackbar("LowV","Threshold",&iLowV,255);createTrackbar("HighV","Threshold",&iHighV,255);

namedWindow("RGB");namedWindow("HSV");namedWindow("Result");

rclcpp::spin(node);

cv::destroyAllWindows();

rclcpp::shutdown();return0;}9.2OpenCV顏色特征提取和目標定位2、設置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(cv_bridgeREQUIRED)find_package(OpenCVREQUIRED)add_executable(cv_hsvsrc/cv_hsv.cpp)ament_target_dependencies(cv_hsv"rclcpp""sensor_msgs""cv_bridge""OpenCV")install(TARGETScv_hsvDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>cv_bridge</depend><depend>OpenCV</depend>4、編譯軟件包cd~/ros2_wscolconbuild9.2OpenCV顏色特征提取和目標定位sourceinstall/setup.bashros2launchwpr_simulation2wpb_balls.launch.py9.2OpenCV顏色特征提取和目標定位9.2.2仿真運行特征提取和目標定位程序sourceinstall/setup.bashros2runcv_pkgcv_hsv打開第2個子窗口。

節(jié)點運行起來之后，會彈出四個窗口，分別是“RGB”窗口、“HSV”窗口、“Result”窗口、“Threshold”窗口。9.2OpenCV顏色特征提取和目標定位9.2OpenCV顏色特征提取和目標定位切換到運行cv_hsv節(jié)點的終端窗口，可以看到追蹤的目標物的中心坐標值。sourceinstall/setup.bashros2runwpr_simulation2ball_random_move

為了測試目標追蹤的效果，可以借助wpr_simulation2附帶的程序讓中間的桔色球動起來，以便進行對比觀察。打開第3個子窗口。

執(zhí)行之后，可以看到仿真窗口里的桔色球開始隨機運動。此時再切換到“Result”窗口，觀察圖像中的桔色目標球移動時，顏色特征提取的效果。9.2OpenCV顏色特征提取和目標定位9.3基于圖像視覺的目標追蹤實現(xiàn)

詳細操作步驟見教材P292-P310頁

在9.2節(jié)的實驗里，使用OpenCV實現(xiàn)機器人視覺中的顏色特征提取和目標定位功能。如圖所示，這一次將對目標定位功能進行擴展，根據(jù)目標位置計算速度并輸出給機器人，讓機器人跟隨球進行移動，實現(xiàn)一個目標跟隨的閉環(huán)控制。9.3基于圖像視覺的目標追蹤實現(xiàn)在編寫例程代碼前，先設計一下這個程序的實現(xiàn)思路，如以下四步：1）對機器人視覺圖像進行顏色空間轉換，從RGB空間轉換到HSV空間，排除光照影響。2）對轉換后的圖像進行二值化處理，將目標物體分割提取出來。3）對提取到的目標像素進行計算統(tǒng)計，得出目標物的質心坐標。4）根據(jù)目標位置計算機器人運動速度，完成目標跟隨功能。1、編寫節(jié)點代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_follow.cpp.cpp”。下面編寫這個代碼文件內容：#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>#include<geometry_msgs/msg/twist.hpp>

std::shared_ptr<rclcpp::Node>node;

usingnamespacecv;usingnamespacestd;9.3基于圖像視覺的目標追蹤實現(xiàn)9.3.1編寫目標追蹤程序staticintiLowH=10;staticintiHighH=40;

staticintiLowS=90;staticintiHighS=255;

staticintiLowV=1;staticintiHighV=255;geometry_msgs::msg::Twistvel_cmd;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtrvel_pub;

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){cv_bridge::CvImagePtrcv_ptr;cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

MatimgOriginal=cv_ptr->image;

MatimgHSV;cvtColor(imgOriginal,imgHSV,COLOR_BGR2HSV);

9.3基于圖像視覺的目標追蹤實現(xiàn)vector<Mat>hsvSplit;split(imgHSV,hsvSplit);equalizeHist(hsvSplit[2],hsvSplit[2]);merge(hsvSplit,imgHSV);

MatimgThresholded;inRange(imgHSV,Scalar(iLowH,iLowS,iLowV),Scalar(iHighH,iHighS,iHighV),imgThresholded);

Matelement=getStructuringElement(MORPH_RECT,Size(5,5));morphologyEx(imgThresholded,imgThresholded,MORPH_OPEN,element);morphologyEx(imgThresholded,imgThresholded,MORPH_CLOSE,element);9.3基于圖像視覺的目標追蹤實現(xiàn)intnTargetX=0;intnTargetY=0;intnPixCount=0;intnImgWidth=imgThresholded.cols;intnImgHeight=imgThresholded.rows;for(inty=0;y<nImgHeight;y++){for(intx=0;x<nImgWidth;x++){if(imgThresholded.data[y*nImgWidth+x]==255){nTargetX+=x;nTargetY+=y;nPixCount++;}}}9.3基于圖像視覺的目標追蹤實現(xiàn)if(nPixCount>0){nTargetX/=nPixCount;nTargetY/=nPixCount;printf("Target(%d,%d)PixelCount=%d\n",nTargetX,nTargetY,nPixCount);Pointline_begin=Point(nTargetX-10,nTargetY);Pointline_end=Point(nTargetX+10,nTargetY);line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);line_begin.x=nTargetX;line_begin.y=nTargetY-10;line_end.x=nTargetX;line_end.y=nTargetY+10;line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);

floatfVelFoward=(nImgHeight/2-nTargetY)*0.002;floatfVelTurn=(nImgWidth/2-nTargetX)*0.003;vel_cmd.linear.x=fVelFoward;vel_cmd.linear.y=0;vel_cmd.linear.z=0;vel_cmd.angular.x=0;vel_cmd.angular.y=0;vel_cmd.angular.z=fVelTurn;}9.3基于圖像視覺的目標追蹤實現(xiàn)else{printf("Targetdisappeared...\n");vel_cmd.linear.x=0;vel_cmd.linear.y=0;vel_cmd.linear.z=0;vel_cmd.angular.x=0;vel_cmd.angular.y=0;vel_cmd.angular.z=0;}

vel_pub->publish(vel_cmd);

imshow("Result",imgThresholded);imshow("RGB",imgOriginal);cv::waitKey(5);}9.3基于圖像視覺的目標追蹤實現(xiàn)intmain(intargc,char**argv){rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_follow_node");

vel_pub=node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel",10);autosub=node->create_subscription<sensor_msgs::msg::Image>("/kinect2/qhd/image_raw",1,CamRGBCallback);

namedWindow("RGB");namedWindow("Result");

rclcpp::spin(node);

cv::destroyAllWindows();

rclcpp::shutdown();return0;}9.3基于圖像視覺的目標追蹤實現(xiàn)2、設置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(cv_bridgeREQUIRED)find_package(OpenCVREQUIRED)find_package(geometry_msgsREQUIRED)add_executable(cv_followsrc/cv_follow.cpp)ament_target_dependencies(cv_follow"rclcpp""sensor_msgs""cv_bridge""OpenCV""geometry_msgs")install(TARGETScv_followDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>cv_bridge</depend><depend>OpenCV</depend><depend>geometry_msgs</depend>9.3基于圖像視覺的目標追蹤實現(xiàn)4、編譯軟件包cd~/ros2_wscolconbuild9.3基于圖像視覺的目標追蹤實現(xiàn)9.3.2仿真運行目標追蹤程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_balls.launch.pysourceinstall/setup.bash打開第2個子窗口。

節(jié)點運行起來之后，會彈出兩個窗口，分別是“RGB”窗口和“Result”窗口。9.3基于圖像視覺的目標追蹤實現(xiàn)ros2runcv_pkgcv_follow9.3基于圖像視覺的目標追蹤實現(xiàn)切換到仿真窗口，可以看到里面的機器人對準桔色球，輕微向后移動，與桔色球保持固定距離。如圖所示，在運行cv_follow節(jié)點的終端窗口，可以看到追蹤目標物的中心坐標值在不停地刷新。sourceinstall/setup.bashros2runwpr_simulation2ball_random_move

為了測試機器人追蹤目標球的效果，可以借助wpr_simulation2附帶的程序讓桔色球動起來。以便進行觀察。打開第3個子窗口。

執(zhí)行之后，可以看到仿真窗口里的桔色球開始隨機運動，而機器人也追著桔色球在移動。此時再切換到“RGB”窗口，觀察藍色十字標記是對桔色目標球的追蹤效果。9.3基于圖像視覺的目標追蹤實現(xiàn)9.3基于圖像視覺的目標追蹤實現(xiàn)

經過前面三個實驗，終于將識別檢測和運動行為結合起來，形成一個典型的視覺閉環(huán)控制系統(tǒng)。機器人與外部世界的交互，形式雖然多樣，但是本質上都是這樣一套“識別→定位→操作”的閉環(huán)控制系統(tǒng)。通過這樣一個簡單的例子，了解和學習這種實現(xiàn)思路，可以為將來構建更復雜的機器人系統(tǒng)奠定一個基礎。9.4基于圖像視覺的人臉檢測實現(xiàn)本節(jié)借助現(xiàn)成的人臉識別算法庫來實現(xiàn)人臉檢測，這個算法庫的調用在wpr_simulaiton2的face_detector.py節(jié)點中已經實現(xiàn)，直接使用即可。face_detector.py節(jié)點會訂閱話題“/face_detector_input”，作為人臉圖像的輸入。圖像中的人臉被檢測到后，其坐標值會發(fā)布到話題“/face_position”中去。所以，這個實驗只需要編寫一個節(jié)點，從相機的話題中獲取圖片，轉發(fā)給face_detector.py節(jié)點進行人臉檢測。然后從face_detector.py的“/face_position”話題獲取人臉坐標結果即可。9.4基于圖像視覺的人臉檢測實現(xiàn)

詳細操作步驟見教材P311-P326頁在編寫代碼前，需要安裝人臉檢測節(jié)點face_detector.py的依賴項。cd~/ros2_ws/src/wpr_simulation2/scripts/./install_dep_face.sh9.4.1編寫人臉檢測程序1、編寫節(jié)點代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_face_detect.cpp”。#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<sensor_msgs/msg/region_of_interest.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>

std::shared_ptr<rclcpp::Node>node;cv::MatimgFace;rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtrframe_pub;

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){cv_bridge::CvImagePtrcv_ptr;cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

imgFace=cv_ptr->image;

frame_pub->publish(*msg);}9.4基于圖像視覺的人臉檢測實現(xiàn)voidFacePosCallback(constsensor_msgs::msg::RegionOfInterest::SharedPtrmsg){cv::rectangle(imgFace,cv::Point(msg->x_offset,msg->y_offset),cv::Point(msg->x_offset+msg->width,msg->y_offset+msg->height),cv::Scalar(0,0,255),2,cv::LINE_8);cv::imshow("Face",imgFace);cv::waitKey(1);}

9.4基于圖像視覺的人臉檢測實現(xiàn)intmain(intargc,char**argv){rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_face_detect");

autorgb_sub=node->create_subscription<sensor_msgs::msg::Image>("/kinect2/qhd/image_raw",1,CamRGBCallback);frame_pub=node->create_publisher<sensor_msgs::msg::Image>("/face_de