ROS中订阅和发布视频中的图像消息

程序文件

imagepub.cpp

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

#include<opencv2/opencv.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>

#include<stdio.h>

using namespace cv;
using namespace std;

int main(int argc, char** argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle n; 
    ros::Time time = ros::Time::now();
    ros::Rate loop_rate(5);
    
     // 定义节点句柄   
    image_transport::ImageTransport it(n);
    image_transport::Publisher pub = it.advertise("video_image", 1);
    sensor_msgs::ImagePtr msg;
        
    // opencv准备读取视频
    cv::VideoCapture video;
    video.open("/home/vlt/Pictures/1.mp4");  

    if( !video.isOpened() )
    {
        ROS_INFO("Read Video failed!\n");
        return 0;
    }
    Mat frame;
    int count = 0;
    while(1)
    {
        video >> frame;
        if( frame.empty() )
            break;
        count++;
        
        msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
        pub.publish(msg);
        
        ROS_INFO( "read the %dth frame successfully!", count );
        loop_rate.sleep();
        ros::spinOnce();
    }
    video.release();
    
    return 0;
}

imagesub.cpp

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

#include<opencv2/opencv.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>

#include<stdio.h>
#include<math.h>
#include<vector>


void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
{
	ROS_INFO("Received \n");
	try{
        cv::imshow( "video", cv_bridge::toCvShare(msg, "bgr8")->image );
        cv::waitKey(30);
    }
    catch( cv_bridge::Exception& e )
    {
        ROS_ERROR( "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str() );
    }
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "image_listener");
	ros::NodeHandle n;
    cv::namedWindow("video");
    cv::startWindowThread();

    image_transport::ImageTransport it(n);
    image_transport::Subscriber sub = it.subscribe( "video_image", 1, imageCalllback );

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

需要修改的CMakeList.txt

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

include_directories(include ${catkin_INCLUDE_DIRS})
include_directories(include ${OpenCV_INCLUDE_DIRS})
 
add_executable(imagepub src/imagepub.cpp)
target_link_libraries(imagepub ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
#add_dependencies(mytest opencvexam_gencpp)

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

需要修改的地方，和之前是相同的，find_package中加上OpenCV，然后将对应的add_executable和target_link_libraries修改一下就可以了。

运行结果

发布

接收

看到的视频，实际上是彩色版，不知道为什么截图就成了黑白。

程序解读

image_transport::ImageTransport it(n);
image_transport::Publisher pub = it.advertise("video_image", 1);
sensor_msgs::ImagePtr msg;

这几句就是定义图像传输的句柄，发布者，还有消息，注意，advertise后面跟的第一个参数是话题的名称，第二个是缓冲区的大小（即消息队列的长度，在发布图像消息时消息队列的长度只能是1，因为我们期望算法处理的总是当前帧）。

msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();

这句话是将OpenCV格式的图像转化为ROS中的消息，需要借助cv_bridge来实现，std_msgs::Header()表示的是一些默认的头信息，"bgr8"是指需要编码的文件格式，frame则是需要转换的图像，msg就是转换之后的消息了。

cv::imshow( "video", cv_bridge::toCvShare(msg, "bgr8")->image );

这句话用来显示图像，cv_bridge::toCvShare(msg, “bgr8”)->image表示的是一副图像，它将ROS中的消息msg通过cv_bridge的toCvShare函数转换成OpenCV的图像格式，也是按照bgr8进行编码，结果是一个指针，加上->image就是图像了。这句话后面一定要加上cv::waitKey(30); 要不然无法显示接收到的图像。

image_transport::Subscriber sub = it.subscribe( "video_image", 1, imageCalllback );

这句话和发布话题是相对应的，同时，缓冲区的长度也设置成1。

参考博客

1：ROS中关于cv_bridge的介绍
2：在ROS中opencv发布和接收图像消息
3：ROS中使用opencv
4：Ros图像与Opencv图像的相互转换（C++）