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

ROS1/一代机器人系统 少儿编程 253浏览 0评论
Ros教程01

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.hFollower.cppmain.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

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