ROS Melodic 的cv_brigde只支持python2版本, python3 需要自己编译, 比较麻烦。 可以直接使用sensor_msg.Image
直接传输
Publish
将opencv图像转为sensor_msg
img_msg.data = np.array(cv2.cvtColor(QueryImg, cv2.COLOR_BGR2RGB)).tostring()
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import cv2
import rospy
from sensor_msgs.msg import Image as Image_msg
import numpy as np
rospy.init_node("usb_cam_pub")
cam = cv2.VideoCapture(-1, cv2.CAP_V4L2)
# 图片话题
img_pub = rospy.Publisher('/cam_img', Image_msg, queue_size=1)
img_msg = Image_msg()
img_msg.header.stamp = rospy.Time.now()
img_msg.header.frame_id = 'world'
img_msg.encoding='rgb8'
while cam.isOpened() and not rospy.is_shutdown():
ret, QueryImg = cam.read()
if ret == True:
img_msg.header.stamp = rospy.Time.now()
img_msg.height, img_msg.width = QueryImg.shape[0], QueryImg.shape[1]
img_msg.data = np.array(cv2.cvtColor(QueryImg, cv2.COLOR_BGR2RGB)).tostring() # rviz 显示的格式rgb
img_pub.publish(img_msg)
cv2.imshow('QueryImage', QueryImg)
else:
print("NOT FRAME!!")
cv2.destroyAllWindows()
cam.release()
Subscribe
将sensor_msg 转为 opencv图像
img = np.ndarray(shape=(img_msg.height, img_msg.width, 3), dtype=np.uint8, buffer=img_msg.data)
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import numpy as np
from sensor_msgs.msg import Image as Image_msg
import rospy
def img_callback(img_msg):
"""
img_msg.header.stamp;
img_msg.height;
img_msg.width;
img_msg.encoding
"""
global start_time
img = np.ndarray(shape=(img_msg.height, img_msg.width, 3), dtype=np.uint8, buffer=img_msg.data)
print("--> cost time", time.time() - start_time)
start_time = time.time()
cv2.imshow('img', img)
cv2.waitKey(1)
global start_time
start_time = time.time()
if __name__ == "__main__":
rospy.init_node("sub_cam")
sub_cam = rospy.Subscriber('/cam_img', Image_msg, callback=img_callback, queue_size=1)
rospy.spin()
Note:
在sub和pub时, quene_size=1
, 将消息队列设为1,不然延时较为明显。