ROS——发布摄像头节点并编写opencv图像处理节点（python）

软硬件环境

硬件

• 下位机：树莓派4B（4G）
• 上位机：PC
• USB摄像头

软件

• 上下位机都是Ubuntu 18.04系统
• ROS melodic

Opencv和ROS

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

我们只需要定义以下数据，即可发布图像数据。

发布摄像头数据节点

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

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

if __name__=="__main__":
    capture = cv2.VideoCapture(0) # 定义摄像头
    rospy.init_node('camera_node', anonymous=True) #定义节点
    image_pub=rospy.Publisher('/image_view/image_raw', Image, queue_size = 1) #定义话题

    while not rospy.is_shutdown():    # Ctrl C正常退出，如果异常退出会报错device busy！
        start = time.time()
        ret, frame = capture.read()
        if ret: # 如果有画面再执行
            # frame = cv2.flip(frame,0)   #垂直镜像操作
            frame = cv2.flip(frame,1)   #水平镜像操作   
    
            ros_frame = Image()
            header = Header(stamp = rospy.Time.now())
            header.frame_id = "Camera"
            ros_frame.header=header
            ros_frame.width = 640
            ros_frame.height = 480
            ros_frame.encoding = "bgr8"
            ros_frame.step = 1920
            ros_frame.data = np.array(frame).tostring() #图片格式转换
            image_pub.publish(ros_frame) #发布消息
            end = time.time()  
            print("cost time:", end-start ) # 看一下每一帧的执行时间，从而确定合适的rate
            rate = rospy.Rate(25) # 10hz 

    capture.release()
    cv2.destroyAllWindows() 
    print("quit successfully!")

图像接收和处理节点

我在这里只写了简单的接收图像并显示代码，可以在基本代码的基础上进行扩写即可。

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

import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
 
def callback(data):
    global bridge
    cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
    cv2.imshow("frame" , cv_img)
    cv2.waitKey(1)

if __name__ == '__main__':
    rospy.init_node('img_process_node', anonymous=True)
    bridge = CvBridge()
    rospy.Subscriber('/image_view/image_raw', Image, callback)
    rospy.spin()

效果展示：

参考文章：

• ROS操作系统 opencv-python读取摄像头+rviz可视化
• ROS系统中订阅者和发布者之间的延迟很大