最終更新: 2025-09-16

Jetson Orin NX で以下を扱います。

• 最新フロー（推奨・検証済み）: OAK-D Lite ステレオVO → MAVROS（ExternalNav）
• レガシー: IMX219 モノクロ 1280x720@60fps（単眼VO）

詳細ドキュメント:

• OAK-D Stereo VO 手順: a2rl_NX_oak.md
• 単眼VO（IMX219）: a2rl_NX_pycuvslam.md

• 60fps・低CPUの安定カメラ配信（JetPack 6.x / Argus 経由）
• ROS 2 から簡単に起動・確認（ros2 launch）
• 単眼Visual SLAM（PyCuVSLAM）で pose/pose_slam/tf を配信
• 今後、IMU（MAVROS）連携やFCとの統合へ拡張

以下は映像なし（Pose/TFのみ）の最小構成です。各端末は前景で常時起動してください。

• JetPack 6.2.x / ROS 2 Humble
• DepthAI v3（Python）

source /opt/ros/humble/setup.bash
python3 /home/nonsaya-j/repo/a2rl/scripts/oak_stereo_publisher.py \
  --ros-args -p info_fx:=460.0 -p info_fy:=460.0 -p baseline_m:=0.075

source /opt/ros/humble/setup.bash
ros2 run tf2_ros static_transform_publisher \
  --x 0.09 --y 0 --z 0.05 \
  --roll 0 --pitch 0 --yaw 0 \
  --frame-id oakd_stereo --child-frame-id base_link

source /opt/ros/humble/setup.bash
ros2 launch mavros apm.launch fcu_url:=serial:///dev/ttyTHS1:921600 fcu_protocol:=v2.0

source /opt/ros/humble/setup.bash
ros2 launch /home/nonsaya-j/repo/a2rl/launch/pycuvslam.launch.py \
  image_left:=/camera/left/image_rect image_right:=/camera/right/image_rect \
  camera_info_left:=/camera/left/camera_info camera_info_right:=/camera/right/camera_info \
  encoding:=mono8 world_frame:=map base_frame:=oakd_stereo \
  enable_slam:=true rectify:=false remap_to_ros:=true use_imu:=false \
  stereo_baseline_m:=0.075

（IMU併用を試す場合: use_imu:=true imu_topic:=/mavros/imu/data_raw imu_remap_to_cuv:=true）

source /opt/ros/humble/setup.bash
python3 /home/nonsaya-j/repo/a2rl/scripts/vision_pose_bridge.py \
  --ros-args -p input_topic:=/pycuvslam_node/pose \
  -p output_topic:=/mavros/vision_pose/pose -p force_frame_id:=map -p rate_limit_hz:=60.0 \
  -p enable_tf_transform:=true -p child_source_frame:=oakd_stereo -p child_target_frame:=base_link \
  -p tf_timeout_sec:=0.1

確認:

• RViz2: Fixed Frame を map に。TF/Pose 表示で /mavros/local_position/pose の更新を確認。
• odom/odom_ned→map の警告は無害（可視化のみの注意）。

VISO_DELAY_MS 調整の要点（ArduPilot）:

• LOITERで前進→スティック離しのブレーキ遅い→ +10ms、過敏に早すぎ→ -10ms。目安 60–120ms。
• 詳細は a2rl_NX_oak.md の「ArduPilot 調整メモ」を参照。

• JetPack 6.2.x（L4T r36.4）
• ROS 2 Humble

git clone git@github.com:nonsaya/a2rl.git cd a2rl

• imx219_1280x720.yaml を本リポジトリ直下に配置

source /opt/ros/humble/setup.bash ros2 launch launch/a2rl_camera.launch.py
camera_info_url:=file://$PWD/imx219_1280x720.yaml
sensor_id:=0 fps:=60

ros2 topic hz /camera/image_raw ros2 topic echo --once /camera/camera_info

詳細は a2rl_NX_mavros.md を参照。

停止:

pkill -u "$USER" -f "ros2 (launch mavros|run mavros)" || true

起動（別端末でIMU設定を行う前提）:

source /opt/ros/humble/setup.bash
ros2 launch mavros apm.launch \
  fcu_url:=serial:///dev/ttyTHS1:921600 \
  fcu_protocol:=v2.0

IMU 200Hz（別端末）:

source /opt/ros/humble/setup.bash
ros2 service call /mavros/set_message_interval mavros_msgs/srv/MessageInterval "{message_id: 26,  message_rate: 200.0}"
ros2 service call /mavros/set_message_interval mavros_msgs/srv/MessageInterval "{message_id: 116, message_rate: 200.0}"
ros2 service call /mavros/set_message_interval mavros_msgs/srv/MessageInterval "{message_id: 129, message_rate: 200.0}"

検証:

timeout 3s ros2 topic hz /mavros/imu/data_raw

詳細は a2rl_NX_pycuvslam.md を参照。

1. 環境変数（cuVSLAMをホストで読み込む）

source /opt/ros/humble/setup.bash
export PYCUVSLAM_BIN_PATH=/home/<user>/a2rl_docker/pycuvslam/PyCuVSLAM/bin/aarch64
export PYTHONPATH="$PYCUVSLAM_BIN_PATH:$PYTHONPATH"
export LD_LIBRARY_PATH="$PYCUVSLAM_BIN_PATH:$LD_LIBRARY_PATH"
python3 -c "import cuvslam; print('cuvslam:', getattr(cuvslam,'__file__','<pkg>'))"

