【ROS】小车机器视觉巡线行驶

摄像头

USB摄像头是最普遍的摄像头，如笔记本内置的摄像头，在ROS中使用这类设备很简单，可以直接使用usb_cam功能包驱动，USB摄像头输出的是二维图像数据。
usb_cam是针对V4L协议USB摄像头的ROS驱动包，核心节点是usb_cam_nod
1、使用PC内置摄像头
安装usb_cam功能包

$ sudo apt-get install ros-melodic-usb-cam

运行

$ roslaunch usb_cam usb_cam-test.launch

报错：
在这里插入图片描述

ERROR: cannot launch node of type [image_view/image_view]: image_view
安装image-view

sudo apt-get install ros-kinetic-image-view

ERROR: Cannot identify ‘/dev/video0’: 2, No such file or directory
是因为虚拟机连接不上主机的摄像头
解决：
https://blog.csdn.net/qq_54253413/article/details/128599092
在这里插入图片描述

再次运行

$ roslaunch usb_cam usb_cam-test.launch

可以成功调用本地摄像头

2、调用外部USB摄像头
usb_cam安装，在工作空间中采用源代码安装：

$ cd catkin_ws/src  
$ git clone https://github.com/bosch-ros-pkg/usb_cam.git  
$ cd ..  
$ catkin_make

报错：
在这里插入图片描述

问题：- No package ‘libv4l2’ found
解决：

sudo apt-get install libv4l-dev

在这里插入图片描述

进入下载的包，找到usb_cam-test.launch或robot_vision中usb_cam.launch文件，修改里面的内容video0改成video1，保存并退出，source一下。
在这里插入图片描述

<launch><node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" ><param name="video_device" value="/dev/video1" /><param name="image_width" value="640" /><param name="image_height" value="480" /><param name="pixel_format" value="yuyv" /><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>

source devel/setup.bash

然后执行：

roslaunch usb_cam usb_cam-test.launch #开启摄像头
或
roslaunch robot_vision usb_cam.launch

OpenCV库

OpenCV库是一个基于BSD许可发行的跨平台开源计算机库，可以运行在Linux、Windows和mac OS等操作系统上。OpenCV由一系列C函数和少量C++类构成，同时提供C++、Python、Ruby、Matlab等语言的接口，实现了图像处理和计算机视觉方面的很多通用算法。
ROS开发者提供了与OpenCV的接口功能包——cv_bridge。如下图所示，开发者可以通过该功能包将ROS中的图像数据转换成OpenCV格式的图像，并且调用OpenCV库进行各种图像处理；或者将OpenCV处理过后的数据转换成ROS图像，通过话题发布，实现各节点之间的图像传输。
在这里插入图片描述

ROS中已经集成了OpenCV库和相关的接口功能包，使用如下命令即可安装：

sudo apt-get install ros-melodic-vision-opencv libopencv-dev python-opencv

或在catkin_ws/src目录下下载robot_vision安装包

git clone https://github.com/1417265678/robot_vision.git
cd ..
catkin_make

在这里插入图片描述

测试cv_bridge_test样例

$ roslaunch robot_vision usb_cam.launch
重新开启终端
$ catkin_make
$ source ./devel/setup.bash
$ rosrun robot_vision cv_bridge_test.py
再开启一个终端
$ rqt_image_view
没有的话就安装一下
sudo apt-get install ros-melodic-rqt-image-view

该例程中，一个ROS节点订阅摄像头驱动发布的图像消息，然后将其转换成OpenCV的图像格式进行显示，然后再将该OpenCV格式的图像转换成ROS图像消息进行发布并显示。
运行效果如下图所示，图像左边是通过cv_bridge将ROS图像转换成OpenCV图像数据之后的显示效果，使用OpenCV库在图像的左上角绘制了一个红色的圆；图像右边是将OpenCV图像数据再次通过cv_bridge转换成ROS图像后的显示效果，左右两幅图像背景应该完全一致。
在这里插入图片描述

巡线代码

新建scout_control_demo2.cpp，设置成可执行文件

