RealSenseの時刻同期したRGBD画像をSubscribeする(realsense2_camera_msgs)

CoRE-1に参加します❗‥と勢い良く宣言してから早1か月。

かなり目まぐるしく進捗が進んでいます。

先週の高専ロボコンでは、完成度の高いロボットが素晴らしい試合をしてくれました。

私も、あと4ヶ月後の本番に向けて日々進捗を進めていきますよ💪

CoRE-1の自動機チームが私のチーム以外いないのが寂しいですね。

このままでは私が最強の自動機を作っちゃいますよ？（煽り）

RealSenseに時刻同期のメッセージ型が！？

最初に触れたCoRE-1という大会についてですが、この大会では複数ロボットで構成される2チームが陣地の獲得のために撃ち合いをするものとなっています。

この大会の自動機チームのセンサにはRealSense D455というデプスカメラが使用可能で、貸与されるコンピュータで画像処理・点群処理が可能になります。

RealSenseは多くのロボットに搭載される深度カメラでサポートが充実していおり、ROS 2向けのrealsense_rosがあります。

realsense_rosは簡単なコマンドでデータ取得形式を変更できたりするので非常に便利なので、今回の大会でもぜひ使いたいところです。

久しく使っていなかったのでリファレンスを見ていたところ、「new」との表記が目に留まりました。

github.com

よく読んでみると、どうやらRealSenseでRGBDの時刻同期された画像が取得できるとのこと。

これはとても便利ですね。

これまで画像と深度を同時に取得する場合は、2つのトピックをどちらもsubscribeすることになるのですが、この時に問題になるのが「本当にその画像は時刻同期されているのか」です。

画像の時刻がもし不一致なら、物体位置の推定にミスが出る可能性があるのでいちいち画像に付与されている時刻を参照しないといけません。

ROSでは、このようなケースを想定してApproximateTimeという同期ポリシーを使用してフレーム欠損を極力抑えつつ時刻同期を実現させることはできます。

でも、そもそもRealSense SDK側は本来同時にRGBとD画像を取得できるのにわざわざROS 2で一回バラす理由もない‥となり、敬遠されている方もいるのではないでしょうか？

なんと、4ヶ月前にrealsense_rosに RGBD というメッセージ型が追加されてその時刻同期メッセージがサポートされたみたいです。

RGBD.msg

このメッセージには次の情報が入ります。

ヘッダー（std_msgs/Header）
RGBカメラ情報（sensor_msgs/CameraInfo）
深度カメラ情報（sensor_msgs/CameraInfo）
RGB画像データ（sensor_msgs/Image）
深度画像データ（sensor_msgs/Image）

このメッセージさえsubscribeしておけばいいのはとても良いですね！

今回はこれを試してみました。

環境

Ubuntu22.04
ROS 2 Humble
最新のlibrealsense

realsense2_camera はaptでインストールしなくていいですがインストールしてもいいです。

ビルド

現時点では、 RGBD.msg の適用のためにはソースコードからビルドする必要があります。

mkdir ~/ws_rs_ros/src -p
cd ~/ws_rs_ros
git clone https://github.com/IntelRealSense/realsense-ros.git src/realsense-ros

source /opt/ros/humble/setup.bash
colcon build

実行方法

source ~/ws_rs_ros/install/setup.bash
ros2 launch realsense2_camera rs_launch.py enable_rgbd:=true enable_sync:=true align_depth.enable:=true enable_color:=true enable_depth:=true

ros2 topic list でトピック一覧を取得すると次のような出力が出ます。

/camera/camera/rgbd が realsense2_camera_msgs/msg/RGBD 型のメッセージです。

rqt_image_viewやrviz2のデフォルトプラグインでは受信できないので、簡単な受信用サンプルプログラム（sub_rs.py）を作成しました。

import cv2
from cv_bridge import CvBridge
import numpy as np
import rclpy
from rclpy.node import Node
from realsense2_camera_msgs.msg import RGBD

min_depth, max_depth = 200, 500 # mm

class RsSub(Node):
    def __init__(self):
        super().__init__('minimal_subscriber')
        self.bridge = CvBridge()
        self.subscription = self.create_subscription(RGBD, '/camera/camera/rgbd', self.listener_callback, 10)

    def listener_callback(self, msg):
        cv2.imshow("Image window", self.mask_rgb(self.bridge.imgmsg_to_cv2(msg.rgb, "bgr8"), self.bridge.imgmsg_to_cv2(msg.depth, "passthrough")))
        cv2.waitKey(1)

    def mask_rgb(self, rgb, depth) -> np.ndarray:
        mask = (depth <= min_depth) | (depth >= max_depth)
        return np.where(np.broadcast_to(mask[:, :, None], rgb.shape), 0, rgb).astype(np.uint8)

rclpy.init(args=None)
rclpy.spin(RsSub())

source ~/ws_rs_ros/install/setup.bash
python3 sub_rs.py

実行すると20cm〜50cmの範囲外の画像がマスクされます。

トピックを複数引き込むことなくRGB・Dの画像とカメラパラメータを同時に引き込めるので、結構便利そう。

えいあーるれいの技術日記

ROS2やM5 Stack、Ubuntuについて書いています

RealSenseの時刻同期したRGBD画像をSubscribeする(realsense2_camera_msgs)

RealSenseに時刻同期のメッセージ型が！？

環境

ビルド

実行方法