ROS读取摄像头数据并发布

一、工作空间建立及运行详见上一篇

二、程序代码

#!/usr/bin/env python
import rospy
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image

class CameraPublisher:
    def __init__(self):
        # 初始化 ROS 节点
        rospy.init_node('camera_publisher', anonymous=True)

        # 创建 CvBridge 对象，用于 OpenCV 图像与 ROS 图像消息之间的转换
        self.bridge = CvBridge()

        # 配置摄像头参数
        self.laptop_camera_id = 0  # 笔记本摄像头设备编号
        #self.usb_camera_id = 1     # USB 摄像头设备编号

        # 打开摄像头
        self.laptop_camera = cv2.VideoCapture(self.laptop_camera_id)
        #self.usb_camera = cv2.VideoCapture(self.usb_camera_id)

        if not self.laptop_camera.isOpened():
            rospy.logerr("无法打开笔记本摄像头")
            exit(1)

        # if not self.usb_camera.isOpened():
        #    rospy.logerr("无法打开 USB 摄像头")
        #    exit(1)

        # 创建发布者，分别发布两个摄像头的图像数据到不同的话题
        self.laptop_camera_pub = rospy.Publisher('laptop_camera_image', Image, queue_size=10)
        #self.usb_camera_pub = rospy.Publisher('usb_camera_image', Image, queue_size=10)

        # 设置发布频率
        self.rate = rospy.Rate(30)  # 30Hz

    def publish_camera_images(self):
        while not rospy.is_shutdown():
            # 读取笔记本摄像头图像
            ret_laptop, frame_laptop = self.laptop_camera.read()
            if ret_laptop:
                # 将 OpenCV 图像转换为 ROS 图像消息并发布
                try:
                    ros_image_laptop = self.bridge.cv2_to_imgmsg(frame_laptop, "bgr8")
                    self.laptop_camera_pub.publish(ros_image_laptop)
                    rospy.loginfo("发布笔记本摄像头图像数据")
                except Exception as e:
                    rospy.logerr("转换或发布笔记本摄像头图像数据失败: %s", e)

            # 读取 USB 摄像头图像
            #ret_usb, frame_usb = self.usb_camera.read()
            #if ret_usb:
                # 将 OpenCV 图像转换为 ROS 图像消息并发布
             #   try:
             #       ros_image_usb = self.bridge.cv2_to_imgmsg(frame_usb, "bgr8")
             #       self.usb_camera_pub.publish(ros_image_usb)
             #       rospy.loginfo("发布 USB 摄像头图像数据")
             #   except Exception as e:
             #       rospy.logerr("转换或发布 USB 摄像头图像数据失败: %s", e)

            self.rate.sleep()

    def shutdown(self):
        # 释放摄像头资源
        self.laptop_camera.release()
        self.usb_camera.release()
        cv2.destroyAllWindows()

if __name__ == '__main__':
    try:
        camera_publisher = CameraPublisher()
        camera_publisher.publish_camera_images()
    except rospy.ROSInterruptException:
        pass
    finally:
        camera_publisher.shutdown()

三、rqt订阅图片

• 打开一个新的终端，运行 rqt 并添加图像视图插件：

  rqt

• 在 rqt 中，选择 "Plugins" -> "Visualization" -> "Image View"。

• 在图像视图插件中，分别订阅 /laptop_camera_image 和 /usb_camera_image 话题，即可查看摄像头图像。