はじめに

前回の記事では、Jetson Nano の JetBot と Isaac Sim 5.1.0 を /cmd_vel でつなぎ、実機と Sim の JetBot を同時に動かせるようにしました。

今回はその続きとして、実機の CSI カメラ映像を PC に送り、Isaac Sim 上の JetBot のカメラ映像と並べて表示するところまで進めます。

![実機（左）と Sim（右）のカメラ映像]

構成

項目	内容
実機カメラ	JetBot 付属の CSI カメラ（IMX219 系、広角）
実機側の出力	640×360、15fps、JPEG（`sensor_msgs/CompressedImage`）
Sim 側の出力	640×360、`sensor_msgs/Image`（同じ PC 内なので非圧縮）
表示	PC の Humble Docker で rqt_image_view を2つ起動

1. 実機カメラの映像を圧縮して送る

なぜ圧縮が必要か

もともとのカメラノードは、sensor_msgs/Image（BGR の生画像）を publish していました。640×360 の BGR 画像は 1 フレームあたり約 691KB あり、15fps で約 10MB/s になります。これを Wi-Fi 越しの DDS で送るのは厳しいため、Jetson 側で JPEG に圧縮します。

実測では、1 フレームあたり約 27KB、帯域は約 400KB/s になり、約 25 分の 1 に減りました。

$ ros2 topic bw /jetbot/camera/image_raw/compressed
403.66 KB/s from 100 messages
        Message size mean: 26.78 KB min: 26.68 KB max: 26.88 KB

カメラノード

OpenCV の VideoCapture で GStreamer（nvarguscamerasrc）から画像を取得し、cv2.imencode で JPEG に変換して publish します。640×360 程度であれば、Nano の CPU でも 15fps で余裕を持って処理できました。

コンテナは --rm で起動しているため、コンテナ内でファイルを編集しても終了すると消えてしまいます。そこでスクリプトは Jetson 本体の ~/jetbot_tools に置き、コンテナにマウントして使います。

~/jetbot_tools/csi_camera_node.py

import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge


def gstreamer_pipeline(cw, ch, w, h, fps, flip):
    return (
        f"nvarguscamerasrc ! "
        f"video/x-raw(memory:NVMM), width=(int){cw}, height=(int){ch}, "
        f"format=(string)NV12, framerate=(fraction){fps}/1 ! "
        f"nvvidconv flip-method={flip} ! "
        f"video/x-raw, width=(int){w}, height=(int){h}, format=(string)BGRx ! "
        f"videoconvert ! video/x-raw, format=(string)BGR ! appsink drop=1 max-buffers=1"
    )


class CsiCameraNode(Node):
    def __init__(self):
        super().__init__('csi_camera')
        p = lambda n, v: self.declare_parameter(n, v).value
        cw, ch = p('capture_width', 1280), p('capture_height', 720)
        w, h = p('width', 640), p('height', 360)
        fps, flip = p('framerate', 30), p('flip_method', 0)
        rate = p('publish_rate', 15.0)
        self.quality = p('jpeg_quality', 70)
        self.publish_raw = p('publish_raw', False)
        self.frame_id = p('frame_id', 'camera_link')

        self.pub_jpeg = self.create_publisher(
            CompressedImage, '/jetbot/camera/image_raw/compressed', 1)
        self.pub_raw = self.create_publisher(
            Image, '/jetbot/camera/image_raw', 1) if self.publish_raw else None
        self.bridge = CvBridge()

        self.cap = cv2.VideoCapture(
            gstreamer_pipeline(cw, ch, w, h, fps, flip), cv2.CAP_GSTREAMER)
        if not self.cap.isOpened():
            self.get_logger().error('カメラを開けませんでした（nvarguscamerasrc）')
            return
        self.get_logger().info(
            f'{w}x{h} @ {rate}fps, JPEG q={self.quality}, raw={self.publish_raw}')
        self.timer = self.create_timer(1.0 / rate, self.timer_callback)

    def timer_callback(self):
        ret, frame = self.cap.read()
        if not ret:
            self.get_logger().warn('フレーム取得に失敗しました')
            return
        stamp = self.get_clock().now().to_msg()

        ok, buf = cv2.imencode('.jpg', frame,
                               [int(cv2.IMWRITE_JPEG_QUALITY), self.quality])
        if ok:
            msg = CompressedImage()
            msg.header.stamp = stamp
            msg.header.frame_id = self.frame_id
            msg.format = 'jpeg'
            msg.data = buf.tobytes()
            self.pub_jpeg.publish(msg)

        if self.pub_raw:
            raw = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
            raw.header.stamp = stamp
            raw.header.frame_id = self.frame_id
            self.pub_raw.publish(raw)


