ROS视觉和图像-ROS和OpenCV图像转换桥梁cv_bridge

    ROS用sensor_msgs/Image消息格式传输图像，但是很多用户希望用OpenCV来处理图像，这就需要做图像格式转化。 cv_bridge功能包提供了ROS图像和OpenCV图像转换的接口，建立了一座桥梁。

   本文创建了2个节点，一个是图像发布者节点用来从摄像头获取图像并发布ROS图像信息，另一个是图像转换节点（首先订阅ROS图像消息，然后转换成OpenCV图像格式经处理后，再次转化成ROS图像格式又发布出去）。

1）发布者节点程序如下：

<span style="font-size:14px;">#include <ros/ros.h>  
#include <image_transport/image_transport.h>  
#include <opencv2/highgui/highgui.hpp>  
#include <opencv2/imgproc/imgproc.hpp>  
#include <cv_bridge/cv_bridge.h>  
#include <sstream>

#if 0 /*读取图像文件Case*/
int main(int argc, char** argv)  
{  
	ros::init(argc, argv, "image_publisher");  
	ros::NodeHandle nh;  
	image_transport::ImageTransport it(nh);  
        /*主题<span style="color:#3333FF;">camera/image</span></span><span style="font-size:14px;">*/
	image_transport::Publisher pub = it.advertise("<span style="color:#3333FF;">camera/image</span>", 1); 
        /*OpenCv接口读取图像文件*/ 
	cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);  
        cv::waitKey(30);  
        /*转化OpenCV image成ROS图像msg*/
    sensor_msgs::ImagePtr msg = <strong>cv_bridge::CvImage</strong>(std_msgs::Header(), "bgr8", image).toImageMsg();  

    ros::Rate loop_rate(5);  
   while (nh.ok()) {  
	   pub.publish(msg);  
	   ros::spinOnce();  
	   loop_rate.sleep();  
   }  
}  

#else /*读取摄像头case*/

int main(int argc, char** argv)  
{  
	ros::init(argc, argv, "image_publisher");  
	ros::NodeHandle nh;  
	image_transport::ImageTransport it(nh);  
	image_transport::Publisher pub = it.advertise("camera/image", 1);


	std::istringstream video_sourceCmd(argv[1]);
	int video_source;

	if(!(video_sourceCmd >> video_source))
	{
		ROS_INFO("source cmd error\n");
		return 1;
	}

	cv::VideoCapture cap(video_source);

	if(!cap.isOpened())
	{
		ROS_INFO("open image device failed\n");
		return 1;
	}

	cv::Mat frame;  
    sensor_msgs::ImagePtr msg;  

    ros::Rate loop_rate(5);

   while (nh.ok()) { 
	   cap>>frame;

	   if(!frame.empty())
	   {
		   msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();  
		   pub.publish(msg);
	   }
	   ros::spinOnce();  
	   loop_rate.sleep();  
   }  
}  
#endif</span>

2)图像订阅者节点程序

<span style="font-size:14px;">#include "ros/ros.h"
#include "image_transport/image_transport.h"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/image_encodings.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>

namespace enc = sensor_msgs::image_encodings;

/*准备再次发布的图像显示到本窗口*/
static const char OUT_WINDOW[] = "Image Out";
/*读取订阅的图像并显示到本窗口*/
static std::string IN_WINDOW = "Image In";

class ImageConvertor
{
	ros::NodeHandle nh_;
	image_transport::ImageTransport it_;
	image_transport::Subscriber image_sub_;
	image_transport::Publisher image_pub_;
	
	public:
	ImageConvertor():it_(nh_){
		/*发布主题out*/
		image_pub_ = it_.advertise("<span style="color:#33FF33;">out</span>", 1);
                /*订阅主题camera/image*/
                image_sub_ = it_.subscribe("<span style="color:#3366FF;">camera/image</span>", 1, &ImageConvertor::ImageCb, this);
		cv::namedWindow(OUT_WINDOW, CV_WINDOW_AUTOSIZE);
		cv::namedWindow(IN_WINDOW, CV_WINDOW_AUTOSIZE);
	}

	~ImageConvertor()
	{
		cv::destroyWindow(IN_WINDOW);
		cv::destroyWindow(OUT_WINDOW);
	}

	void ImageCb(const sensor_msgs::ImageConstPtr& msg)
	{
		cv_bridge::CvImagePtr cv_ptr;

		try
		{
		     /*转化成CVImage*/
                     cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
		}

		catch (cv_bridge::Exception& e)
		{
			ROS_ERROR("cv_bridge exception is %s", e.what());
			return;
		}

		if(cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
		  cv::circle(cv_ptr->image, cv::Point(50,50), 10, CV_RGB(255,0,0));

		cv::imshow(IN_WINDOW, cv_ptr->image);
		cv::Mat img_out;

                /*convert RGB to GRAY*/
                cv::cvtColor(cv_ptr->image, img_out, CV_RGB2GRAY);
		cv::imshow(OUT_WINDOW, img_out);

		cv::waitKey(3);
                /*转化成ROS图像msg发布到主题out*/
		image_pub_.publish(cv_ptr->toImageMsg());
	}


};


int main(int argc, char** argv)
{
	ros::init(argc, argv, "image_convertor");
	ImageConvertor ic;
	ros::spin();
	
	return 0;
}</span>