ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

友情提示：380元/半年，儿童学编程，就上码丁实验室。

1.10.1 简介OpenCV

OpenCV是开源计算机视觉库，由英特尔创建/维护，包含许多流行的计算机视觉算法的实现，高效且经过充分测试，专注于实时图像处理和2D + 3D计算机视觉。

参考链接：http://docs.opencv.org

（1）OpenCV的基本元素

Point, Point2f : 2D 点;

Size: 2D 尺寸结构;

Rect: 2D 矩形对象;

RotatedRect: 带角度的Rect对象，如下代码创建90×90 矩形：

cv::Rect(10, 10, 100, 100);

Mat: 图片对象；

如下代码创建2X2像素彩色图像，其中，CV_8UC3 表示我们使用8位的无符号字符类型，每个像素有3个通道。

cv::Mat M(2,2, CV_8UC3, cv::Scalar(0,0,255));

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图1 OpenCV图像对象示例

如下代码创建500X500像素灰度图像：

cv::Mat L(500,500, CV_8UC(1), cv::Scalar::all(0));

（2）ROI

感兴趣的图像区域(Image Region of Interest, ROI)。

cv::Mat mat = getCameraImage(); // allocate matrix
cv::Mat D (mat, cv::Rect(10, 10, 100, 100) ); // ROI using a rectangle

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图2 感兴趣的图像区域

（3）颜色空间

BGR: 默认颜色，通常包括3个颜色通道；

HSV: Hue, Saturation, Value的缩写，分别表示色度，饱和度，亮度， 3个频道；

GRAYSCALE: 灰度值，单通道；

OpenCV显示图像支持BGR或灰度格式，如下代码实现颜色空间转换：

cv::cvtColor(image, image, code)

其中，code: CV_<colorspace>2<colorspace>表示转换的方式，例如:

CV_BGR2GRAY；//BGR转换为GRAY
CV_BGR2HSV
CV_BGR2LUV

示例:

cv::cvtColor(mat_in, mat_out, CV_BGR2GRAY);    //change image to grayscale

灰度图与BGR图的存储：

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

（4）图像亮度

更改亮度是每个像素上的点操作，如果要增加亮度，则必须为每个像素添加一些常数。

增加亮度：

new_img (i, j) = img(i, j) + c;

降低亮度:

new_img (i, j) = img(i, j) – c;

将亮度增加75：

cv::Mat img; // Creating a matrix
img = img + cv::Scalar(75, 75, 75);

（5）计算机视觉基本技术

滤波：消除图像中的噪点；

分割：将图像划分为像素组，相似程度可根据强度、颜色、图案来决定；

特征检测：从图像中提取干星期的部分，包括边缘检测、角点检测，例如，在导航系统中，从图像中仅提取地板线条可用于定位等。

（6）内核

在图像处理中，许多运算符都是基于对局部窗口内的像素应用某些操作。“卷积运算符”（内核）是我们作为窗口内像素的加权平均来应用的函数。例如，如果窗口大小为3×3像素，我们可以通过提供3×3权重矩阵来定义函数。因此，在图像中每个像素的位置，我们通过3×3矩阵执行逐元素乘法再进行求和。该总和被视为该位置的输出值，不同的内核可能导致不同的效果，如图3所示。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图3 不同的内核具有不同的效果

OpenCV封装实现了各种各样前沿的计算机视觉算法，使用时非常方便。例如，如下代码演示了使用OpenCV检测办公椅的轮子，摄像机拍摄接近地面的场景，通过检测圆圈判断是否存在轮子。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

1.10.2 基于OpenCV 和 ROS的机器视觉应用

ROS传递图像时使用sensor_msgs/Image消息类型，ROS软件包cv_bridge提供了在sensor_msgs/Image消息与OpenCV使用的对象(cv::Mat)之间进行转换的功能，如图4所示。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图4 cv_bridge实现图像在ROS和OpenCV之间的转换

（1）获取图像

ROS使用sensor_msgs/Image消息类型发送的图像，我们的节点使用图像流，需要订阅相应图像发布话题。每个机器人都有自己发布/订阅图像消息的方式，名称可能会有所不同。使用rostopic list找出包含机器人的摄像机数据的话题，例如运行TurtleBot仿真程序，在命令行窗口输入rostopic list，将输出几十个话题，其中一些与图像有关。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

