Карты глубины Gemini 335L в ROS 2: от запуска драйвера до rviz2

Отлаживать восприятие робота по логам невозможно: нужно видеть, что именно камера отдаёт в пайплайн, где в данных дырки и как реагирует глубина на движение. Карта глубины Orbbec Gemini 335L — активной IR-стереокамеры с базовой линией 100 мм — это матрица uint16, где каждый пиксель кодирует расстояние в миллиметрах, а ноль означает «точка не измерена». В этом гайде мы подключим Gemini 335L к ROS 2 штатным драйвером orbbec_camera, проверим частоту depth-топиков, соберём рабочую конфигурацию rviz2 и напишем небольшой узел-колоризатор на Python, который превращает сырую глубину в читаемую радужную карту. Получится базовый каркас, на который дальше навешиваются обход препятствий, SLAM и сбор датасетов.

Почему в связке с ROS 2 это стоит делать именно так? ROS даёт готовую инфраструктуру: камера становится источником стандартных сообщений, и всё, что вы дальше напишете — от обхода препятствий до записи датасетов, — не будет знать, какая именно камера стоит в кадре. rviz2 же экономит самое дорогое — время разработчика: каждое изменение геометрии сцены, каждый «почему робот поехал в стену» проверяется визуально за секунды. Гайд написан по живому рабочему сценарию: сначала диагностика топиков, потом визуализация и только затем собственный код — в этом порядке проблемы локализуются быстрее всего.

Что понадобится

• Ubuntu 22.04 с ROS 2 Humble (на Iron/Jazzy отличий минимум);
• камера Orbbec Gemini 335L и кабель USB 3.0 (пассивный — до 3 метров);
• пакет драйвера OrbbecSDK_ROS2 с узлом orbbec_camera;
• rviz2, cv_bridge, OpenCV и NumPy;
• 15 минут и терминал.

Пошаговая настройка

Шаг 1. Собираем драйвер

Драйвер OrbbecSDK_ROS2 распространяется через GitHub и собирается стандартным конвейером colcon. Создайте рабочее пространство, склонируйте репозиторий и подтяните зависимости:

mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/orbbec/OrbbecSDK_ROS2.git
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install
source install/setup.bash

Если rosdep не смог разрешить какой-то пакет, чаще всего не хватает ros-humble-usb-cam-зависимостей или библиотек OpenCV — доустановите по подсказке rosdep. После сборки выполняйте source install/setup.bash в каждом новом терминале или добавьте строку в ~/.bashrc, иначе ROS 2 просто не увидит узел драйвера.

Шаг 2. Запускаем камеру

Для серии Gemini 330 у драйвера есть готовый launch-файл. Запустите его в отдельном терминале:

source ~/ros2_ws/install/setup.bash
ros2 launch orbbec_camera gemini_330_series.launch.py

Точное имя launch-файла зависит от версии драйвера — сверьте с README вашей ревизии репозитория (для других моделей Orbbec там лежат свои launch-файлы). Если камера поднялась, в консоли появятся строки о найденном устройстве, серийном номере и открытых потоках depth и color. Камеру подключайте в порт USB 3.0 — на USB 2.0 драйвер либо не откроет полное разрешение, либо резко уронит частоту кадров.

Параметры запуска задают профиль потоков: разрешение и частоту кадров можно понизить, не трогая код, — это штатный способ разгрузить шину на слабом хосте. Для отладки визуализации логично стартовать с умеренного профиля: меньше полосы и нагрузки на перекладку сообщений, а rviz2 от этого только выигрывает. Помните, что профили depth и color независимы: иногда глубине оставляют максимум разрешения, а цвет режут, либо наоборот — зависит от того, что важнее для вашей задачи.

Шаг 3. Проверяем топики

В новом терминале посмотрите список топиков и убедитесь, что глубина реально публикуется с нужной частотой:

ros2 topic list
ros2 topic hz /camera/depth/image_rect_raw
ros2 topic echo /camera/depth/camera_info --once