代码

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>#include <tf/transform_datatypes.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>//速度控制话题消息类型类型
geometry_msgs::TwistStamped cmd_speed;
//
cv_bridge::CvImagePtr cv_ptr;
//小车x,y方向速度
double linear_x;
double linear_y;// th
//转速
double angular_z;void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);       
}int main(int argc, char **argv)
{ros::init(argc, argv, "scout_control_demo2");ros::NodeHandle n;// 发布话题 "/cmd_vel_raw"ros::Publisher pub = n.advertise<geometry_msgs::TwistStamped>("/cmd_vel_raw", 5);// 订阅话题ros::Subscriber image_sub_ = n.subscribe("/usb_cam/image_raw", 1, imageCallback);//配合r.sleep控制循环频率ros::Rate r(50);// 设置底盘运动速度，初始前进linear_x = 0.0;angular_z = 0.0;linear_y = 0.0;// 运动状态标识符int tag = 0;while(ros::ok()){  cmd_speed.twist.linear.x = linear_x;cmd_speed.twist.linear.y = linear_y;// thcmd_speed.twist.angular.z = angular_z;cv::Mat image = cv_ptr -> image;cv::Mat hsv = image.clone();cv::Mat res = image.clone();cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);//取颜色cv::inRange(hsv, cv::Scalar(0, 0, 20), cv::Scalar(180, 255, 150), res);int h = image.rows;int w = image.cols;cv::Moments M = cv::moments(res);if(M.m00 > 0){int cx = int (cvRound(M.m10 / M.m00));int cy = int (cvRound(M.m01 / M.m00));ROS_INFO("cx: %d cy: %d", cx, cy);cv::circle(cv_ptr->image, cv::Point(cx, cy), 20, (0, 255, 0));int v = cx - w / 2;linear_x = 0.1;angular_z = -float(v) / 300 * 0.3;
//?//image_pub_.publish(cv_ptr->toImageMsg());pub.publish(cmd_speed);ROS_INFO("linearx: %F,angularz: %F",linear_x,angular_z);}else{ROS_INFO("not found line!");linear_x =0;angular_z =0.2;pub.publish(cmd_speed);}//当处理到ros::spinonce()时，会去话题订阅缓冲区中查看有没有回调函数，如果有则去处理回调函数，如果没有则继续往下执行ros::spinOnce();r.sleep();}return 0;
}

cv::inRange() 函数在 OpenCV 中用于确定图像中像素值在指定范围内的区域，并将这些像素标记为白色（255），其他像素标记为黑色（0），结果存储在 res 中。
编译日常出错及解决

CMakeList
链接Opencv库文件
在这里插入图片描述

添加cv_bridge
在这里插入图片描述

在这里插入图片描述

运行实验


sudo ip link set can0 up type can bitrate 500000
cd catkin_ws/
终端1) 运行scout底盘节点对应的launch文件
roslaunch scout_bringup scout_robot_base.launch
终端2） 运行摄像头
source ./devel/setup.bash
roslaunch robot_vision usb_cam.launch
roslaunch usb_cam usb_cam-test.launch 
终端3） 运行我们编写的节点 
source ./devel/setup.bash
rosrun scout_base scout_control_demo2需要的话可以检查虚拟机连接的摄像仪编号
ls /dev/video*

报错：core dumped
在这里插入图片描述

问题分析：
在回调函数中处理图像时，赋值cv::Mat image = cv_ptr -> image;和其后的颜色转换步骤可能发生在图像显示之前，导致cv_ptr 可能为空值，产生了段错误（访问空指针）。
所以在对图像进行操作之前，需要确保它已经成功接收到并且不是空的。
这种并行处理也可能出问题：在回调函数中将图像显示在窗口中，并在回调函数之外尝试进行颜色空间转换。这样的处理方式可能导致在图像处理的同时，图像数据发生变化，从而导致颜色空间转换时出现问题。

问题解决：
将将颜色空间转换放到回调函数内部：
在这里插入图片描述

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>#include <tf/transform_datatypes.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>//速度控制话题消息类型类型
geometry_msgs::TwistStamped cmd_speed;
//
cv_bridge::CvImagePtr cv_ptr;
//小车x,y方向速度
double linear_x;
double linear_y;// th
//转速
double angular_z;
//image
cv::Mat image;
cv::Mat hsv;
cv::Mat res;
int h;
int w;
cv::Moments M;void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{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());}image = cv_ptr->image;hsv = image.clone();res = image.clone();cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);//取颜色cv::inRange(hsv, cv::Scalar(0, 0, 20), cv::Scalar(180, 255, 150), res);h = image.rows;w = image.cols;M = cv::moments(res);cv::imshow("camera_view", image);cv::waitKey(3);}int main(int argc, char **argv)
{ros::init(argc, argv, "scout_control_demo2");ros::NodeHandle n;// 发布话题 "/cmd_vel_raw"ros::Publisher pub = n.advertise<geometry_msgs::TwistStamped>("/cmd_vel_raw", 5);// 订阅话题ros::Subscriber image_sub_ = n.subscribe("/usb_cam/image_raw", 1, imageCallback);//配合r.sleep控制循环频率ros::Rate r(50);// 设置底盘运动速度，初始前进linear_x = 0.0;angular_z = 0.0;linear_y = 0.0;// 运动状态标识符int tag = 0;while(ros::ok()){  cmd_speed.twist.linear.x = linear_x;cmd_speed.twist.linear.y = linear_y;// thcmd_speed.twist.angular.z = angular_z;//image = cv_ptr->image;if(M.m00 > 0){int cx = int (cvRound(M.m10 / M.m00));int cy = int (cvRound(M.m01 / M.m00));ROS_INFO("cx: %d cy: %d", cx, cy);cv::circle(cv_ptr->image, cv::Point(cx, cy), 20, (0, 255, 0));int v = cx - w / 2;linear_x = 0.1;angular_z = -float(v) / 300 * 0.3;
//?//image_pub_.publish(cv_ptr->toImageMsg());pub.publish(cmd_speed);ROS_INFO("linearx: %F,angularz: %F",linear_x,angular_z);}else{ROS_INFO("not found line!");linear_x =0;angular_z =-0.2;pub.publish(cmd_speed);}//当处理到ros::spinonce()时，会去话题订阅缓冲区中查看有没有回调函数，如果有则去处理回调函数，如果没有则继续往下执行ros::spinOnce();r.sleep();}return 0;
}

