ROS2写一个海康威视相机图片发布节点

本文使用的设备是海康威视相机，这是一个ROS2的节点，进行图片发布。可以直接使用OpenCV的VideoCapture()函数直接读取相机设备。如果是简单版本可以直接这样操作，在我结合目标检测算法后，简单版本无法直接奏效，出现了画面延迟的问题。原因应该是和读取和写入的速度不匹配造成的，由于VideoCapture是顺序读取，它会从内存中依次读取存入的图片，因此若read()的频率低于相机写入缓存的频率，则会造成缓存区溢出，报错。此时，可以使用队列queue的方法，开子线程实时读取图片内容并写入队列。在主线程中，若需要使用图片，则从队列中进行读取。

#! /home/jetson/.virtualenvs/py36/bin/python3
import rclpy  # Python Client Library for ROS2
from rclpy.node import Node  # Handles the creation of nodes
from sensor_msgs.msg import Image  # Image is the message type
from cv_bridge import CvBridge  # Package to convert between ROS2 and OpenCV Images
import cv2  # OpenCV Library
import time
# import multiprocessing as mp
from queue import Queue
from threading import Thread


class HikVision(object):
    # set rtsp url
    _defaults = {
        "url": "rtsp://admin:rjc123456@192.168.2.64:554/h264/ch1/main/av_stream"
    }

    @classmethod
    def get_defaults(cls, n):
        if n in cls._defaults[n]:
            return cls._defaults[n]
        else:
            return "Unrecognized attribute name '" + n + "'"

    def __init__(self, **kwargs):
        # update parameters
        self.__dict__.update(self._defaults)
        self.__dict__.update(**kwargs)
        # open cam
        self.cap = cv2.VideoCapture(self.__dict__['url'])
        # self.cap.set(cv2.CAP_PROP_FPS, 5)

    def get_image(self):
        # get an image 
        try:
            #ret = self.cap.get(cv2.CAP_PROP_POS_FRAMES)
            ret, frame = self.cap.read()
            return ret, frame
        except Exception as e:
            # handle exception when cam is not connected or other reason
            self.cap.release()
            ret, frame = False, None
            return ret, frame


def image_put(q):
    # 初始化相机
    cam = HikVision()
    while True:
        try:        
            ret, frame = cam.get_image()
            if ret:
                q.put(frame)
                q.get() if q.qsize() > 1 else time.sleep(0.01)
            else:
                cam = HikVision()
        except Exception as e:
            cam = HikVision()
     

class HikVisionNode(Node):
    """
    Create a node for hikvision camera to capture images
    """

    def __init__(self, name):
        super().__init__(name)
        self.get_logger().info("Node for HikVision to capture images：%s" % name)
        # create and init a publisher
        self.publisher_ = self.create_publisher(Image, 'hikcam', 10)
        # open cam threading
        self.queue = Queue(maxsize=2)
        self.th_cam = Thread(target=image_put, args=(self.queue,))
        self.th_cam.start()
        # publish a message every i seconds
        timer_period = 0.5  # seconds
        # create the timer
        self.timer = self.create_timer(timer_period, self.timer_callback)
        # used to convert between ROS2 and OpenCV images
        self.br = CvBridge()

    def timer_callback(self):
        """
        Callback function.
        This function gets called every 1 seconds
        """
        timestr = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(time.time()))
        if self.queue.qsize() >= 1:
            frame = self.queue.get()
            self.publisher_.publish(self.br.cv2_to_imgmsg(frame))
            self.get_logger().info(timestr + ' Publishing a frame')
        else:
            self.get_logger().info(timestr + ' Can not publish a frame')

    def destroy_node(self) -> bool:
        self.th_cam.setDaemon(True)
        return super().destroy_node()


def main(args=None):
    """
    The entry function of the camera node that ROS2 runs
    """
    rclpy.init(args=args)  # initial rclpy
    node = HikVisionNode("hikvision")  # create a new node

    rclpy.spin(node)  # keep node running，and check whether an exit command is received (Ctrl+C)

    node.destroy_node()  # destroy the node explicitly
    rclpy.shutdown()  # shut down rclpy

这个脚本的主要功能是从HikVision相机中捕获图像，并将它们以ROS 2消息的形式发布出去。捕获和发布是在不同的线程中进行的，以确保相机图像的连续性。ROS 2节点通过定时器触发图像发布操作。当不再需要时，节点可以被销毁，ROS 2也被关闭。

HikVision类：这是一个用于连接HikVision相机的类，它包含了相机的默认参数和方法。

image_put函数：这是一个线程函数，负责初始化相机并持续获取图像帧，然后将图像帧放入队列中。

HikVisionNode类：这是一个自定义的ROS 2节点类，用于捕获图像并发布为ROS 2消息。

timer_callback函数：这是一个回调函数，它在固定的时间间隔内被调用，用于发布图像帧。

main函数：这是程序的入口点，初始化ROS 2，创建HikVisionNode节点，然后开始ROS 2的消息循环。