ros2 topic hz должен показывать стабильные 30 Гц (или то значение, что задано параметрами launch-файла). Из camera_info возьмите fx, fy, cx, cy — эти интринсики понадобятся любому коду, который переводит пиксели в лучи в пространстве. Полный набор топиков зависит от версии драйвера и флагов запуска, типовая картина — в таблице ниже.

Шаг 4. Настраиваем rviz2

Запустите rviz2 и сделайте три вещи. Первое: задайте Fixed Frame = camera_link — без этого дисплей Image и облако точек не привяжутся к кадру камеры. Второе: добавьте дисплей типа Image и выберите топик /camera/depth/image_rect_raw — увидите grayscale-картину глубины, где ближе — темнее. Третье: добавьте PointCloud2 с топиком /camera/depth/points (если он не публикуется, перезапустите launch с параметром enable_point_cloud:=true). Чтобы облако было цветным, используйте топик /camera/depth/color/points. Поворачивая облако мышью, сразу видно геометрию сцены — это главный аргумент держать rviz2 открытым во время любой отладки.

Два практичных твика дисплея PointCloud2: уменьшите Size (m) до 0,01 — облако перестанет выглядеть крупными квадратами, и поставьте Style в Points, если rviz2 проседает по частоте. Для дисплея Image полезно включить нормализацию диапазона — rviz2 сам растянет миллиметры в видимую шкалу. Если картинка обновляется рывками, снизьте частоту обновления отображения: rviz2 рендерит в цикле GUI-фреймворка, и на слабой графике это заметнее, чем нагрузка от самого ROS.

Шаг 5. Пишем узел-колоризатор

Gray-scale глубина плохо читается. Сделаем узел, который подписывается на depth-топик с сенсорным профилем QoS и публикует цветную версию через COLORMAP_JET:

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np


class DepthColorizer(Node):
    def __init__(self):
        super().__init__('depth_colorizer')
        self.bridge = CvBridge()
        self.sub = self.create_subscription(
            Image, '/camera/depth/image_rect_raw',
            self.on_depth, qos_profile_sensor_data)
        self.pub = self.create_publisher(
            Image, '/camera/depth/colormap', 10)

    def on_depth(self, msg):
        depth = self.bridge.imgmsg_to_cv2(
            msg, desired_encoding='passthrough')
        valid = depth > 0
        d = depth.astype(np.float32)
        vmax = np.percentile(d[valid], 98) if valid.any() else 1.0
        norm = np.zeros_like(d, dtype=np.uint8)
        norm[valid] = np.clip(d[valid] / vmax * 255.0, 0, 255).astype(np.uint8)
        colored = cv2.applyColorMap(norm, cv2.COLORMAP_JET)
        colored[~valid] = 0
        out = self.bridge.cv2_to_imgmsg(colored, encoding='bgr8')
        out.header = msg.header
        self.pub.publish(out)


def main():
    rclpy.init()
    node = DepthColorizer()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Сохраните как depth_colorizer.py, сделайте исполняемым и запустите: python3 depth_colorizer.py. В rviz2 добавьте ещё один дисплей Image на топик /camera/depth/colormap — и карта глубины станет наглядной: тёплые цвета — ближе, холодные — дальше.

Разбор кода

Здесь три решения, которые экономят часы отладки. Первое — QoS: depth-поток камера публикует в профиле sensor data (best effort, малая история), поэтому подписка создаётся с qos_profile_sensor_data. Подписка по умолчанию (reliable) с таким издателем просто не договорится, и колбэк никогда не сработает — об этом подробнее в разделе проблем. Второе — desired_encoding='passthrough': мы просим cv_bridge не перекодировать данные, а отдать сырую mono16-матрицу как есть. Любая переконвертация в 8 бит до нормализации убила бы диапазон миллиметров. Третье — нормализация по 98-му перцентилю валидных значений, а не по максимуму: один случайный «выброс» в 65 метров иначе превратил бы всю картинку в однородное пятно. Нули (неизмеренные пиксели) заранее исключены из нормализации и залиты чёрным уже после раскраски — так дырки данных остаются видимыми, что для отладки только полезно.