实验结果

找不到什么问题，代码好像也没毛病，能够正常识别线条更改速度并显示在屏幕上但是驱动不了小车。可能是src内部某些文件出了问题吧。实验失败。。
这里是同学的代码，更换了他的src文件夹，能够正常运行。

#include<ros/ros.h>
#include<sensor_msgs/Image.h>
#include<geometry_msgs/Twist.h>
#include<cv_bridge/cv_bridge.h>
#include<opencv2/opencv.hpp>
#include<opencv2/imgproc.hpp>
#include<opencv2/imgproc/types_c.h>
#include<opencv2/core/core.hpp>double twist_linear_x , twist_angular_z;				// two kinds speedsensor_msgs::Image hsv_image;						//svoid image_Callback(const sensor_msgs::Image& msg);int main(int argc, char **argv){ros::init(argc, argv, "follower_line");			// init noteros::NodeHandle nh;ros::Subscriber img_sub = nh.subscribe("/usb_cam/image_raw", 10, image_Callback); 	// 更改为订阅 /usb_cam/image_raw 订阅者img_sub来接收来自USB摄像头的原始图像,and image_Callbackros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);	// 分别用于发布	小车的速度指令	和	处理后的图像。		ros::Publisher img_pub = nh.advertise<sensor_msgs::Image>("/image_hsv",10);while(ros::ok()){geometry_msgs::Twist twist;twist.linear.x = twist_linear_x;twist.angular.z = twist_angular_z;cmd_pub.publish(twist);img_pub.publish(hsv_image);ros::spinOnce();}return 0;
}void image_Callback(const sensor_msgs::Image& msg){// 当从摄像头接收到图像时，函数触发, public speed cmdcv_bridge::CvImagePtr cv_ptr;							// 确保使用正确的图像编码try {cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);		// 使用cv_bridge将ROS的图像消息转换为OpenCV的图像格式} catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());return;}cv::Mat image = cv_ptr->image;			// 原始图像cv::Mat hsv = image.clone();			// 用于后续的HSV转换cv::Mat res = image.clone();			// 用于存储颜色过滤后的结果 (keep medium)cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);	// 颜色空间转换cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(180, 255, 46), res);		// 颜色过滤 -> res// show/*cv::imshow("Filtered Image", res);  // 显示过滤后的图像cv::waitKey(1);  // 等待1毫秒以更新窗口*/// 处理逻辑// origin imageint h = image.rows;int w = image.cols;// search windowint search_top = 5 * h / 6;int	search_bot = search_top + 20;for(int i = 0; i < search_top; i ++){for(int j = 0; j < w; j ++){res.at<uchar>(i,j) = 0;			// set = 0 ,if not in search window}}for(int i = search_bot; i < h; i++){for(int j = 0; j < w; j ++){res.at<uchar>(i,j) = 0;			// set = 0 ,if not in search window}}cv::Moments M = cv::moments(res);			// 图像矩if(M.m00 > 0){int cx = int (cvRound(M.m10 / M.m00));int cy = int (cvRound(M.m01 / M.m00));// center in imageROS_INFO("cx: %d cy: %d", cx, cy);cv::circle(image, cv::Point(cx, cy), 10, (255, 255, 255));// set speed // 假设摄像头是再中间的int v = cx - w / 2;twist_linear_x = 0.1;twist_angular_z = -float(v) / 300 * 0.4;//cmd_pub.publish(twist);} else{ROS_INFO("not found line!");twist_linear_x = 0;twist_angular_z = -0.1;//cmd_pub.publish(twist);}// line's center,in image sensor_msgs::ImagePtr hsv_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();hsv_image = *hsv_image_;
}