在Gazebo仿真中使用OpenCV获取ROS机器人的视觉图像

 

没有机器人，如何学习ROS

作为ROS机器人视觉编程的开篇实验，这次将带同学们完成一个基本功能：在ROS系统中获取机器人的视觉图像。在这个实验里，我们将了解图像数据是以什么形式存在于ROS系统中，以及如何转换成我们熟悉的OpenCV格式，为后续的视觉编程实验奠定基础。

[实验目标]

• 从ROS机器人的头部相机Topic中获取实时的视觉图像；

• 将获取的视觉图像转换成OpenCV格式，并显示在窗口程序中；

 

[源码下载]

ROS Kinetic (Ubuntu 16.04)

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

ROS Melodic (Ubuntu 18.04)

cd ~/catkin_ws/src/ git clone https://github.com/6-robot/wpr_simulation.git cd ~/catkin_ws/src/wpr_simulation/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_image_node.cpp”。

命名完毕后，在IDE的右侧可以开始编写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());         return;     }       Mat imgOriginal = cv_ptr->image;     imshow("RGB", imgOriginal);     waitKey(1); }   int main(intargc, char **argv) {     ros::init(argc, argv, "cv_image_node");        ros::NodeHandle nh;     ros::Subscriber rgb_sub = nh.subscribe("/kinect2/qhd/image_color_rect", 1 , Cam_RGB_Callback);       namedWindow("RGB");     ros::spin(); }

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

• ros.h是ROS的系统头文件；

• cv_bridge.h是ROS图像格式和OpenCV图像格式相互转换的函数头文件；

• image_encodings.h是图像数据编码格式头文件；

• imgproc.hpp是OpenCV的图像处理函数头文件；

• highgui.hpp是OpenCV里图像数据存储及显示函数的头文件；

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

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

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

(5)定义一个Mat类型的对象imgOriginal，从cv_ptr中获取图像数据。我们知道Mat是OpenCV中常用的图像数据类型，经过这步操作之后，我们就可以使用常规的OpenCV函数对imgOriginal进行处理，进入OpenCV的编程阶段。

(6)在本实验里，咱们先不对imgOriginal做太多的处理，只是调用imshow()函数将图像显示在一个名为“RGB”窗口程序中，这个窗口的初始化在后面的main函数里。

(7)调用waitKey(1)让程序停顿1毫秒，等待imshow()函数显示完成。

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

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

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

(11)namedWindow()是OpenCV的窗口初始化函数，通常与图像显示函数imshow()组合使用。这里初始化一个名为“RGB”的窗口，用来显示机器人相机获取的图像。图像的显示操作imshow()在前面定义的Cam_RGB_Callback回调函数里。

(12)调用ros::spin()挂起主函数，让回调函数（Cam_RGB_Callback）得以执行。如果没有这一句ros::spin()，主函数会立刻返回，整个程序执行完毕退出，回调函数也就不会被激活调用。

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

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

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

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

find_package(OpenCV REQUIRED)

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

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

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

cd catkin_ws/

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

catkin_make

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

2、下面我们需要启动运行这个cv_image_node节点的虚拟仿真环境。打开一个终端程序，键入如下命令：

roslaunch wpr_simulation wpb_balls.launch

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

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

rosrun cv_pkg cv_image_node

按下回车键，cv_image_node节点就启动起来了。此时会弹出一个名为“RGB”窗体程序，里面显示的是机器人头部相机所看到的四个颜色球的图像。（有的时候这个“RGB”窗体不会自动弹出，需要从Ubuntu左侧任务栏里点选其图标才能显示窗体）

4、为了测试这个图像是不是实时捕捉的，我们可以借助wpr_simulation附带的程序让中间的桔色球动起来，以便进行对比观察。在Ubuntu桌面再开一个新的终端程序，输入如下指令：

rosrun wpr_simulation ball_random_move

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

此时再切换到“RGB”窗口程序，可以看到图像中的桔色球也跟着运动，说明这个采集到的图像是实时更新的。

另外，还可以试试下面这些指令，让其他的颜色球也随机运动：

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

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

~/catkin_ws/src/wpr_simulation/src/demo_cv_image.cpp

其运行指令为：

rosrun wpr_simulation demo_cv_image

[实验结果]

通过这个实验，我们已经获得ROS机器人的视觉图像并转换成OpenCV的数据格式。在后续的实验中，我们将在ROS机器人上逐步施展OpenCV的魔法，带同学们体验各种视觉算法在机器人上产生的奇妙效果。

下期再见~

 

更多产品了解

欢迎扫码加入云巴巴企业数字化交流服务群

产品交流、问题咨询、专业测评

都在这里！