在ROS中使用OpenCV进行图像处理，并发布处理结果

背景

博主想用Gazebo联合ROS以及Matlab对机械臂视觉伺服进行仿真。希望将Gazebo里面获取的sensor_msgs/Image格式图像转换为Opencv可以处理的cv::Mat格式，然后提取角点后将角点坐标发布，这样就可以用Matlab进行接收。这样的好处是Matlab不需要处理图像，只运行控制代码就行，减轻了各平台负担。

步骤

1 创建一个沟通ROS和Opencv的功能包“robot_vision”，负责图像格式转换（sensor_msgs/Image -> cv::Mat），图像特征提取与特征点坐标发布。

1. 1 新建功能包

//在工作空间下（假设工作空间名为catkin_ws）
cd src
catkin_create_pkg robot_vision roscpp std_msgs cv_bridge image_transport sensor_msgs
cd ..
catkin_make

1. 2 创建feature_extractor.cpp源文件

在1.1中新建功能包下的src子目录下创建feature_extractor.cpp并粘贴下面代码，其中注释已经很详细了，这里就不多解释了。注意，代码中涉及到有cv_bridge的知识点，可以学习这里.，另外，提取特征点采用的是opencv中著名的cv::goodFeaturesToTrack()函数与角点位置亚像素化函数cv::cornerSubPix()，其优点是计算快，并且可以指定最大检测角点数量（这对于想检测矩形的顶点等固定角点数情况是一个巨好的条件啊）关于这种角点检测方法的更多细节可以参考链接.。

#include<ros/ros.h> //ros标准库头文件
#include<iostream> //C++标准输入输出库
#include<cv_bridge/cv_bridge.h> 
#include<sensor_msgs/image_encodings.h> 
//image_transport 头文件用来在ROS系统中的话题上发布和订阅图象消息
#include<image_transport/image_transport.h> 

//发布坐标点消息需要用到的
#include <sstream>
#include "std_msgs/Float32MultiArray.h"

//opencv图像处理需要用到的
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <ctime>
#include <opencv2/opencv.hpp>
#include <math.h>
#include <string>
#include <fstream>

using namespace cv;
using namespace std;

Mat src, graysrc;//原图与灰度化图像
vector<Point2f>corners; //检测到的角点

//定义一个转换的类
class RGB_GRAY
{
public:
    ros::NodeHandle nh_; //定义ROS句柄
    image_transport::ImageTransport it_; //定义一个image_transport实例
    image_transport::Subscriber image_sub_; //声明ROS图象接收器
    ros::Publisher feature_cordinates_pub_; //声明发布器

    RGB_GRAY()
      :it_(nh_) //构造函数
    {
        //定义图象接受器，订阅话题是“camera/rgb/image_raw”
        image_sub_ = it_.subscribe("/firefly/simple_camera/image_raw", 1, &RGB_GRAY::convert_callback, this); 
        //定义角点坐标发布器，发布话题是“feature_cordinates”
        feature_cordinates_pub_ = nh_.advertise<std_msgs::Float32MultiArray>("feature_cordinates", 1000);
    }

    //消息回调函数，将图象格式从sensor_msgs/Image转换为cv::Mat，并发布角点坐标
    void convert_callback(const sensor_msgs::ImageConstPtr& msg) 
    {
        cv_bridge::CvImagePtr cv_ptr; // 声明一个CvImage指针的实例
 
        try
        {
            cv_ptr =  cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); 

        }
        catch(cv_bridge::Exception& e)  
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }
 
        image_process(cv_ptr->image); //得到了cv::Mat类型的图象，在CvImage指针的image中，将结果传送给处理函数   
    }

    //图象处理以及角点发布的主要函数
    void image_process(cv::Mat src) 
    {
     cv::cvtColor(src, graysrc, CV_BGR2GRAY);   //转换成灰度图象

     goodFeaturesToTrack(graysrc, corners, 4, 0.01, 10);//提取角点（非亚像素）

     //TermCriteria类是用来作为迭代算法的终止条件的，参数：类型(EPS表示迭代到阈值终止)，
     //第二个参数为迭代的最大次数，最后一个是特定的阈值
     TermCriteria tc = TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 40, 0.001);
     cornerSubPix(graysrc, corners, Size(5, 5), Size(-1, -1), tc);//亚像素定位，为后续计算提供更高精确度的值

     std_msgs::Float32MultiArray msg_;//存储4对角点坐标

     msg_.data.push_back(corners[0].x);
     msg_.data.push_back(corners[0].y);
     msg_.data.push_back(corners[1].x);
     msg_.data.push_back(corners[1].y);
     msg_.data.push_back(corners[2].x);
     msg_.data.push_back(corners[2].y);
     msg_.data.push_back(corners[3].x);
     msg_.data.push_back(corners[3].y);

     feature_cordinates_pub_.publish(msg_);//发布消息
    }

};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "RGB");
    RGB_GRAY obj;
    ros::spin();
}

1. 3 创建CMakeLists.txt文件

在建立的robot_vision的程序包中的CMakeLists.txt文件中加入如下代码（先把CMakeLists.txt中已有的内容全部删掉）。
（注意这里有一个隐藏的坑，很多人装了ROS以后没有单独装Opencv，虽然ROS自带Opencv，但若要单独使用这个Opencv，通过默认路径可能找不到Opencv，所以CMakeLists.txt需要加入一行 " set(OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev/) "以指定Opencv的路径，注意每个人的路径可能不同，一般在ROS的安装目录下）

cmake_minimum_required(VERSION 2.8.3)
project(robot_vision)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  OpenCV
  cv_bridge
  image_transport
)

generate_messages(
  DEPENDENCIES
  std_msgs
)


include_directories(
  include ${catkin_INCLUDE_DIRS}
  include ${OpenCV_INCLUDE_DIRS}
)

set(OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev/)

find_package( OpenCV REQUIRED )  
include_directories( ${OpenCV_INCLUDE_DIRS} )
add_executable(feature_extractor src/feature_extractor.cpp)  
target_link_libraries(feature_extractor ${catkin_LIBRARIES} ${OpenCV_LIBS})  
add_dependencies(feature_extractor robot_vision_generate_messages_cpp)  
 

1. 3 编译工作空间

//在catkin_ws下
catkin_make 

2 编写launch文件

步骤1中写的robot_vision功能包，可以在launch文件中通过节点来运行，不管你的launch文件里有别的什么东西，只要再多加一行：（type填写.cpp文件的名称，这里是feature_extractor.cpp，所以填feature_extractor，与节点名字相同）

<node name="feature_extractor" pkg="robot_vision" type="feature_extractor" /> 

然后就ok了，这时运行launch文件，步骤1中写的功能包就能生效

运行结果

我的gazebo中相机的刷新频率是20Hz，从下图可以看出检测到的4个角点为[15.48,177.11]，[37.73,169.77]，[24,147]，[195.31,219.78]，非常完美，哈哈哈哈哈！！！！！