（2）将ROS消息转换为OpenCV图像

cv_bridge定义一个CvImage类，其中包含ROS标头、编码和OpenCV图像。CvImage包含了sensor_msgs/Image需要的信息，因此，我们可以方便地在两种形式之间互换。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

转换ROS的sensor_msgs/Image 消息为CvImage时，CvBridge可以识别两种不同的情况：

修改数据，必须复制ROS消息数据；
不修改数据，可以安全地共享ROS消息所拥有的数据，而不必进行复制。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

（3）将OpenCV图像转换为ROS消息

要将CvImage转换为ROS图像消息，使用toImageMsg()成员函数：

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

（4）ROS节点示例

文中的代码已放在[github](https://github.com/LinHuican/ros_tutorial/tree/master/lecture1-10)。

我们将创建一个订阅ROS图像消息话题的节点，将该图像转换为cv :: Mat，在其上绘制一个圆并使用OpenCV显示该图像。首先，创建一个具有以下依赖关系的程序包：

$ cd ~/catkin_ws/src
$ catkin_create_pkg image_converter std_msgs roscpp sensor_msgs cv_bridge image_transport

在/src文件夹中添加以下内容，创建一个ImageConverter类。

ImageConverter.h

#include <ros/ros.h>
#include <image_transport/image_transport.h>
class ImageConverter {
private:
   ros::NodeHandle nh;
   image_transport::ImageTransport imageTransport;
   image_transport::Subscriber imageSubscriber;
   void imageCallback(const sensor_msgs::ImageConstPtr& msg);
public:
   ImageConverter();
   virtual ~ImageConverter();
};

ImageConverter.cpp

#include "ImageConverter.h"
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
static const std::string OPENCV_WINDOW = "Image window";
ImageConverter::ImageConverter() : imageTransport(nh) {
   imageSubscriber = imageTransport.subscribe("/camera/rgb/image_raw", 1,
           &ImageConverter::imageCallback, this);
   // Create a display window
   cv::namedWindow(OPENCV_WINDOW);
}
ImageConverter::~ImageConverter() {
   // Close the display window
   cv::destroyWindow(OPENCV_WINDOW);
}void ImageConverter::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
   // convert the ROS image message to a CvImage
   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;
  }
   // Draw an example circle at the center of the video stream
   if (cv_ptr->image.rows > 50 && cv_ptr->image.cols > 50) {
       int cx = cv_ptr->image.cols / 2;
       int cy = cv_ptr->image.rows / 2;
       cv::circle(cv_ptr->image, cv::Point(cx, cy), 25, CV_RGB(255, 0, 0));
  }
   // Update the GUI window
   cv::imshow(OPENCV_WINDOW, cv_ptr->image);
   cv::waitKey(3);
}

main.cpp

#include "ImageConverter.h"int main(int argc, char **argv) {
   ros::init(argc, argv, "image_converter");
   ImageConverter imageConverter;
   ros::spin();
   return 0;
}

/launch目录下添加image_converter.launch文件

<launch> 
   <param name="/use_sim_time" value="true" />  
   <!-- Launch turtle bot world -->
   <include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch"/>         <!-- Launch image converter node -->
   <node name="image_converter" pkg="image_converter" type="image_converter" output="screen"/>
</launch>

此外，还需要对CMakeLists.txt文件进行编辑，尤其需要注意的是需要链接OpenCV：

cmake_minimum_required(VERSION 2.8.3)
project(image_converter)## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
roscpp
sensor_msgs
std_msgs
)## SySTEM dependencies are found with CMake's conventions
find_package(OpenCV)## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
 ${catkin_INCLUDE_DIRS}
)## Declare a C++ executable
add_executable(image_converter src/ImageConverter.cpp src/main.cpp)## Specify libraries to link a library or executable target against
target_link_libraries(image_converter
  ${catkin_LIBRARIES}
  ${OpenCV_LIBS}
)

catkin_make编译上述程序：

cd ~/catkin_ws/
catkin_make

启动image_converter:

