【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;
	    // th
	    cmd_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;
	    // th
	    cmd_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 speed

sensor_msgs::Image hsv_image;						//s


void image_Callback(const sensor_msgs::Image& msg);



int main(int argc, char **argv){

    ros::init(argc, argv, "follower_line");			// init note
    ros::NodeHandle nh;

    ros::Subscriber img_sub = nh.subscribe("/usb_cam/image_raw", 10, image_Callback); 	// 更改为订阅 /usb_cam/image_raw 订阅者img_sub来接收来自USB摄像头的原始图像,and image_Callback

    ros::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 cmd

    cv_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 image
    int h = image.rows;
    int w = image.cols;
				
				// search window
    int 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 image
        ROS_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_;
}

参考资料

[1] https://zhuanlan.zhihu.com/p/370996539
[2 ]https://www.bilibili.com/read/cv14789297/