def main(args=None):
    rclpy.init(args=args)
    node = CsiCameraNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.cap.release()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

主なパラメータは次のとおりです。

パラメータ	既定値	説明
`width` / `height`	640 / 360	publish する画像サイズ
`publish_rate`	15.0	publish の周期（fps）
`jpeg_quality`	70	JPEG の画質（0〜100）
`publish_raw`	False	`true` にすると生画像も `/jetbot/camera/image_raw` に出す
`flip_method`	0	映像が上下逆の場合は 2

起動スクリプト

JetBot を再起動するとコンテナもノードもすべて止まるので、コンテナの起動と3つのノード（motors、watchdog、カメラ）の起動を1本のスクリプトにまとめました。

~/jetbot_tools/start_jetbot.sh

#!/bin/bash
sudo docker run -it --rm --name jetbot \
  --runtime nvidia --privileged \
  --net=host --ipc=host \
  -v /tmp/argus_socket:/tmp/argus_socket \
  -e ROS_DOMAIN_ID=30 \
  -e FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml \
  -v $HOME/.ros/fastdds.xml:/fastdds.xml \
  -v $HOME/jetbot_tools:/jetbot_tools \
  jetbot_ros:humble-r32.7.1-stable \
  bash -c "source /opt/ros/humble/install/setup.bash; \
           source /workspace/install/local_setup.bash; \
           ros2 run jetbot_ros motors_waveshare & \
           python3 /jetbot_tools/cmd_vel_watchdog.py & \
           python3 /jetbot_tools/csi_camera_node.py"

--name jetbot を付けておくと、別のシェルから sudo docker exec -it jetbot bash で同じコンテナに入れます。名前を付けないと Docker がランダムな名前（nifty_banzai など）を付けるので、毎回 docker ps で確認する必要があります。

2. PC で実機の映像を見る

PC の Humble Docker（前回の記事で作った操作端末）に、rqt_image_view と圧縮画像用のプラグインを入れます。

apt update && apt install -y ros-humble-rqt-image-view ros-humble-image-transport-plugins
ros2 run rqt_image_view rqt_image_view

ドロップダウンで /jetbot/camera/image_raw/compressed を選ぶと、実機の映像が表示されます。

起動時に libGL error: failed to load driver: nouveau などが表示されますが、コンテナに GPU ドライバがないため OpenGL がソフトウェア描画に切り替わったことを知らせる警告です。rqt_image_view の表示には影響ありません。

3. Isaac Sim の JetBot カメラを publish する

シーンに環境を読み込む

床（GroundPlane）しかないシーンだと、Sim のカメラには灰色の床と真っ黒な空しか写りません。見比べられるように、Isaac Sim Assets の Environments から Hospital などの環境をステージにドラッグして読み込みました。

カメラ用の OmniGraph

JetBot アセットのカメラは /World/jetbot/chassis/rgb_camera/jetbot_camera にあります。Script Editor で次のスクリプトを実行し、カメラ画像を /sim/camera/image_raw に publish するグラフを作ります。

create_camera_graph.py

import omni.graph.core as og
import omni.usd
from pxr import UsdGeom, Gf

