深度カメラ

Elf3 プロジェクトでは、bxi_depth_camera を通じて深度カメラのデータを配信します。Intel RealSense および Orbbec Gemini 335 カメラに対応しています。アプリケーションは ROS 2 の画像トピックをサブスクライブするだけで、ベンダー SDK を直接呼び出すことなく、カラー画像・深度画像・カメラ内部パラメータを取得できます。

本ガイドでは、ロボット本体のホスト上でカメラデータを読み取ります。実行環境を読み込み、カメラノードとトピックを確認したうえで、Python で RGB 配列と深度配列を取得します。

1. 環境の読み込み

ソースコード：bxi_controller_ros2（導入ユーザー様に HumansX より提供します）。デプロイ方法に応じてプロジェクトディレクトリを選択してください。インストール先が異なる場合は、ロボット上の実際のパスを使用してください。

新しいターミナルを開くたびに、ROS 環境とプロジェクト環境を読み込みます。以下の例はビルド済みのソースワークスペースを前提としています。App によるデプロイの場合は、cd の後のパスを置き換えてください。新規のソースデプロイでまだ install/setup.bash が存在しない場合は、先に ROS 環境を読み込み、後述の手順でビルドしてください。

source /opt/bxi/bxi_ros2_pkg/setup.bash
cd ~/bxi_ws/bxi_rl_controller_ros2_example
source install/setup.bash

ros2 pkg prefix bxi_depth_camera

標準的な ROS 2 Humble をインストールした開発用 PC では、1 行目を source /opt/ros/humble/setup.bash に置き換え、ご自身のワークスペースディレクトリを使用してください。ros2 pkg prefix を実行すると、カメラパッケージのインストールパスが表示されます。

ソースデプロイの場合は、初回使用前またはソース更新後に、プロジェクトのビルド依存関係をインストールし、プロジェクトルートでビルドスクリプトを実行してください。App でダウンロードしたビルド済みプログラムではこの手順は不要です。ターミナルを開くたびに再ビルドする必要はありません。

cd ~/bxi_ws/bxi_rl_controller_ros2_example
bash build.sh
source install/setup.bash

ワークスペースの依存関係がすでに揃っており、カメラパッケージのみをビルドする場合は、プロジェクトルートで次を実行します。

colcon build --merge-install --packages-select bxi_depth_camera
source install/setup.bash

ビルド後、再度 ros2 pkg prefix bxi_depth_camera を実行してインストールパスを確認してください。

後述の Python サンプルには rclpy、sensor_msgs、cv_bridge、message_filters が必要です。標準的な Ubuntu 22.04 / ROS 2 Humble 環境にこれらのパッケージがない場合は、次のコマンドでインストールしてください。

sudo apt install ros-humble-rclpy ros-humble-sensor-msgs \
  ros-humble-cv-bridge ros-humble-message-filters

2. カメラの確認と起動

2.1 コントローラ起動時のサービス環境に合わせる

ロボットをコントローラと自動起動サービスで運用している場合、カメラデータを読み取るターミナルはサービスと同じ ROS 2 通信環境を使用する必要があります。環境の不一致をカメラ停止と誤認しないよう、ノードを確認する前にこの設定を行ってください。カメラを手動で起動する場合は 2.2 節へ進んでください。

コントローラ起動サービスの状態と設定を確認します。

systemctl status ros_elf_launch.service --no-pager
systemctl cat ros_elf_launch.service

アクセスが拒否された場合は、該当コマンドの先頭に sudo を付けてください。トピックをサブスクライブするだけであれば root に切り替える必要はありません。ユーザーを切り替えた場合は、新しいターミナルで 1 節の環境を再度読み込んでください。

サービス設定内の ROS_DOMAIN_ID、RMW_IMPLEMENTATION、ROS_LOCALHOST_ONLY、CYCLONEDDS_URI を確認します。起動スクリプトでこれらの変数を設定している場合は、ExecStart で参照されているスクリプトも確認してください。

1 節の環境を読み込んだ後、データ読み取り用ターミナルで実際のサービスに合わせて変数を設定します。例えば、サービスがドメイン ID 89 と CycloneDDS を使用している場合は次のとおりです。

export ROS_DOMAIN_ID=89
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=0
export CYCLONEDDS_URI='<CycloneDDS><Domain Id="any"><General><Interfaces><NetworkInterface name="lo" multicast="true"/></Interfaces><AllowMulticast>true</AllowMulticast></General></Domain></CycloneDDS>'

2.2 既存ノードの確認

設定済みのターミナルで次のコマンドを実行します。

ros2 node list
ros2 topic list

example_demo_hw.launch.py がすでに実行中であれば、カメラノードも含まれているため、その既存ノードを使用してください。/depth_camera_manager と 3 節のトピックが存在するかを確認し、深度アライメントとフレーム同期が有効になっていることを確認してください。

2.3 カメラが起動していない場合の個別起動

カメラノードが起動していないことを確認したうえで、次のコマンドを実行し、カラー、深度、フレーム同期、深度アライメントを有効にします。

ros2 launch bxi_depth_camera cameras.launch.py \
  enable_color:=true \
  enable_depth:=true \
  enable_sync:=true \
  align_depth.enable:=true

このターミナルは起動したままにしておきます。別のターミナルを開き、同じ実行環境と通信設定を読み込んでから、以下の手順でデータを読み取ってください。

3. 画像トピックの確認

シリアル番号によるマッピングを明示していない単一カメラは、デフォルト名 head_depth_camera を使用します。

実際のトピックを一覧表示し、画像が継続的に届いていることを確認します。

ros2 topic list
ros2 topic hz /hardware/head_depth_camera/color/image_raw