roslaunch image_converter image_converter.launch

通过键盘操控机器人运动，观察效果，效果如图5所示。

roslaunch turtlebot_teleop keyboard_teleop.launch

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图5 image_converter效果

1.10.3 编程实现寻迹机器人

现在，我们来创建一个寻迹机器人，该机器人可以使用摄像机跟踪地面上的线条，能够检测和跟踪线条是自动驾驶所需的许多技能之一。为构建该系统，我们需要执行以下步骤：

从摄像机获取图像并将其传递给OpenCV；
过滤图像以识别我们要跟随的线条的中心；
移动机器人，使机器人的中心保持在直线的中心。

（1）创建gazebo仿真环境

通过如下命令创建一个名为follow_bot的程序包：

$ cd ~/catkin_ws/src
$ catkin_create_pkg follow_bot std_msgs roscpp sensor_msgs cv_bridge image_transport gazebo_ros

我们现在将在Gazebo环境中创建一条明亮的黄线，首先在follow_bot包中创建一个/models目录，然后将course.png和course.material复制到其中。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图6 course

course.material:

material course
{
receive_shadows on
technique
{
  pass
  {
    ambient 0.5 0.5 0.5 1.0
    texture_unit
    {
      texture course.png
    }
  }
}
}

然后创建/worlds目录，并将course.world复制到该目录

course.world:

<?xml version="1.0"?> 
<sdf version="1.4">
 <world name="default">
   <scene>
     <ambient>0 0 0 1</ambient>
     <shadows>0</shadows>
     <grid>0</grid>
     <background>0.7 0.7 0.7 1</background>
   </scene>
   <include>
     <uri>model://sun</uri>
   </include>
   <model name="ground">
     <pose>1 2.3 -.1 0 0 0</pose>
     <static>1</static>
     <link name="ground">
       <collision name="ground_coll">
         <geometry>
           <box>
             <size>10 10 .1</size>
           </box>
         </geometry>
         <surface>
           <contact>
             <ode/>
           </contact>
         </surface>
       </collision>
       <visual name="ground_vis">
         <geometry>
           <box>
             <size>10 10 .1</size>
           </box>
         </geometry>
         <material>
           <script>
             <uri>model://course.material</uri>
             <name>course</name>
           </script>
         </material>
       </visual>
     </link>
   </model>
 </world>
</sdf>

在/launch目录中添加course.launch文件：

<launch>
 <include file="$(find gazebo_ros)/launch/empty_world.launch">
   <arg name="use_sim_time" value="true"/>
   <arg name="debug" value="false"/>
   <arg name="world_name" value="$(find follow_bot)/world/course.world"/>
 </include> <include file="$(find turtlebot_gazebo)/launch/includes/kobuki.launch.xml">
   <arg name="base" value="kobuki"/>
   <arg name="stacks" value="hexagons"/>
   <arg name="3d_sensor" value="kinect"/>
 </include> <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
   <param name="publish_frequency" type="double" value="30.0" />
 </node> <!-- Fake laser -->
 <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
 <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
       args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
   <param name="scan_height" value="10"/>
   <param name="output_frame_id" value="/camera_depth_frame"/>
   <param name="range_min" value="0.45"/>
   <remap from="image" to="/camera/depth/image_raw"/>
   <remap from="scan" to="/scan"/>
 </node>
</launch>

编辑package.xml文件，添加导出，以便gazebo能够找到程序包中的模型路径”models”:

<export>
   <!-- You can specify that this package is a metapackage here: -->
   <!-- <metapackage/> -->   <!-- Other tools can request additional information be placed here -->
   <gazebo_ros gazebo_model_path="${prefix}/models"/>
</export>

运行如下命令，将看到图7的效果。

$ roslaunch follow_bot course.launch

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图7 gazebo加载course图像

（2）创建程序框架

接下来通过程序实现机器人的寻迹功能，在/src目录下添加Follower.h、Follower.cpp和main.cpp文件。

Follower.h

#include <ros/ros.h>
#include <image_transport/image_transport.h>
class ImageConverter {
private:
   ros::NodeHandle nh;
   image_transport::ImageTransport imageTransport;
   image_transport::Subscriber imageSubscriber;
   void imageCallback(const sensor_msgs::ImageConstPtr& msg);
public:
   ImageConverter();
   virtual ~ImageConverter();
};

