ros2双目摄像头驱动

流程：

• 从 USB 摄像头采集视频数据

• 对采集到的合成图像进行拆分，提取左右视图

• 将左右图像分别发布到 ROS2 话题

 设备：

• 树莓派5（ubuntu24.04系统）

• 支持usb的双目摄像头

节点代码 stereo_camera_node.py：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class StereoCameraPublisher(Node):
    def __init__(self):
        super().__init__('stereo_camera_publisher')
        self.get_logger().info("Initializing Stereo Camera Publisher Node")

        # 创建发布者：分别发布左图和右图
        self.left_pub = self.create_publisher(Image, 'left/image_raw', 10)
        self.right_pub = self.create_publisher(Image, 'right/image_raw', 10)
        self.bridge = CvBridge()

        # 仅打开 /dev/video0，因为该设备输出的是合成图像（左右并排）
        self.cap = cv2.VideoCapture(0, cv2.CAP_V4L2)

        if not self.cap.isOpened():
            self.get_logger().error("无法打开 /dev/video0")
        
        # 可选：设置摄像头分辨率（根据实际情况修改）
        # 例如：假设合成图像分辨率为 3840x1080，左右各 1920x1080
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

        # 设置定时器，每 0.1 秒读取一次图像（约 10Hz）
        self.timer = self.create_timer(0.1, self.timer_callback)

    def timer_callback(self):
        ret, frame = self.cap.read()
        if ret:
            height, width, _ = frame.shape
            # 假设左右图像水平并排，宽度各占总宽度的一半
            half_width = width // 2
            left_frame = frame[:, :half_width]
            right_frame = frame[:, half_width:]

            # 将 OpenCV 图像转换为 ROS2 Image 消息
            img_msg_left = self.bridge.cv2_to_imgmsg(left_frame, encoding="bgr8")
            img_msg_right = self.bridge.cv2_to_imgmsg(right_frame, encoding="bgr8")
            current_time = self.get_clock().now().to_msg()
            img_msg_left.header.stamp = current_time
            img_msg_right.header.stamp = current_time

            self.left_pub.publish(img_msg_left)
            self.right_pub.publish(img_msg_right)
        else:
            self.get_logger().warn("读取 /dev/video0 图像失败")

    def destroy_node(self):
        # 释放摄像头资源
        self.cap.release()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = StereoCameraPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('Keyboard Interrupt (SIGINT)')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

通过usb传输的图像走的是一个通道（在我的设备里是dev/video0），需要自行分割左右摄像头图像， 做这一步需要查询摄像头支持的分辨率和格式，使用以下命令：

v4l2-ctl --device=/dev/video0 --list-formats-ext

这个命令会列出该摄像头支持的所有像素格式、对应的分辨率以及每种分辨率下的帧率选项。

得到支持分辨率后在设置宽度高度的部分进行修改，宽度需要乘2，比如需要的是640*480的视频，就设置成1280*480，这样在左右分割后刚好是我们需要的分辨率。

最后编译运行节点后，就可以通过rqt_image_view包中的同名节点分别查看左右摄像头的画面了。