ROS通过话题发布订阅Image类型的视频帧（python）

前言：

    前段时间，学习了YOLO在Python下的实现，而且经过在原代码基础上添加部分简单函数，已经可以实现利用YOLO在Python下实现（1）检测图片中的物体，（2）检测本地视频，（3）调用摄像头实时检测。后来又需要利用Kinect实时检测，所以要用的ROS进行各脚本之间的通信。而ROS Image是我写程序时遇到的一个难点，在此记录下来，以供后期查阅和网友参考。

本文中，主要是关于OpenCV格式图片（或视频帧）和ROS数据格式图片（或视频帧）之间的转换。或者直白点书，通过ROS发送图片（Image）数据类型的消息（message）。

【文中代码由于TAB缩进量可能会出现缩进问题，敬请谅解。】

本文其实是为下一篇博文“YOLO在ROS下的使用”打下基础。

1、使用环境和平台

ubuntu 14.04+ python2.7+opencv2.4

2、示例代码

其实，下述代码完全可以在一个脚本中完成，而且不需要结合ROS；本文为了讲述通过ROS发送Image的方法，故而拆分开来。一个脚本中，只进行图像捕捉；另一个订阅之后，只进行图像现实。

(1)通过调用webcam捕捉视频，然后经过ROS的Topic发布出去：

#!/usr/bin/env python
#!coding=utf-8

#write by leo at 2018.04.26
#function: 
#1, Get live_video from the webcam.
#2, With ROS publish Image_info (to yolo and so on).
#3, Convert directly, don't need to save pic at local.

import rospy
from sensor_msgs.msg import Image
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
import sys


def webcamImagePub():
    # init ros_node
    rospy.init_node('webcam_puber', anonymous=True)
    # queue_size should be small in order to make it 'real_time'
    # or the node will pub the past_frame
    img_pub = rospy.Publisher('webcam/image_raw', Image, queue_size=2)
    rate = rospy.Rate(5) # 5hz

    # make a video_object and init the video object
    cap = cv2.VideoCapture(0)
    # define picture to_down' coefficient of ratio
    scaling_factor = 0.5
    # the 'CVBridge' is a python_class, must have a instance.
    # That means "cv2_to_imgmsg() must be called with CvBridge instance"
    bridge = CvBridge()

    if not cap.isOpened():
        sys.stdout.write("Webcam is not available !")
        return -1

    count = 0
    # loop until press 'esc' or 'q'
    while not rospy.is_shutdown():
        ret, frame = cap.read()
        # resize the frame
        if ret:
            count = count + 1
        else:
            rospy.loginfo("Capturing image failed.")
        if count == 2:
            count = 0
            frame = cv2.resize(frame,None,fx=scaling_factor,fy=scaling_factor,interpolation=cv2.INTER_AREA)
            msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
            img_pub.publish(msg)
            print '** publishing webcam_frame ***'	
        rate.sleep()

if __name__ == '__main__':
    try:
        webcamImagePub()
    except rospy.ROSInterruptException:
        pass

(2)通过ROS订阅Image类型的视频帧，然后在窗口显示出来：

#!/usr/bin/env python
#!coding=utf-8

#right code !
#write by leo at 2018.04.26
#function: 
#display the frame from another node.

import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2

def callback(data):
    # define picture to_down' coefficient of ratio
    scaling_factor = 0.5
    global count,bridge
    count = count + 1
    if count == 1:
        count = 0
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
        cv2.imshow("frame" , cv_img)
        cv2.waitKey(3)
    else:
        pass

def displayWebcam():
    rospy.init_node('webcam_display', anonymous=True)

    # make a video_object and init the video object
    global count,bridge
    count = 0
    bridge = CvBridge()
    rospy.Subscriber('webcam/image_raw', Image, callback)
    rospy.spin()

if __name__ == '__main__':
    displayWebcam()

3、代码解释（函数讲解）

从代码中可以看出：

from cv_bridge import CvBridge, CvBridgeError

导入了一个模块下的两个类，然后实例化一个对象：

    bridge = CvBridge()

接下来，调用该对象下的方法（函数）：

            msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
            img_pub.publish(msg)

发布信息的脚本（上程序（1）中）里，利用此方法将OpenCV类型的图片转化为ROS类型，然后通过话题发布出去；

然后：

        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")

订阅话题的脚本（上程序（2）中）里，利用此方法将订阅到的ROS类型的数据转化为OpenCV格式的图片，然后通过imshow函数在窗口显示出图像。

PS：上边的程序中，不论发布还是订阅，都可以跳过一些帧（通过改变count的值即可）。

4、参考链接

ROS初学者教程：

http://wiki.ros.org/cn/ROS/Tutorials

OpnCV-Python教程：

https://docs.opencv.org/3.0-beta/doc/py_tutorials/py_tutorials.html

Converting between ROS images and OpenCV images (Python)

https://answers.ros.org/question/226819/trouble-converting-cv2-to-imgmsg/

http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython

https://github.com/pirobot/ros-by-example/blob/master/rbx_vol_1/rbx1_vision/nodes/cv_bridge_demo.py

后记：

程序很简单，在我自己的电脑上完全可以实现，如有问题欢迎批评指正！

（PS：下一篇介绍“用yolo结合ROS识别视频的python代码实现”）

联系邮箱：2052383522@qq.com