首先是publisher.py
中的代码
#encoding:utf-8
import cv2
import rclpy
import threading
import numpy as np
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import CompressedImage
from std_msgs.msg import String
cap = None
class NodePublisher(Node):
def __init__(self, node_name):
super().__init__(node_name)
self.get_logger().info("hello,my name is %s" % node_name)
def quit(data):
print("Receive Message={}".format(data.data))
if data.data == "quit":
# 释放摄像头资源
cap.release()
cv2.destroyAllWindows()
exit(0)
def run_camera(cap, image_pub):
bridge = CvBridge()
while cap.isOpened():
ret,frame = cap.read()
if ret:
frame = np.array(cv2.flip(frame, 1))
data = bridge.cv2_to_compressed_imgmsg(frame)
#data = bridge.cv2_to_imgmsg(frame,"bgr8")
image_pub.publish(data)
def main():
global cap
height = 640
width = 480
cap = cv2.VideoCapture(-1)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
rclpy.init()
node = NodePublisher("Camera")
image_pub = node.create_publisher(CompressedImage, "image_data", 10)
node.create_subscription(String, "client_quit", quit, 1)
t = threading.Thread(target=run_camera,args=(cap,image_pub))
t.start()
rclpy.spin(node)
rclpy.shutdown()
if __name__ == "__main__":
main()
接着是subscriber.py
中的代码
#encoding:utf-8
import cv2
import rclpy
import time
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CompressedImage
from std_msgs.msg import String
bridge = CvBridge()
pub = None
def callback(data):
cv_img = bridge.compressed_imgmsg_to_cv2(data,"bgr8")
#cv_img = bridge.imgmsg_to_cv2(data,"bgr8")
cv2.imshow("frame",cv_img)
key = cv2.waitKey(1000//20)
if (key & 0xFF) == ord("q"):
msg = String()
msg.data = "quit"
pub.publish(msg)
cv2.destroyAllWindows()
exit(0)
class NodeSubscribe(Node):
def __init(self, name):
super().__init__(name)
self.get_logger().info("hello,I am %s", name)
def main(args=None):
global pub
rclpy.init()
node = NodeSubscribe("image_node")
node.create_subscription(CompressedImage, "image_data", callback, 10)
pub = node.create_publisher(String,"client_quit", 1)
rclpy.spin(node)
rclpy.shutdown()
if __name__ == "__main__":
main()
使用线程的方式启动摄像头的捕获,再利用rclpy.spin
及rclpy.shutdown
让其在主循环中运行从而接收消息。