在Gazebo仿真中实现ROS机器人的目标跟随-云巴巴

立即咨询

立即试用

商务合作

首页

数字化社区

数字化选型

在Gazebo仿真中实现ROS机器人的目标跟随

2022-11-21

没有机器人，如何学习ROS

上一期实验，我们使用OpenCV实现机器人视觉中的颜色特征提取和目标定位功能，这一次我们根据目标位置计算速度并输出给机器人，完成一个目标跟随闭环控制。

[实验目标]

复习OpenCV颜色特征提取和目标定位知识；
根据目标位置计算机器人运动速度，完成目标跟随功能。

[源码下载]

ROS Kinetic (Ubuntu 16.04)

cd ~/catkin_ws/src/
git clone https://github.com/6-robot/wpr_simulation.git
git clone https://github.com/6-robot/wpb_home.git
cd ~/catkin_ws/src/wpr_simulation/scripts
./install_for_kinetic.sh
cd ~/catkin_ws/src/wpb_home/wpb_home_bringup/scripts
./install_for_kinetic.sh

ROS Melodic (Ubuntu 18.04)

cd ~/catkin_ws/src/
git clone https://github.com/6-robot/wpr_simulation.git
git clone https://github.com/6-robot/wpb_home.git
cd ~/catkin_ws/src/wpr_simulation/scripts
./install_for_melodic.sh
cd ~/catkin_ws/src/wpb_home/wpb_home_bringup/scripts
./install_for_melodic.sh

注：需将上述指令中的“catkin_ws”替换成实际的工作空间目录。

[实验步骤]

1.首先，新建一个ROS源码包Package，在Ubuntu里打开一个终端程序，输入如下指令进入ROS工作空间：

cd catkin_ws/src/

按下回车之后，即可进入ROS工作空间，然后输入如下指令新建一个ROS源码包：

catkin_create_pkg cv_pkg roscpp cv_bridge

这条指令的具体含义是：

指令	含义
catkin_create_pkg	创建ROS源码包（package）的指令
cv_pkg	新建的ROS源码包命名
roscpp	C++依赖项，本例程使用C++编写，所以需要这个依赖项
cv_bridge	将ROS中的图像数据格式转换成OpenCV数据格式的工具包

按下回车键后，可以看到如下信息，表示新的ROS软件包创建成功。

在VSCode中，可以看到工作空间里多了一个cv_pkg文件夹，在其src子文件夹上右键点击鼠标，选择“New File”新建一个代码文件。

新建的代码文件，我们为其命名为“cv_follow_node.cpp”。

命名完毕后，在IDE的右侧可以开始编写cv_follow_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>

#include <geometry_msgs/Twist.h>

usingnamespace cv;

usingnamespace std;

staticint iLowH = 10;

staticint iHighH = 40;

staticint iLowS = 90;

staticint iHighS = 255;

staticint iLowV = 1;

staticint iHighV = 255;

geometry_msgs::Twist vel_cmd; //速度消息包

ros::Publisher vel_pub; //速度发布对象

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());

return;

}

Mat imgOriginal = cv_ptr->image;

//将RGB图片转换成HSV

Mat imgHSV;

vector 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("横向宽度= %d 纵向高度= %d \n",nImgWidth,nImgHeight);

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),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);

//计算机器人运动速度

float fVelFoward = (nImgHeight/2-nTargetY)*0.002; //差值*比例

float fVelTurn = (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;

}

else

