ROS Kinetic + OpenCV 同一节点中订阅、发布图像

OpenCV基于ROS处理图像时经常需要在同一个节点中实现订阅和发布，像函数有输入输出一样，所以这样一个节点需要经历（1）订阅Topic、（2）ROS类型（sensor_msgs/Image）转换为Mat类型、（3）OpenCV处理图像、（4）Mat类型转换为ROS类型、（5）发布Topic这样五个步骤。下面的源码定义了一个可以实现这些功能的类，供大家作为模板编写自己需要的代码。

#include  
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

class SubscribeAndPublish
{
private:
	image_transport::Publisher  pub;
	image_transport::Subscriber sub;

public:
	SubscribeAndPublish(ros::NodeHandle &nh, image_transport::ImageTransport &it)
	{
		pub = it.advertise("/the/topic/to/be/published",1);
		sub = it.subscribe("/the/topic/to/be/subscribed", 1, &SubscribeAndPublish::callback, this);
	}

	void thresholdImage(const sensor_msgs::ImageConstPtr& msg, sensor_msgs::ImageConstPtr& out)
	{
		//Receive the input and convert it from ros type to cv type 
		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
                Mat image_optput;
		//Do something using OpenCV with the input and generate the output...
	        /*...your code...*/
		//Convert the output from cv type to ros type and publish it
		out = cv_bridge::CvImage(std_msgs::Header(), "bgr8",image_optput).toImageMsg();
	}

	void callback(const sensor_msgs::ImageConstPtr& msg)
	{
		sensor_msgs::ImageConstPtr output;
		thresholdImage(msg,output);
		pub.publish(output);
	}
};//End of class SubscribeAndPublish
 
int main(int argc, char **argv)
{
	//Initiate ROS
	ros::init(argc, argv, "Node_Name");
	ros::NodeHandle nh;
	image_transport::ImageTransport it(nh);

	//cv::namedWindow("Image");
	//cv::startWindowThread();
	//Create an object of class SubscribeAndPublish that will take care of everything
	SubscribeAndPublish SAPObject(nh,it);

	ros::spin();
	//cv::destroyWindow("Image"); 
	return 0;
}

Tips：

    1. 根据输入输出图像格式改变关键字"bgr8"，对应格式如下：

mono8: CV_8UC1, grayscale image

mono16: CV_16UC1, 16-bit grayscale image

bgr8: CV_8UC3, color image with blue-green-red color order

rgb8: CV_8UC3, color image with red-green-blue color order

bgra8: CV_8UC4, BGR color image with an alpha channel

rgba8: CV_8UC4, RGB color image with an alpha channel

例如输出的image_output是灰度图，则改为“mono8”或“mono16”。

原文链接：Converting between ROS images and OpenCV images (C++) 

    2. 如果报错“error: invalid use of non-static member function”就在相应函数前加入关键字“static”。

    3. sub = it.subscribe("/the/topic/to/be/subscribed", 1, &SubscribeAndPublish::callback, this); //这一句在调用回调函数的时候名字要写全，不能只写callback，如果改了类的名称记得把这里也改掉；this不能省略。

    4. 非注释部分可以视为固定模板，只编辑/*...your code...*/部分即可。也就是说可以a.添加自定义成员数据和函数，b.编辑thresholdImage函数。其他部分不变。

    5. image_transport可能需要安装(Ubuntu 16.04下终端运行：sudo apt-get install ros-kinetic-image-transport-plugins)。