ROS-发布与订阅

首先是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()