CAM = "/World/jetbot/chassis/rgb_camera/jetbot_camera"
GRAPH = "/World/JetbotCamera"
WIDTH, HEIGHT = 640, 360          # 実機カメラノードの出力解像度に合わせる
NEAR_CLIP = 0.01                  # 手前の床が切り取られないよう小さくする（m）

stage = omni.usd.get_context().get_stage()
if stage.GetPrimAtPath(GRAPH):
    stage.RemovePrim(GRAPH)

# Clipping Range の手前側を小さくする
cam = UsdGeom.Camera(stage.GetPrimAtPath(CAM))
far = cam.GetClippingRangeAttr().Get()[1]
cam.GetClippingRangeAttr().Set(Gf.Vec2f(NEAR_CLIP, far))

og.Controller.edit(
    {"graph_path": GRAPH, "evaluator_name": "execution"},
    {
        og.Controller.Keys.CREATE_NODES: [
            ("Tick", "omni.graph.action.OnPlaybackTick"),
            ("Ctx", "isaacsim.ros2.bridge.ROS2Context"),
            ("RP", "isaacsim.core.nodes.IsaacCreateRenderProduct"),
            ("Cam", "isaacsim.ros2.bridge.ROS2CameraHelper"),
        ],
        og.Controller.Keys.SET_VALUES: [
            ("RP.inputs:cameraPrim", CAM),
            ("RP.inputs:width", WIDTH),
            ("RP.inputs:height", HEIGHT),
            ("Cam.inputs:topicName", "/sim/camera/image_raw"),
            ("Cam.inputs:type", "rgb"),
            ("Cam.inputs:frameId", "sim_camera"),
        ],
        og.Controller.Keys.CONNECT: [
            ("Tick.outputs:tick", "RP.inputs:execIn"),
            ("RP.outputs:execOut", "Cam.inputs:execIn"),
            ("RP.outputs:renderProductPath", "Cam.inputs:renderProductPath"),
            ("Ctx.outputs:context", "Cam.inputs:context"),
        ],
    },
)
print(f"JetbotCamera graph: OK (clipping range = {NEAR_CLIP}, {far})")

Isaac Sim で Play ▶ を押し、PC の Humble Docker でもう1つ rqt_image_view を起動して /sim/camera/image_raw を選ぶと、実機と Sim の映像を並べて見られます。

画面下の帯の正体は Clipping Range だった

最初にグラフを作ったとき、Sim の映像の一番下に、床とは違う模様の帯が写りました。

切り分けの結果は次のとおりです。

状態	下の帯
Hospital の環境あり	白っぽく、緑の点がある
Hospital の環境を非表示	黒（上の空と同じ色）
JetBot 全体を非表示	変わらず残る

JetBot を消しても残るので、車体が写っているわけではありません。帯の色は環境の背景（屋外の HDR 画像、環境なしなら黒）と一致していました。

原因はカメラの Clipping Range でした。手前側の値より近い物体は描画されないため、床から数 cm の高さにある JetBot のカメラでは、画面下に写るはずの手前の床が切り取られ、その向こうの背景が見えていました。jetbot_camera の Clipping Range の手前側を 0.01 に下げると、帯は消えて床が描画されるようになりました。上のスクリプトでは、この設定も行っています。

4. 画角を合わせる

実機のカメラは広角レンズなので、同じ位置から見ても Sim のカメラより写る範囲がかなり広くなります。実機の水平画角（HFOV）を実測し、Sim カメラの Focal Length に反映させます。

実機の HFOV を測る

カメラのレンズから壁まで D（例：0.5m）離して JetBot を置く
rqt_image_view を見ながら、映像の左右の端に写る位置を壁に印す（映像の高さの中央あたりで）
2つの印の間隔 W を測る
HFOV = 2 × atan((W / 2) / D) で計算する

Sim カメラに設定する

set_camera_fov.py