4. Python でカラーと深度を同時に読み取る

次のコードを ~/read_rgb_depth.py として保存します。メッセージのタイムスタンプで画像をペアリングし、コールバック内で 2 つの NumPy 配列 rgb と depth を生成して、そのサイズと中心画素の深度を 1 秒に 1 回出力します。

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
from message_filters import Subscriber, ApproximateTimeSynchronizer


class CameraReader(Node):
    def __init__(self):
        super().__init__("camera_reader")
        camera_name = self.declare_parameter(
            "camera_name", "head_depth_camera"
        ).value
        # Meters per data unit; 0 means unspecified, so only raw values are printed.
        self.depth_scale = self.declare_parameter("depth_scale", 0.0).value
        self.bridge = CvBridge()
        base = f"/hardware/{camera_name}"

        self.color_sub = Subscriber(
            self, Image, base + "/color/image_raw",
            qos_profile=qos_profile_sensor_data,
        )
        self.depth_sub = Subscriber(
            self, Image, base + "/aligned_depth_to_color/image_raw",
            qos_profile=qos_profile_sensor_data,
        )
        self.sync = ApproximateTimeSynchronizer(
            [self.color_sub, self.depth_sub],
            queue_size=5,
            slop=0.03,  # Timestamp pairing tolerance: 30 ms.
        )
        self.sync.registerCallback(self.on_frames)

    def on_frames(self, color_msg, depth_msg):
        # rgb: uint8, shape (height, width, 3), with RGB channel order.
        rgb = self.bridge.imgmsg_to_cv2(color_msg, desired_encoding="rgb8")
        # depth: uint16, shape (height, width), preserving depth values.
        depth = self.bridge.imgmsg_to_cv2(
            depth_msg, desired_encoding="passthrough"
        )

        y, x = depth.shape[0] // 2, depth.shape[1] // 2
        raw = int(depth[y, x])  # Array indexing is [row y, column x].
        if raw == 0:
            value = "invalid"
        elif self.depth_scale > 0:
            value = f"{raw * self.depth_scale:.3f} m"
        else:
            value = f"{raw} (raw)"
        self.get_logger().info(
            f"RGB={rgb.shape}, depth={depth.shape}, center={value}",
            throttle_duration_sec=1.0,
        )
        # Process rgb and depth here, for example to detect objects or read pixel depth.


def main():
    rclpy.init()
    node = CameraReader()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    main()

1 節の環境を読み込んだターミナルで実行します。コントローラ起動サービスを使用している場合は、2.1 節の通信設定も適用してください。

/usr/bin/python3 ~/read_rgb_depth.py

画像を受信すると、次のような出力になります。実際のサイズはデバイスとネゴシエートされたプロファイルによって異なります。

RGB=(1080, 1920, 3), depth=(1080, 1920), center=1234 (raw)

rgb[y, x] にはその画素の RGB 色が、depth[y, x] には対応する深度値が格納されます。後続処理で OpenCV の BGR インターフェースを使用する場合は、desired_encoding="rgb8" を "bgr8" に変更してください。このサンプルは qos_profile_sensor_data で画像を受信し、2 つのストリーム間で約 30 ms のタイムスタンプ差を許容します。これは近似的なペアリングであり、露光時刻が完全に一致することは保証されません。

デプロイ環境ですでにマッピングされている別の論理カメラ名（例：body_depth_camera）を選択する場合は、次のようにします。

/usr/bin/python3 ~/read_rgb_depth.py --ros-args \
  -p camera_name:=body_depth_camera

5. 深度をメートルに変換する

16UC1 は各画素が符号なし 16 ビット整数であることを示すだけで、それ自体で深度の単位を決めるものではありません。値を変換する前に、ご使用のカメラとドライバ設定における深度スケールを確認してください。

深度（メートル）= 生の深度値 × depth_scale

サンプルのデフォルトは depth_scale=0.0 で、この場合は生の値のみを出力します。深度の 1 単位が 1 ミリメートルであることを確認済みであれば、スケールを 0.001 に設定します。

/usr/bin/python3 ~/read_rgb_depth.py --ros-args \
  -p depth_scale:=0.001

この場合、生の値 1234 は 1.234 m に相当します。

トラブルシューティング

USB 接続とデバイスの確認

ロボット上で次のコマンドを実行し、USB のリンク速度とカメラが認識されているかを確認します。

lsusb -t
cameras-inspect

lsusb -t の出力からカメラの USB ブランチを探し、リンク速度が 5000M（5 Gbit/s、USB 3.0）以上であるかを確認してください。480M と表示される場合は USB 2.0 で接続されています。USB ポート、ケーブル、中継機器を確認し、カメラに十分な電源が供給されていることを確認してください。

ビデオデバイスノードも確認できます。

ls /dev/video*

これらのノードは他のデバイスのものである場合があります。存在するだけでは深度カメラが正常に動作しているとは言えないため、cameras-inspect の認識結果とカメラノードのログも併せて確認してください。

ホストをまたいだサブスクライブ

現在のコントローラ起動プログラムは、デフォルトでローカルループバック通信を使用しており、ロボット本体上でのデータ読み取りに対応しています。別のコンピュータからサブスクライブする場合は、ロボット上の別ターミナルでカメラプログラムを起動し、パブリッシャ側とサブスクライバ側の双方でネットワーク接続、一致する ROS_DOMAIN_ID、DDS 通信設定を構成してください。

この個別起動では、2.1 節の lo にバインドした設定例を実際のネットワークインターフェースに置き換えてください。別のコンピュータでサブスクライバを起動するだけでは、ループバック通信を使用しているトピックは公開されません。