{

printf("目标颜色消失...\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);

printf("机器人运动速度( linear= %.2f , angular= %.2f )\n",vel_cmd.linear.x,vel_cmd.angular.z);

//显示处理结果

imshow("RGB", imgOriginal);

imshow("Result", imgThresholded);

cv::waitKey(1);

}

int main(intargc, char **argv)

{

ros::init(argc, argv, "cv_follow_node");

ros::NodeHandle nh;

ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect", 1 , Cam_RGB_Callback);

vel_pub = nh.advertise("/cmd_vel", 30);

ros::Rate loop_rate(30);

//生成图像显示和参数调节的窗口空见

namedWindow("Threshold", CV_WINDOW_AUTOSIZE);

cvCreateTrackbar("LowH", "Threshold", &iLowH, 179); //Hue (0 - 179)

cvCreateTrackbar("HighH", "Threshold", &iHighH, 179);

cvCreateTrackbar("LowS", "Threshold", &iLowS, 255); //Saturation (0 - 255)

cvCreateTrackbar("HighS", "Threshold", &iHighS, 255);

cvCreateTrackbar("LowV", "Threshold", &iLowV, 255); //Value (0 - 255)

cvCreateTrackbar("HighV", "Threshold", &iHighV, 255);

namedWindow("RGB");

namedWindow("Result");

while( ros::ok())

{

ros::spinOnce();

loop_rate.sleep();

}

(1)代码的开始部分，先include了六个头文件：

ros.h是ROS的系统头文件；
cv_bridge.h是ROS图像格式和OpenCV图像格式相互转换的函数头文件；
image_encodings.h是图像数据编码格式头文件；
imgproc.hpp是OpenCV的图像处理函数头文件；
highgui.hpp是OpenCV里图像数据存储及显示函数的头文件；
Twist.h是ROS速度控制消息包的定义头文件。

(2)“using namespace cv”表示引入“cv”这个函数空间，这样代码中调用的所有OpenCV函数都不用再带上空间名，直接写函数名即可。

(3)“using namespace std”表示引入“std”标准库的函数空间，后面会用到容器vector，可以不用带上空间名，保持简洁方便阅读。

(4)定义了六个变量，分别是HSV颜色空间的三个向量的阈值范围：

iLowH是色调（Hue）的下限值；
iHighH是色调（Hue）的上限值；
iLowS是颜色饱和度（Saturation）的下限值；
iHighS是颜色饱和度（Saturation）的上限值；
iLowV是颜色亮度（Value）的下限值；
iHighV是颜色亮度（Value）的上限值；

HSV即“色调、饱和度、亮度”，是在颜色检测领域常用的一种颜色空间，将传统的RGB（红绿蓝）图像转换到HSV空间后，可以很大程度上排除光照对颜色检测的影响，从而获得更好的颜色检测结果。HSV的颜色空间如下图：

(5)定义一个回调函数Cam_RGB_Callback，用来处理视频流的单帧图像。其参数msg为ROS里携带图像数据的结构体，机器人每采集到一帧新的图像就会自动调用这个函数。

(6)在Cam_RGB_Callback回调函数内部，我们使用cv_bridge的toCvCopy函数将msg里的图像转换编码成BGR8格式，并保存在cv_ptr指针指向的内存区域。

(7)将cv_ptr指针指向的图像数据复制到imgOriginal，使用cvtColor将其色彩空间转换成HSV，然后进行直方图均衡化，再用咱们设定的阈值进行像素二值化。二值化的目的是将图像中目标物（比如橘色的球）的像素标记出来以便计算位置坐标，二值化结果就是后面咱们会看到的黑白图像，黑的为0，白色为1，其中白色区域就是目标物占据的像素空间。

(8)二值化后使用开操作（腐蚀）去除离散噪点，再闭操作（膨胀）将前一步操作破坏的大连通域再次连接起来。

(9)最后遍历二值化图像的所有像素，计算出橘色像素的坐标平均值，即为该颜色物体的质心。对于圆形这样中心对称的形状物体，可以认为质心坐标就是目标物的中心坐标。这里调用了OpenCV的line()函数，在RGB彩色图像中的目标物质心坐标处，绘制了一个蓝色的十字标记。

(10)得到目标物中心坐标后，开始计算机器人跟随运动的速度值。这里我们假定机器人跟上目标时，目标物位于机器人视野图像的正中心，也就是横坐标为nImgWidth/2，纵坐标为nImgHeight/2。于是可以用目标物当前坐标值去减视野中心的坐标值，得到一组差值，将这组差值乘上比例系数，作为速度值输出给机器人，就可以控制机器人运动，让目标物在机器人视野中逐步趋近中心坐标，以此达到跟随目标的效果。在本例程序中，我们把纵坐标差值乘以一个系数0.002作为机器人前后移动的速度值，把横坐标差值乘以一个系数0.003作为机器人的旋转速度值，通过vel_pub发送给机器人执行。vel_pub的初始化在后面的main函数中。

(11)在Cam_RGB_Callback回调函数的末尾，使用imshow将原始的彩色图像和最后二值化处理过的图像显示出来。绘制了物体质心坐标的彩色图像显示在一个标题为“RGB”的窗口中，二值化后的黑白图像显示在一个标题为“Result”的窗口中。最后还需要调用一个cv::waitKey(1);延时1毫秒，让上面的两个图像能够刷新显示。

(12)接下来的main(int argc, char** argv)是ROS节点的主体函数，其参数定义和其他C++程序一样。

(13)main函数里，首先调用ros::init(argc, argv, "cv_follow_node")进行该节点的初始化操作，函数的第三个参数是本实验节点名称。

(14)接下来声明一个ros::NodeHandle对象nh，并用nh生成一个订阅对象rgb_sub，调用的参数里指明了rgb_sub将向主题“/kinect2/qhd/image_color_rect”订阅消息。机器人的摄像头启动后，会将图像数据源源不断的发布到这个主题上，这样我们的程序就能持续激活Cam_RGB_Callback回调函数。

(15)使用nh发布一个主题“/cmd_vel”,发布对象为前面定义的vel_pub。在回调函数中，我们通过vel_pub将机器人速度值发送到主题“/cmd_vel”中，机器人的核心节点会从这个主题获取速度值，驱动机器人进行运动。

(16)这里使用一个while(ros::ok())循环，以ros::ok()返回值作为循环结束条件可以让循环在程序关闭时正常退出。为了准确控制这个循环的运行周期，生成了一个ros::Rate的频率对象loop_rate，并在构造函数里赋初始值30，表示这个loop_rate对象会让while循环每秒钟循环30次。

(17)在进入while主循环前，先进行一些初始化，使用namedWindow初始化一个“Threshold”窗口。然后用cvCreateTrackbar给这个窗体添加六个滑动条，分别关联到程序开头声明的六个HSV的阈值变量，这样在运行的时候我们就可以用滑动条来实时调节这六个变量的数值大小。

(18)使用namedWindow创建“RGB”和“Result”两个窗体，在前面的回调函数里将会在这两个窗体里显示图像数据。

(19)在while循环中调用ros::spinOnce()函数给其他回调函数得以执行（本例程的回调函数是Cam_RGB_Callback）。

(20)调用loop_rate的sleep函数，确保while循环1秒钟循环30次。

程序编写完后，代码并未马上保存到文件里。这时可以看到VSCode界面右上编辑区的文件名“cv_follow_node.cpp”右侧有个白色小圆点，标示此文件并未保存。需要按下键盘组合键“Ctrl + S”，界面右上编辑区的文件名标题“cv_follow_node.cpp”右侧的白色小圆点变成了白色关闭按钮，此时代码文件才算保存成功。

完成保存操作后，还需要将源码文件添加到编译文件里才能进行编译。编译文件在cv_pkg的目录下，文件名为“CMakeLists.txt”。

在VSCode界面左侧工程目录中点击该文件，右侧会显示文件内容。对CMakeLists.txt的修改分为三个步骤：

(1)使用find_package()查找并引入OpenCV依赖包：

find_package(OpenCV REQUIRED)

(2)添加OpenCV的函数头文件目录路径：

(3)为cv_follow_node.cpp添加编译规则。内容如下：

add_executable(cv_follow_node

src/cv_follow_node.cpp

)

add_dependencies(cv_follow_node

${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(cv_follow_node

${catkin_LIBRARIES}

${OpenCV_LIBS}

)

同样，修改完需要按下键盘快捷键Ctrl+S进行保存，代码上方的文件名右侧的小白点会变成白色关闭按钮，说明保存文件成功。下面开始进行代码文件的编译，启动一个终端程序，键入如下命令进入ROS的工作空间：

cd catkin_ws/

然后再执行如下命令开始编译：

catkin_make

执行这条指令之后，会出现滚动的编译信息，直到出现“[100%] Built targetcv_follow_node”信息，此时新的cv_follow_node节点已经编译成功。

2、下面我们需要启动Gazebo仿真环境。打开一个终端程序，键入如下命令：

roslaunch wpr_simulation wpb_balls.launch

回车执行，会启动一个Gazebo窗口，一台ROS机器人面前摆着四个颜色球，机器人的头部相机俯视着这四个球。

3、运行我们编写的cv_follow_node节点。启动一个新的终端程序，输入以下指令：

rosrun cv_pkg cv_follow_node

按下回车键，cv_follow_node节点就启动起来了。此时会弹出三个窗口：

“RGB”窗体程序，里面显示的是机器人头部相机所看到的四个颜色球的图像。
“Threshold”窗体程序，里面显示的是当前使用的HSV颜色阈值，我们可以使用鼠标直接拖动窗体中的滑杆来改变阈值大小，在其他窗体中会实时显示阈值变化的效果。
“Result”窗体程序，里面显示的是转换颜色空间并二值化后的结果。白色的部分是检测到目标物的像素区域，黑色的部分是被剔除掉的非目标物的像素区域。

这时候我们切换到运行cv_follow_node节点的终端窗口，可以看到三组信息：

视频图像的横向宽度和纵向高度；
检测到的目标物的中心坐标值；
输出给机器人的速度控制信息。

此时机器人开始运动，在终端信息中，可以看到图像的纵向高度是540，中心点纵坐标为540÷2=270。而目标物的纵坐标为278，中间有个8个像素的差值，所以机器人的速度控制信息linear.x输出为8×0.002=0.016≈0.02，机器人向后退。在仿真环境里观察，确实是如此。

4、我们可以借助wpr_simulation附带的程序让中间的桔色球动起来，以观察机器人跟随目标的运动效果。在Ubuntu桌面再开一个新的终端程序，输入如下指令：

rosrun wpr_simulation ball_random_move

执行之后，可以看到Gazebo里的橘色球开始随机运动，机器人也开始跟随其运动。

5、实验中我们编写的cv_follow_node.cpp在wpr_simulation中有个对应的例程，同学们在遇到编译问题时可以在VSCode中打开这个例程源码文件进行对比参考，文件位置：

~/catkin_ws/src/wpr_simulation/src/demo_cv_follow.cpp

其运行指令为：

rosrun wpr_simulation demo_cv_follow

6、可以在“Threshold”窗体中改变目标颜色的HSV阈值范围，锁定其他颜色的目标球。然后使用下面这些指令，让其他的颜色球也随机运动，验证这种算法的普适性。

红色球	rosrun wpr_simulation ball_random_move red
绿色球	rosrun wpr_simulation ball_random_move green
蓝色球	rosrun wpr_simulation ball_random_move blue