Follower.cpp

#include "Follower.h"
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
static const std::string OPENCV_WINDOW = "Image window";
Follower::Follower(): imageTransport(nh) {
   imageSubscriber = imageTransport.subscribe("/camera/rgb/image_raw", 1,
           &Follower::imageCallback, this);
   // Create a display window
   cv::namedWindow(OPENCV_WINDOW);
}
Follower::~Follower() {
   // Close the display window
   cv::destroyWindow(OPENCV_WINDOW);
}void Follower::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
   // convert the ROS image message to a CvImage
   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;  } 
   // Update the GUI window
   cv::imshow(OPENCV_WINDOW, cv_ptr->image);
   cv::waitKey(3);
}

main.cpp

#include "Follower.h"int main(int argc, char **argv) {
   ros::init(argc, argv, "follower");
   Follower follower;
   ros::spin();
   return 0;
}

将follow_bot节点添加到启动文件course.launch：

<launch>
 <!-- Launch the follower node -->
 <node name="follow_bot" pkg="follow_bot" type="follow_bot" output="screen"/>
 <include file="$(find gazebo_ros)/launch/empty_world.launch">
   <arg name="use_sim_time" value="true"/>
   <arg name="debug" value="false"/>
   <arg name="world_name" value="$(find follow_bot)/world/course.world"/>
 </include>...</launch>

运行course.launch文件:

$ roslaunch follow_bot course.launch

Turtlebot机器人上的摄像机拍摄的图像如图8所示。

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图8 Turtlebot机器人上的摄像机拍摄的图像

（3）检测线条中心

上述代码文件创建了程序的框架，但是还不能完成机器人寻迹功能，下面讲解如何完成线条检测并跟随线条移动机器人。可以使用多种策略来检测和跟踪各种情况下的线条。幸运的是，在我们的案例中，我们仅考虑理想情况下的亮黄色线条。我们的策略是按颜色过滤图片的若干行，然后将机器人驱动到由颜色滤波获取的像素中心。

按颜色过滤

我们现在想在Turtlebot的图像流中找到黄线。问题描述：图像像素的RGB值是亮度和颜色的综合反应；解决方案：将RGB图像转换为色调、饱和度和值（HSV）图像。HSV图像将RGB分量分为色调（颜色）、饱和度（颜色强度）和值（亮度）。在HSV图像上，我们可以对接近黄色的色调应用阈值判断，以获得二进制图像，其中像素值为true（通过滤镜）或false（不通过滤镜）。

如下代码实现转换为HSV图像：

void ImageConverter::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
   // convert the ROS image message to a CvImage
   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;
  }
   // Convert input image to HSV
   cv::Mat image = cv_ptr->image;
   cv::Mat hsvImage;
   cv:cvtColor(image, hsvImage, CV_BGR2HSV);
   // Update the GUI window
   cv::imshow(OPENCV_WINDOW, hsvImage);
   cv::waitKey(3);
}

显示Turtlebot摄像机拍摄的HSV图像：

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图9_HSV Image

过滤黄色部分，代码实现如下所示：

void ImageConverter::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
   // convert the ROS image message to a CvImage
   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;
  }
   // Convert input image to HSV
   cv::Mat image = cv_ptr->image;
   cv::Mat hsvImage;
   cv:cvtColor(image, hsvImage, CV_BGR2HSV);   // Threshold the HSV image, keep only the yellow pixels
   cv::Mat mask;
   cv::Scalar lower_yellow(20, 100, 100);
   cv::Scalar upper_yellow(30, 255, 255);
   cv::inRange(hsvImage, lower_yellow, upper_yellow, mask);
   // Update the GUI window
   cv::imshow(OPENCV_WINDOW, mask);
   cv::waitKey(3);
}

通过色调(Hue)滤镜在HSV图像上获得过滤后的二进制图像:

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图10 过滤后的二进制图像

检测线条中心

