ROS+Opencv ----- 读取图片并发布话题

最近看完了Opencv By Example这本书，感觉里面的案例很不错，其中有个案例是通过背景差分识别桌子上的螺钉螺母等机械零件，这对后期的机械臂视觉抓取提供了视觉处理部分的方法论，虽然有很多现成的ROS库可用于视觉抓取工作，但是都略显复杂，对传感器的要求也较高，所以想尝试使用opencv先进行简单处理。方向有了，接下来首要解决的问题当然就是ROS和Opencv结合。

ROS在安装的过程中就已经集成了Opencv3.0以上的版本，所以安装配置几乎不需要咱ROS用户过多考虑，ROS虽然无法直接进行图像处理，但是提供的cv_bridge可以完美转换和被转换图像数据格式。

但即使如此，想要完整的将opencv的项目移植到ROS项目中，也需要费一点功夫，接下来我会将Opencv By Example书中的代码转换成ROS中可用的程序，提供CMake和package.xml配置情况。

创建ROS包命名为redwall_arm_vision,里面只有三个文件，src目录和CMakeList.txt、package.xml，在src目录添加一张图片用于接下来的处理，我用的是大家非常熟悉的 lena.jpg:

在添加一个cpp文件，就命名为main.cpp把，程序内容如下：

#include <iostream>
#include <string>
#include <sstream>
using namespace std;

// OpenCV includes
#include <opencv2/video.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>

int main( int argc, char** argv )
{
    int sample;
    cout << "请输入0~7：" << endl;
    cin>>sample;
    switch(sample){
	case 0:
	{
            cout << "Sample 0, Mat zeros" << endl;
	    Mat m= Mat::zeros(5,5, CV_32F);
	    cout << m << endl;
            break;
	}
        case 1:
        {	
            cout << "Sample 0, Mat ones" << endl;
	    Mat m= Mat::ones(5,5, CV_32F);
	    cout << m << endl;
	    break;
	}
	case 2:
	{	
            cout << "Sample 0, Mat eye" << endl;
            Mat m= Mat::eye(5,5, CV_32F);
	    cout << m << endl;

	    Mat a= Mat::eye(Size(3,2), CV_32F);
	    Mat b= Mat::ones(Size(3,2), CV_32F);
	    Mat c= a+b;
	    Mat d= a-b;
            cout << "Sample 0, 矩阵元素和差" << endl;
	    cout << a << endl;
	    cout << b << endl;
	    cout << c << endl;
	    cout << d << endl;
	    break;
	}
	case 3:
	{	
            cout << "Sample 0, Mat operations:" << endl;
	    Mat m0= Mat::eye(3,3, CV_32F);
	    m0=m0+Mat::ones(3,3, CV_32F);
	    Mat m1= Mat::eye(2,3, CV_32F);
	    Mat m2= Mat::ones(3,2, CV_32F);

	    cout << "\nm0\n" << m0 << endl;
	    cout << "\nm1\n" << m1 << endl;
	    cout << "\nm2\n" << m2 << endl;

	    cout << "\nm1.*2\n" << m1*2 << endl;
	    cout << "\n(m1+2).*(m1+3)\n" << (m1+1).mul(m1+3) << endl;
	    cout << "\nm1*m2\n" << m1*m2 << endl;
	    cout << "\nt(m2)\n" << m2.t() << endl;
	    cout << "\ninv(m0)\n" << m0.inv() << endl;
	    break;
	}
	case 4:
	{
            Mat image= imread("/home/redwall/catkin_ws/src/redwall_arm_vision/src/lena.jpg", CV_LOAD_IMAGE_COLOR);
            int myRow=511;
            int myCol=511;
	    int val=*(image.data+myRow*image.cols*image.channels()+ myCol);
	    cout << "Pixel value: " << val << endl;
	    // 有imshow就会报段错误
	    // imshow("Lena", image);
	    break;
	}
	case 5:
	{
            Mat image= imread("/home/redwall/catkin_ws/src/redwall_arm_vision/src/lena.jpg", CV_LOAD_IMAGE_COLOR);
            int myRow=511;
	    int myCol=511;
	    int B=*(image.data+myRow*image.cols*image.channels()+ myCol + 0);
	    int G=*(image.data+myRow*image.cols*image.channels()+ myCol + 1);
	    int R=*(image.data+myRow*image.cols*image.channels()+ myCol + 2);
	    cout << "Pixel value (B,G,R): (" << B << "," << G << "," << R << ")" << endl;
	    break;
	}
	case 6:
	{
            ros::init(argc,argv,"image_color");
            ros::NodeHandle nh;    
            image_transport::ImageTransport it(nh);
            image_transport::Publisher pub = it.advertise("camera/image", 1);
            /**************ROS与Opencv图像转换***********************/
            Mat image= imread("/home/redwall/catkin_ws/src/redwall_arm_vision/src/lena.jpg", CV_LOAD_IMAGE_COLOR);
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
            ros::Rate loop_rate(5);
            while (nh.ok()) {
                pub.publish(msg);
                ros::spinOnce();
                loop_rate.sleep();
            }
	}

        case 7:
        {
            ros::init(argc,argv,"image_gray");
            ros::NodeHandle nh;
            image_transport::ImageTransport it(nh);
            image_transport::Publisher pub = it.advertise("camera/image", 1);
            /**************ROS与Opencv图像转换***********************/
            Mat gray= imread("/home/redwall/catkin_ws/src/redwall_arm_vision/src/lena.jpg", CV_LOAD_IMAGE_GRAYSCALE);
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", gray).toImageMsg();
            ros::Rate loop_rate(5);
            while (nh.ok()) {
                pub.publish(msg);
                ros::spinOnce();
                loop_rate.sleep();
            }
        }
    }

    return 0;

}

以上程序的case1到case5来源于书中的测试代码，可用看到Mat类是可用直接用的，只要包含了cv的头文件，这对于我们进行矩阵运算都是很方便的，并没有太大的区别，但是直接对图像进行处理的函数都会直接报错，例如

// 有imshow就会报段错误
imshow("Lena", image);
imwrite();
// 这个貌似也失效了
waitkey();

重点关注的是case6和case7，创建ROS节点，读取RGB三通道图像"bgr8"或者灰度单通道图像“mono8”，然后通过cv_bridge将图像转换成sensor_msgs话题发布在/camera/image消息下，1表示的是消息队列的长度。

CMakeLists.txt内容如下:

cmake_minimum_required(VERSION 2.8.3)
project(redwall_arm_vision)

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

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES redwall_arm_vision
   CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

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

add_executable(main src/main.cpp)
target_link_libraries(main ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

package.xml内容如下:

<?xml version="1.0"?>
<package format="2">
  <name>redwall_arm_vision</name>
  <version>0.0.0</version>
  <description>redwall_arm_vision</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="sijia@todo.todo">sijia</maintainer>

  <license>TODO</license>

  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>image_transport</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>cv_bridge</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>message_generation</build_export_depend>
  <build_export_depend>image_transport</build_export_depend>

  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>image_transport</exec_depend>

</package>

编译然后启动节点，输入数字6或者7，然后打开RViz添加image消息即可观察到图像：

$ catkin_make -DCATKIN_WHITELIST_PACKAGES="redwall_arm_vision"
$ roscore
$ rosrun redwall_arm_vision main
$ rviz