import math
import omni.usd
from pxr import UsdGeom

CAM = "/World/jetbot/chassis/rgb_camera/jetbot_camera"
HFOV = 109.0   # 実測した水平画角（度）に置き換える

stage = omni.usd.get_context().get_stage()
cam = UsdGeom.Camera(stage.GetPrimAtPath(CAM))
ha = cam.GetHorizontalApertureAttr().Get()

old_f = cam.GetFocalLengthAttr().Get()
print(f"現在: focal={old_f:.3f}, HFOV={math.degrees(2 * math.atan(ha / (2 * old_f))):.1f} deg")

new_f = ha / (2 * math.tan(math.radians(HFOV / 2)))
cam.GetFocalLengthAttr().Set(new_f)
print(f"変更後: focal={new_f:.3f}, HFOV={HFOV} deg")

Sim のカメラは歪みのないピンホールモデルなので、合わせられるのは画面中央付近の写り方までです。実機の広角レンズのような端の湾曲は再現できません。端まで合わせたい場合は、Isaac Sim の魚眼カメラモデル（Fisheye Polynomial）を検討します。

5. 設定の保存について

Script Editor で実行した設定は、シーンを File → Save で .usd に保存すれば、OmniGraph やカメラの属性の変更も含めてすべて残ります。JetBot や環境のアセットは参照として読み込まれているため、元のアセットは書き換わらず、変更した部分だけがシーンファイルに保存されます。

ただし、スクリプト自体は保存されないので、別途 .py ファイルとして残しておくと、シーンを作り直すときに便利です。今回は次の3本にまとめました。

ファイル	内容
`create_jetbot_graph.py`	`/cmd_vel` で `/World/jetbot` を差動駆動する OmniGraph（前回の記事）
`create_camera_graph.py`	Sim カメラを `/sim/camera/image_raw` に publish する OmniGraph
`set_camera_fov.py`	実測した HFOV から Focal Length を設定する

ハマりどころ

症状	原因	対処
`Failed to create CaptureSession` が出て、フレームを取得できない	古いカメラノードがまだカメラを使っていた	古いプロセスを止める。それでもだめなら Jetson 本体で `sudo systemctl restart nvargus-daemon`
`docker exec -it jetbot_ros bash` で `No such container`	`jetbot_ros` はイメージ名で、コンテナ名ではない	`docker ps` の NAMES を使うか、`--name` を付けて起動する
コンテナ内で作ったノードが消えた	`--rm` 付きのコンテナは終了すると削除される	スクリプトは Jetson 本体に置いてマウントする
JetBot を再起動したら映像が出なくなった	コンテナとノードがすべて止まった	起動スクリプトで起動し直し、rqt_image_view の更新ボタンを押す
Sim の映像の下に帯が出る	カメラの Clipping Range の手前側が大きすぎる	手前側を 0.01 程度に下げる

ps で ros2-daemon のプロセスを見ると --rmw-implementation rmw_cyclonedds_cpp となっていて、Jetson のコンテナは既定で Cyclone DDS を使っていました。PC 側（Fast DDS）とは DDS の標準プロトコルで通信できるので、/cmd_vel もカメラ映像も問題なく届いています。ただし、Jetson 側では fastdds.xml の設定は効いていません。

おわりに

実機の CSI カメラ映像を JPEG に圧縮して約 400KB/s で PC に送り、Isaac Sim の JetBot のカメラ映像と並べて表示できるようになりました。teleop で JetBot を動かすと、実機と Sim の両方の視点が同時に動きます。

残っている課題は次のとおりです。

実機の HFOV を実測し、Sim の画角を合わせる
実機映像の周辺がピンクがかる色ずれを、ISP の補正ファイルで直す
センサーが 1280×720・120fps のモードで動いているので、sensor-mode を指定して固定する
同じ /cmd_vel に対する実機と Sim の移動量のずれを測り、速度スケールを合わせる

参考