当然，我们的目标是实现机器人跟随轨迹，而不仅仅是为其拍摄有趣的照片！要跟随轨迹，我们使用一种简便策略：我们仅检测图像从四分之三开始的20行，由于我们只关注摄像机视野中的线条轨迹部分，距机器人前方1米。

实现代码如下所示：

void ImageConverter::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
  ... 
   int width = mask.cols;
   int height = mask.rows;
   int search_top = 3 * height / 4;
   int search_bottom = search_top + 20;
   // Zero out pixels outside the desired region
   for (int y = 0; y < height - 2; y++) {
       if (y < search_top || y > search_bottom) {  
           for (int x = 0; x < width; x++) {
               mask.at<cv::Vec3b>(y, x)[0] = 0;
               mask.at<cv::Vec3b>(y, x)[1] = 0;
               mask.at<cv::Vec3b>(y, x)[2] = 0;
          }
      }
  } 
   // Update the GUI window
   cv::imshow(OPENCV_WINDOW, mask);
   cv::waitKey(3);
}

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图11 检测线条

现在，我们将使用OpenCV的 moments()函数来计算通过滤波器的二进制图像的斑点的质心，并绘制红色圆，实现代码如下所示，效果如图12所示。

void ImageConverter::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
  ... 
   // Use the moments() function to calculate the centroid of the blob of the binary image
   cv::Moments M = cv::moments(mask);
   if (M.m00 > 0) {
       int cx = int(M.m10 / M.m00);
       int cy = int(M.m01 / M.m00);
       cv::circle(image, cv::Point(cx, cy), 20, CV_RGB(255, 0, 0), -1);
  }    // Update the GUI window
   cv::imshow(OPENCV_WINDOW, image);
   cv::waitKey(3);
}

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图12 线条中心

（4）机器人跟随线条

现在，我们已经建立并实现了线条检测方案，我们可以继续执行驱动机器人的任务，使机器人跟随线条中心进行移动。我们将使用一个简单的比例控制器，这意味着误差的线性缩放驱动控制输出。

void ImageConverter::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
  ... 
   // Use the moments() function to calculate the centroid of the blob of the binary image
   cv::Moments M = cv::moments(mask);
    if (M.m00 > 0) {
       int cx = int(M.m10 / M.m00);
       int cy = int(M.m01 / M.m00);
       cv::circle(image, cv::Point(cx, cy), 20, CV_RGB(255, 0, 0), -1);
       // Move the robot in proportion to the error signal
       int err = cx - width / 2;
       geometry_msgs::Twist cmd;
       cmd.linear.x = 0.2;
       cmd.angular.z = -(float)err / 100;
       cmdVelPublisher.publish(cmd);
  }  
   // Update the GUI window
   cv::imshow(OPENCV_WINDOW, image);
   cv::waitKey(3);
}

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图13 机器人自主寻迹

以上所有代码已放在[github](https://github.com/LinHuican/ros_tutorial/tree/master/lecture1-10)。

练习

使用Turtlebot摄像机，控制机器人移向除尘器，并在距其约1m处停止

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

图14 练习

转自公众号：
Robot404

ROS教程_1.10 基于ROS和OpenCV的寻迹机器人

1.10.1 简介OpenCV

（1）OpenCV的基本元素

（3）颜色空间

（6）内核

1.10.2 基于OpenCV 和 ROS的机器视觉应用

（1）获取图像

（2）将ROS消息转换为OpenCV图像

（3）将OpenCV图像转换为ROS消息

（4）ROS节点示例

1.10.3 编程实现寻迹机器人

（1）创建gazebo仿真环境

（2）创建程序框架

（4）机器人跟随线条

练习

与本文相关的文章

您必须登录才能发表评论！

1.10.1 简介OpenCV

（1）OpenCV的基本元素

（3）颜色空间

（6）内核

1.10.2 基于OpenCV 和 ROS的机器视觉应用

（1）获取图像

（2）将ROS消息转换为OpenCV图像

（3）将OpenCV图像转换为ROS消息

（4）ROS节点示例

1.10.3 编程实现寻迹机器人

（1）创建gazebo仿真环境

（2）创建程序框架

（4）机器人跟随线条

练习

与本文相关的文章

您必须 登录 才能发表评论！

您必须登录才能发表评论！