ROS学习笔记(六)ROS视觉处理(简单的色彩识别)
1. 摄像头驱动(usb_cam)
安装ROS系统的usb驱动程序usb_cam
cd catkin_ws/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git
sudo apt-get install libv4l-dev
cd ~/catkin_ws
catkin_make
source devel/setup.bash
修改 usb_cam-test.launch文件
<launch>
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen"
>
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="color_format" value="yuv422p" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
<node name="image_view" pkg="image_view" type="image_view"
respawn="false" output="screen">
<remap from="image" to="/usb_cam/image_raw"/>
<param name="autosize" value="true" />
</node>
</launch>
安装ROS系统的图像查看器image_view
sudo apt install ros-noetic-image-view
连接笔记本摄像头到VMware虚拟机系统上:虚拟机->可移动设备->Camera
用以下命令打开摄像头
roslaunch usb_cam usb_cam-test.launch
2. 驱动摄像头
创建cv_image
功能包
cd catkin_ws/src
catkin_create_pkg cv_image std_msgs roscpp cv_bridge sensor_msgs image_transport
创建节点
在cv_image/src
下编写图像处理程序cv_image_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;
void Cam_RGB_Callback(const sensor_msgs::ImageConstPtr &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());
}
Mat imgOriginal = cv_ptr->image;
imshow("RGB", imgOriginal);
waitKey(1);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cv_image_node");
ros::NodeHandle nh;
ros::Subscriber rgb_sub = nh.subscribe("/usb_cam/image_raw", 1, Cam_RGB_Callback);
namedWindow("RGB");
ros::spin();
}
修改CMakeLists.txt和package.xml文件
加上OpenCV的依赖并链接相应库
find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
roscpp
sensor_msgs
std_msgs
)
find_package(OpenCV REQUIRED)
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
add_executable(cv_image_node src/cv_image_node.cpp)
target_link_libraries(cv_image_node ${catkin_LIBRARIES} ${OpenCV_LIBS})
在package.xml文件,在里面添加
<build_depend>opencv2</build_depend>
<build_export_depend>opencv2</build_export_depend>
<exec_depend>opencv2</exec_depend>
编译运行
编译
cd ~/catkin_ws
catkin_make
运行该程序:
roscore
rosrun usb_cam usb_cam_node
rosrun cv_image cv_image_node
运行结果:
3. 仿真环境图像获取
启动wpr
仿真环境
roslaunch wpr_simulation wpb_balls.launch
创建节点
在cv_image/src
中新建文件cv_hsv_code.cpp
#include <ros/ros.h>
#include <image_transport/image_transport.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)
{
//ROS_INFO("Cam_RGB_Callback");
cv_bridge::CvImagePtr cv_ptr;
try
{
// 将ROS图像消息转换为OpenCV图像
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图片转换成HSV
Mat imgHSV;
vector<Mat> hsvSplit;
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
//在HSV空间做直方图均衡化
split(imgHSV, hsvSplit);
equalizeHist(hsvSplit[2],hsvSplit[2]);
merge(hsvSplit,imgHSV);
Mat imgThresholded;
//使用上面的Hue,Saturation和Value的阈值范围对图像进行二值化
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();
//printf("w= %d h= %d size = %d\n",nImgWidth,nImgHeight,nImgChannels);
for (int y = 0; y < nImgHeight; y++)
{
for(int x = 0; x < nImgWidth; x++)
{
//printf("%d ",imgThresholded.data[y*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, "demo_cv_hsv");
ros::NodeHandle nh;
ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect", 1 , Cam_RGB_Callback);
ros::Rate loop_rate(30);
//生成图像显示和参数调节的窗口空见
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");
while( ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
}
修改CMakeLists.txt
添加以下内容:
add_executable(cv_hsv_code src/cv_hsv_code.cpp)
target_link_libraries(cv_hsv_code ${catkin_LIBRARIES} ${OpenCV_INCLUDE_DIRS})
编译运行
编译
cd ~/catkin_ws
catkin_make
启动仿真环境,测试程序:
roslaunch wpr_simulation wpb_balls.launch
rosrun cv_image cv_hsv_code
rosrun wpr_simulation keyboard_vel_ctrl
4 识别红色和绿色物体
创建节点
在cv_image/src
中新建文件cv_color.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;
void Cam_RGB_Callback(const sensor_msgs::ImageConstPtr &msg)// 摄像头回调函数
{
// 定义一个cv_bridge指针
cv_bridge::CvImagePtr cv_ptr;
try
{
// 将ROS图像转换为OpenCV图像
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception &e)
{
ROS_ERROR("cv_bridge exception:%s", e.what());
}
// 获取原始图像
Mat imgOriginal = cv_ptr->image;
Mat imgHSV;// 定义一个HSV图像
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); // 将原始图像转换为HSV图像
Mat mask_red, mask_green;// 红色和绿色物体的二值图像
inRange(imgHSV, Scalar(0, 70, 50), Scalar(10, 255, 255), mask_red);
inRange(imgHSV, Scalar(50, 70, 50), Scalar(100, 255, 255), mask_green);
Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(5, 5));// 形态学操作的内核大小
dilate(mask_red, mask_red, kernel);
dilate(mask_green, mask_green, kernel);
erode(mask_red, mask_red, kernel);
erode(mask_green, mask_green, kernel);
Mat res_red, res_green;// 红色和绿色物体的结果图像
bitwise_and(imgOriginal, imgOriginal, res_red, mask_red);
bitwise_and(imgOriginal, imgOriginal, res_green, mask_green);
imshow("RGB", imgOriginal);//显示原始图像
resize(imgOriginal, imgOriginal, Size(imgOriginal.cols/2, imgOriginal.rows/2));// 调整原始图像的大小
imshow("Red", res_red);
imshow("Green", res_green);
waitKey(100);
}
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "cv_image_node");
ros::NodeHandle nh;// 定义ROS句柄
// 订阅图像话题
ros::Subscriber rgb_sub = nh.subscribe("/usb_cam/image_raw", 1, Cam_RGB_Callback);
namedWindow("RGB");
namedWindow("Red");
namedWindow("Green");
ros::spin();
}
修改CMakeLists.txt
添加以下内容:
add_executable(cv_color src/cv_color.cpp)
target_link_libraries(cv_color ${catkin_LIBRARIES} ${OpenCV_INCLUDE_DIRS})
编译运行
编译
cd ~/catkin_ws
catkin_make
运行程序
roscore
rosrun usb_cam usb_cam_node
rosrun cv_image cv_color
运行结果:
5 识别和追踪红色(绿色)物体
创建节点
在cv_image/src
中新建文件color.cpp
#include <ros/ros.h>
#include <image_transport/image_transport.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 = 0;
static int iHighH = 10;
static int iLowS = 100;
static int iHighS = 255;
static int iLowV = 100;
static int iHighV = 255;
// // 绿色物体
// static int iLowH = 40;
// static int iHighH = 80;
// static int iLowS = 50;
// static int iHighS = 255;
// static int iLowV = 50;
// static int iHighV = 255;
void Cam_RGB_Callback(const sensor_msgs::Image msg)
{
//ROS_INFO("Cam_RGB_Callback");
cv_bridge::CvImagePtr cv_ptr;
try
{
// 将ROS图像消息转换为OpenCV图像
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图片转换成HSV
Mat imgHSV;
vector<Mat> hsvSplit;
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
//在HSV空间做直方图均衡化
split(imgHSV, hsvSplit);
equalizeHist(hsvSplit[2],hsvSplit[2]);
merge(hsvSplit,imgHSV);
Mat imgThresholded;
//使用上面的Hue,Saturation和Value的阈值范围对图像进行二值化
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();
//printf("w= %d h= %d size = %d\n",nImgWidth,nImgHeight,nImgChannels);
for (int y = 0; y < nImgHeight; y++)
{
for(int x = 0; x < nImgWidth; x++)
{
//printf("%d ",imgThresholded.data[y*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, "demo_cv_hsv");
ros::NodeHandle nh;
ros::Subscriber rgb_sub = nh.subscribe("/usb_cam/image_raw", 1 , Cam_RGB_Callback);
ros::Rate loop_rate(30);
//生成图像显示和参数调节的窗口空见
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");
while( ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
}
修改CMakeLists.txt
添加以下内容:
add_executable(color src/color.cpp)
target_link_libraries(color ${catkin_LIBRARIES} ${OpenCV_INCLUDE_DIRS})
编译运行
编译
cd ~/catkin_ws
catkin_make
运行程序
roscore
rosrun usb_cam usb_cam_node
rosrun cv_image color
运行结果: