当前位置：首页 > news >正文

4.1 ROS颜色目标识别与定位

news 2025/6/26 7:28:50

启动仿真环境

roslaunch wpr_simulation wpb_balls.launch

新建cv_hsv_node.cpp

#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>using namespace cv;
using namespace std;//颜色阈值
static int iLowH=10;
static int iHighH=40;static int iLowS=90;
static int iHighS=255;static int iLowV=1;
static int iHighV=255;void Cam_RGB_Callback(const sensor_msgs::Image msg)
{cv_bridge::CvImagePtr cv_ptr;//增加错误处理try{cv_ptr=cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);}catch(cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s",e.what());return;}Mat imgOriginal = cv_ptr->image;//将RGB图像转换为HSVMat imgHSV;cvtColor(imgOriginal,imgHSV,COLOR_BGR2HSV);//在HSV空间做直方图均衡化vector<Mat> hsvSplit;split(imgHSV,hsvSplit);equalizeHist(hsvSplit[2],hsvSplit[2]);merge(hsvSplit,imgHSV);//使用上述的Hue，Saturation和Value的阈值范围进行二值化处理Mat imgThresholded;inRange(imgHSV,Scalar(iLowH,iLowS,iLowV),Scalar(iHighH,iHighS,iHighV),imgThresholded);//图像形态学处理-->开操作（腐蚀，去除噪点）Mat element = getStructuringElement(MORPH_RECT, Size(5,5));morphologyEx(imgThresholded,imgThresholded,MORPH_OPEN,element);//图像形态学处理-->闭操作（膨胀，连通区域）morphologyEx(imgThresholded,imgThresholded,MORPH_CLOSE,element);//遍历二值化后的图像对象int nTargetX = 0;int nTargetY=0;int nPixCount=0;int nImgWidth=imgThresholded.cols;int nImgHeight=imgThresholded.rows;int nImgChannels=imgThresholded.channels();for (int y = 0; y < nImgHeight; y++){for(int x = 0;x<nImgWidth; x++){if(imgThresholded.data[y*nImgWidth + x] == 255){nTargetX += x;nTargetY += y;nPixCount ++;}}}if(nPixCount>0){nTargetX /= nPixCount;nTargetY /=nPixCount;printf("颜色质心坐标（%d , %d）点数=%d\n",nTargetX,nTargetY,nPixCount);//画坐标Point line_begin = Point(nTargetX-10,nTargetY);Point line_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("目标颜色消失...\n");}//显示处理结果imshow("RGB",imgOriginal);imshow("HSV",imgHSV);imshow("Result",imgThresholded);cv::waitKey(5);}int main(int argc, char *argv[])
{ros::init(argc,argv,"cv_hsv_node");ros::NodeHandle nh;ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect",1,Cam_RGB_Callback);namedWindow("Threshold",WINDOW_AUTOSIZE);createTrackbar("LowH","Threshold",&iLowH,179);//Hue(0-179)createTrackbar("HighH","Threshold",&iHighH,179);createTrackbar("LowS","Threshold",&iLowS,255);//Saturation(0-255)createTrackbar("HighS","Threshold",&iHighS,255);createTrackbar("LowV","Threshold",&iLowV,255);//Value(0-255)createTrackbar("HighV","Threshold",&iHighV,255);namedWindow("RGB");namedWindow("HSV");namedWindow("Result");ros::Rate loop_rate(30);while(ros::ok()){ros::spinOnce();loop_rate.sleep();}return 0;
}

在CMakeLists.txt增加编译条件

add_executable(cv_hsv_node src/cv_hsv_node.cpp)
add_dependencies(cv_hsv_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(cv_hsv_node${catkin_LIBRARIES} ${OpenCV_LIBS}
)

运行节点