ROS2 python编写 intel realsense D405相机节点通过launch.py启动多个相机并发送图像话题，基于pyrealsense2库

最新推荐文章于 2025-06-04 16:29:54 发布

原创

最新推荐文章于 2025-06-04 16:29:54 发布 · 638 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#数码相机

环境配置

ROS2 humble

安装rviz工具:(用于可视化测试我们的脚本)

sudo apt install ros-humble-rviz-visual-tools
ros2 run rviz2 rviz2

文件总体结构树

在工作空间的src/目录下创建了dobot_camera功能包，在该功能包目录下创建了dobot_camera/文件夹用于存放功能包源码及驱动依赖，其中dobot_camera.py为功能包节点源码，定义了一个节点初始化并不断接收一个相机的图像帧并往外发送数据；camera.py及realsense_camera.py是白嫖的越疆机器人 xTrainer基于pyrealsense2的相机驱动代码。

在该功能包目录下创建了launch/文件夹用于存放*.launch.py文件于py/文件夹（注意是放在src/dobot_camera/目录下，不是放在工作空间目录下）

下面给出各个脚本及setup.py文件配置（安装编译后的文件到install/目录下可以通过ros2 launch 、ros2 run等命令识别）

camera.py

from pathlib import Path
from typing import Optional, Protocol, Tuple

import numpy as np


class CameraDriver(Protocol):
    """Camera protocol.

    A protocol for a camera driver. This is used to abstract the camera from the rest of the code.
    """

    def read(
        self,
        img_size: Optional[Tuple[int, int]] = None,
    ) -> Tuple[np.ndarray, np.ndarray]:
        """Read a frame from the camera.

        Args:
            img_size: The size of the image to return. If None, the original size is returned.
            farthest: The farthest distance to map to 255.

        Returns:
            np.ndarray: The color image.
            np.ndarray: The depth image.
        """


class DummyCamera(CameraDriver):
    """A dummy camera for testing."""

    def read(
        self,
        img_size: Optional[Tuple[int, int]] = None,
    ) -> Tuple[np.ndarray, np.ndarray]:
        """Read a frame from the camera.

        Args:
            img_size: The size of the image to return. If None, the original size is returned.
            farthest: The farthest distance to map to 255.

        Returns:
            np.ndarray: The color image.
            np.ndarray: The depth image.
        """
        if img_size is None:
            return (
                np.random.randint(255, size=(480, 640, 3), dtype=np.uint8),
                np.random.randint(255, size=(480, 640, 1), dtype=np.uint16),
            )
        else:
            return (
                np.random.randint(
                    255, size=(img_size[0], img_size[1], 3), dtype=np.uint8
                ),
                np.random.randint(
                    255, size=(img_size[0], img_size[1], 1), dtype=np.uint16
                ),
            )


class SavedCamera(CameraDriver):
    def __init__(self, path: str = "example"):
        self.path = str(Path(__file__).parent / path)
        from PIL import Image

        self._color_img = Image.open(f"{self.path}/image.png")
        self._depth_img = Image.open(f"{self.path}/depth.png")

    def read(
        self,
        img_size: Optional[Tuple[int, int]] = None,
    ) -> Tuple[np.ndarray, np.ndarray]:
        if img_size is not None:
            color_img = self._color_img.resize(img_size)
            depth_img = self._depth_img.resize(img_size)
        else:
            color_img = self._color_img
            depth_img = self._depth_img

        return np.array(color_img), np.ar