2. 起動

ros2 launch launch/pycuvslam.launch.py \
  image_topic:=/camera/image_raw camera_info_topic:=/camera/camera_info \
  encoding:=mono8 world_frame:=map base_frame:=imx219_mono \
  enable_slam:=true rectify:=false remap_to_ros:=true

3. 確認

ros2 topic echo --once /pycuvslam_node/pose
ros2 run tf2_ros tf2_echo map imx219_mono

4. 収録（任意）

ros2 bag record \
  /camera/image_raw /camera/camera_info \
  /pycuvslam_node/pose /pycuvslam_node/pose_slam /tf

• 本プロジェクトのカメラは単眼です。PyCuVSLAM/cuVSLAM では、Visual-Inertial の公式サポートは Stereo-VI 前提です。単眼+IMU のVIは公式ドキュメント/例が提示されていません。
  • 公式ドキュメント（cuVSLAM 概念）では stereo-visual-inertial を前提に解説: https://nvidia-isaac-ros.github.io/concepts/visual_slam/cuvslam/index.html
  • PyCuVSLAMの例では Monocular は VO（IMUなし）として記載（スケール不定）、VIの例は Stereo 系: https://github.com/NVlabs/PyCuVSLAM/blob/main/examples/euroc/README.md
  • API上は IMU 構造体/リグが存在するが、単眼+IMUの運用例は提供されていない: https://nvlabs.github.io/PyCuVSLAM/api.html
• 実運用結論: 単眼構成では VO（Monocular）で運用する。
  • 起動例（IMU無効）:

    ros2 launch launch/pycuvslam.launch.py \
      image_topic:=/camera/image_raw camera_info_topic:=/camera/camera_info \
      encoding:=mono8 world_frame:=map base_frame:=imx219_mono \
      enable_slam:=true rectify:=false remap_to_ros:=true \
      use_imu:=false

  • 画質/特徴点/キャリブの整合でVOの安定性を高める。
  • VIが必須な場合はステレオ化を検討（cuVSLAMの想定構成）。

• 方針と詳細: 到達目標.md を参照
• Step1（完了）: カメラ配信 1280x720@60fps
• Step2（進行中）: MAVROS（IMU） — a2rl_NX_mavros.md を参照
• Step3（完了）: PyCuVSLAM（SLAMノード化） — a2rl_NX_pycuvslam.md を参照
• Step4: SLAM→FC 連携（ホバリング）
• Step5: ジョイスティック
• Step6: 経路飛行

• camera_node.py : カメラ配信（mono8 / 60fps）
• launch/a2rl_camera.launch.py : 起動用最小launch
• imx219_1280x720.yaml : キャリブ（例）
• a2rl_NX_cameranode.md : Jetson Orin NX 運用ガイド
• a2rl_NX_mavros.md : MAVROS 運用ガイド（ArduPilot/IMU/ストリーム）
• pycuvslam_wrapper.py : PyCuVSLAM 単眼用ラッパーノード
• launch/pycuvslam.launch.py : PyCuVSLAM 起動用launch
• a2rl_NX_pycuvslam.md : PyCuVSLAM 運用ガイド（単眼SLAMの要点/ハマり所/起動）
• 到達目標.md : 目標/ステップ/運用方針
• トラブル.md : 既知の問題と対処

• GPU 使用率が 0% に見える
  • 画像取り出しは CPU メモリのため CUDA SM 使用率は 0% でも正常（VIC/ISP は別カウンタ）
• RViz で画像が黒い
  • Image Display の QoS を Best Effort に変更
• 60fps に届かない
  • sudo nvpmodel -m 0 と sudo jetson_clocks を適用、他負荷停止

• No module named 'cuvslam' → PYCUVSLAM_BIN_PATH=/home/<user>/a2rl_docker/pycuvslam/PyCuVSLAM/bin/aarch64 を設定し、PYTHONPATH/LD_LIBRARY_PATH に追加。python3 -c "import cuvslam" を確認。
• pose が0のまま → 単眼は並進必須。ラッパーは Mono 強制/rig_from_camera=I 済み。rectify:=false で切り分け。
• TFが無い → world_frame:=map base_frame:=imx219_mono で起動し、RVizの Fixed Frame を map に。
• 静止で数値がロックする → フリーズ検出と自動再初期化を有効化（既定有効）。必要に応じて閾値を調整。
  • 例: 2秒以上ほぼ無変化で再初期化（平行移動<2mm、回転<0.5°）
  • 起動例（既定値を明示）:

    ros2 launch launch/pycuvslam.launch.py \
      image_topic:=/camera/image_raw camera_info_topic:=/camera/camera_info \
      encoding:=mono8 world_frame:=map base_frame:=imx219_mono \
      enable_slam:=true rectify:=false remap_to_ros:=true \
      stuck_max_no_motion_sec:=2.0 stuck_trans_epsilon:=0.002 \
      stuck_rot_epsilon_deg:=0.5 stuck_recovery_cooldown_sec:=3.0

MIT（プロジェクト方針に合わせて記載）