Отдельно про out.header = msg.header: мы переносим исходный штамп времени и frame_id. Это делает выходной топик совместимым с любым кодом, который синхронизирует depth с color по времени или строит на нём геометрию в системе координат камеры.

Если дальше по пайплайну глубина превращается в 3D-точки, держите под рукой camera_info: пара «пиксель — луч» считается через интринсики fx, fy, cx, cy ровно в тех единицах, что лежат в сообщении глубины. Важно использовать интринсики именно depth-камеры: цветной объектив другой, и подмена интринсиков — типичный источник «слегка съехавшей» геометрии, которую потом ищут неделями. Драйвер публикует camera_info для обоих потоков — берите каждый со своего топика и не смешивайте.

Топик	Тип сообщения	Что внутри
/camera/color/image_raw	sensor_msgs/Image	цветной кадр RGB
/camera/depth/image_rect_raw	sensor_msgs/Image	глубина uint16 в миллиметрах, 0 — нет данных
/camera/depth/camera_info	sensor_msgs/CameraInfo	интринсики depth-камеры
/camera/depth/points	sensor_msgs/PointCloud2	облако точек (включается параметром запуска)
/camera/depth/color/points	sensor_msgs/PointCloud2	облако точек с цветом RGB

Частые проблемы

В rviz2 пустое серое окно вместо изображения

В 90% случаев это несовпадение QoS. rviz2 по умолчанию пытается подписаться как reliable, а драйвер камеры публикует best effort — диалог не состоится. В настройках дисплея Image смените Reliability на Best Effort, History — на Keep Last. То же правило действует для любого собственного кода: сенсорные топики ROS 2 почти всегда требуют qos_profile_sensor_data. Проверить профиль издателя можно командой ros2 topic info /camera/depth/image_rect_raw -v — там видны и QoS издателя, и каждого подписчика.

rviz2 пишет «No transform from … to …»

Дисплей ищет преобразование между frame_id сообщения и Fixed Frame. Если в сообщении camera_depth_optical_frame, а Fixed Frame стоит camera_link, трансформа может не быть, пока публикуется статический TF. Самое простое — поставить Fixed Frame в тот frame_id, что указан в сообщении (посмотрите его через ros2 topic echo /camera/depth/camera_info --once). Для стыковки с другими узлами опубликуйте недостающие преобразования через static_transform_publisher, помня, что оптический кадр камеры повёрнут относительно корпуса (Z — вперёд).

Глубина не открывается или падает до единиц кадров

Проверьте, что камера сидит на контроллере USB 3.x: lsusb -t должен показать 5000M на её шине. Дешёвые кабели длиннее трёх метров и хабы — главные виновники. Если dmesg сыплет сообщениями об ошибках USB, замените кабель и порт. Ещё одна причина — занятый драйвер: если NiViewer или OpenNI-утилита уже открыли устройство, ROS-узел его не получит. Закройте конкурирующие процессы и перезапустите launch.

Цвет и глубина приходят с задержкой друг от друга

Depth и color — два независимых потока: сетевые очереди и обработка вносят разное запаздывание. Если ваш код склеивает кадры «последний с последним», на движущихся объектах цвет будет обгонять или отставать от глубины. Правильное решение — синхронизация по штампам: в ROS 2 для этого есть message_filters с политикой ApproximateTime — он отдаёт пары сообщений, снятые максимально близко по времени. Учтите, что штампы в header ставит драйвер в момент получения кадра от камеры, поэтому их разница честно отражает расхождение съёма. Для отладки допуска в единицы миллисекунд достаточно, но если собираете датасет — эту тему стоит закрыть сразу, до записи.

Подобрать камеру под такой сценарий можно в каталоге Orbbec на luwam.ru. Для ROS 2 и карт глубины чаще всего берут Gemini 335L (активная стереобаза 100 мм) и Femto Bolt (iToF, 0,1–5,5 м, RGB 4K) — поможем выбрать под вашу задачу, дадим консультацию по SDK, выставим счёт для юрлица, гарантия 1 год.