はじめに
前回の記事では、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 に置き、コンテナにマウントして使います。
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本のスクリプトにまとめました。
#!/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 するグラフを作ります。
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 カメラに設定する
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 の移動量のずれを測り、速度スケールを合わせる