ZED2i + nvblox + Nav2 自律ナビゲーションシステム技術書

Last updated at 2025-12-12Posted at 2025-12-12

使用環境
本プロジェクトについて
ソースコードについて
このロボットの特徴
起動コマンド
- 2.1 メインシステム起動
- 2.2 起動ファイル構成
システムアーキテクチャ
- 3.1 パッケージ統合とノード構成の全体像
- 3.2 主要ノード一覧と設計意図
TFツリー構成と配信責任
- 4.1 TFツリー全体像
- 4.2 TF配信責任の詳細
- 4.3 重要な設計ポイント
- 4.4 mapフレームの扱い
重要な設計思想: nvbloxをSLAMとして使用
- 5.1 設計の核心
- 5.2 nvbloxの役割
- 5.3 ZED2i Visual Odometryの役割
- 5.4 この設計のメリット
- 5.5 この設計の制約
パッケージ詳細
- 6.1 センサー統合
- 6.2 ナビゲーション
トピック構成
起動シーケンスとタイミング
- 8.1 起動フロー
- 8.2 タイミングの理由
まとめ

使用環境

本システムは以下の環境で開発・検証されています：

項目	仕様	備考
計算プラットフォーム	NVIDIA Jetson Orin NX 16GB	25W MODEで動作
JetPack	6.1	NVIDIA公式開発環境
ZED SDK	5.1.0	Stereolabs公式SDK
ROS2 Distribution	Humble Hawksbill	Ubuntu 22.04 LTS
Isaac ROS	nvblox (ソースビルド)	NVIDIA Isaac ROSパッケージ群

性能プロファイル:

メモリ使用量: システム全体で約10-12GB（16GB中）
CPU使用率: 平均40-60%（ピーク時80%）
GPU使用率: nvblox動作時20-30%
電力モード: 25W MODE（バランス型、長時間運用可能）

環境選定の理由:

Jetson Orin NX 16GB: Isaac ROS nvbloxの推奨プラットフォーム、GPUアクセラレーション対応
25W MODE: 性能と消費電力のバランス、長時間運用に適している
JetPack 6.1: 最新のCUDA/cuDNNサポート、Isaac ROS完全対応
ZED SDK 5.1.0: Body Tracking機能強化、Jetson最適化
Isaac ROS nvblox (ソースビルド): GPUアクセラレーション3Dマッピング、Jetson最適化ライブラリ

📌 補足: 本システムで使用している外部パッケージ（nvblox、Nav2、ZED SDKなど）の詳細については、1.4 使用している主要外部パッケージをご参照ください。

本プロジェクトについて

AI支援開発による効率化

本ロボットシステムは、Anthropic Claude Code（AI支援開発ツール）を全面的に活用して開発しました。

開発プロセス:

システム設計: Claudeとの対話を通じて、TFツリー構造、トピック設計、ノード連携を整理
コード実装: ROS2 launchファイル、パラメータ設定、カスタムノードの実装をClaude支援で効率化
デバッグ: エラーログ解析、TF問題の特定、パラメータチューニングをClaudeと協働
ドキュメント作成: 本技術書もClaudeとの対話を通じて体系的に整理・執筆

AI支援開発がもたらした価値:

開発時間の大幅短縮
- 従来なら数週間かかる統合作業を数日で完了
- パラメータ設定の試行錯誤を大幅に削減
検証に時間を投資できる
- コーディングの時間を削減し、実機での動作検証に集中
- 様々なシナリオでのテスト、パラメータの微調整に時間を使える
- 「動くだけ」から「しっかり動く」への質的向上
設計の一貫性
- TFツリー、トピック、パラメータの命名規則を統一
- システム全体の整合性を保ちながら開発
ドキュメントの充実
- 「なぜこの設計にしたのか」を言語化しやすい
- 後から見返した時に理解しやすい技術書の作成

個人開発とAIの相性:
本プロジェクトは個人開発のため、チームレビューやドキュメント整備に時間を割けません。Claudeが「もう一人の開発者」「技術レビュアー」「ドキュメンテーション担当」として機能し、一人でも高品質なシステムを構築できました。

ソースコードについて

本技術書で解説しているシステムの実装コードは、以下のGitHubリポジトリで公開されています：

📦 リポジトリ: https://github.com/motomsgit/Eclevive_robot

コードの詳細、最新の更新、セットアップ手順については上記リポジトリをご参照ください。

1. このロボットの特徴

本システムは、メカナムホイール移動ロボット上でZED2iステレオカメラ、nvblox 3Dマッピング、Nav2自律ナビゲーションを統合した人とロボットの自然なインタラクションを実現する自律移動システムです。

1.1 最大の特徴: 手を挙げるだけでロボットが近づいてくる

本ロボットの最も革新的な機能は、ZED2i Body Trackingとzed_goal_publisherを活用した人物追従機能です。

動作の流れ:

人物検出: ZED2iのBody Tracking機能が人物の骨格を認識
ジェスチャー認識: 人が手を挙げる動作を検出
ゴール生成: zed_goal_publisherノードが人物の位置を目標地点として自動生成
自律ナビゲーション: Nav2が経路を計画し、ロボットが自動的に人物に近づく

この機能の意義:

直感的な操作: コントローラーやアプリ不要、手を挙げるだけで呼べる
誰でも使える: 年齢や技術知識に関係なく、自然な動作でロボットを操作
日常生活での活用: 家庭内での物の運搬、移動補助、見守りなどの実用シーンを想定
自然なインタラクション: ロボットが「人の意図を理解して動く」体験の実現
技術の民主化: 高度なロボット技術を、専門知識なしで使える形で提供

1.2 技術的な特徴

1.2.1 NVIDIA Isaac ROS nvbloxによる高精度3Dマッピング

なぜnvbloxを選んだのか:
従来のSLAM Toolbox（2D SLAM）ではなく、NVIDIA Isaac ROS nvbloxを採用した理由は以下の通りです：

3D環境認識: TSDF（Truncated Signed Distance Field）により、単なる2D平面でなく3D空間全体をマッピング
Jetsonとの親和性: NVIDIA GPUの並列処理能力を最大限活用、リアルタイム処理が可能
深度画像とLiDARの統合: ZED2iの深度画像とデュアルLiDARを同時に統合し、より密なマップを生成
Nav2完全互換: Occupancy Gridを生成し、既存のNav2スタックとシームレスに連携

設計の想い:
「ロボットが人間のように環境を3次元で理解する」ことを目指しました。床の段差、天井の高さ、障害物の形状を正確に把握することで、より安全で自然な移動を実現します。

1.2.2 ZED2i Visual Odometryによる自己位置推定

なぜAMCLを使わないのか:
一般的なROS2ナビゲーションではAMCL（パーティクルフィルタ）による自己位置推定が主流ですが、本システムではZED2i Visual Odometryのみを使用しています。

その理由:

高周波数: ZED2iは60Hzで高精度なオドメトリを配信、AMCLの10Hz程度より滑らか
計算コスト削減: パーティクルフィルタ（数千のパーティクル計算）が不要、Jetsonの計算リソースを節約
ループクロージャ機能: ZED2iは既知エリアへの復帰を自動認識し、累積誤差を補正
IMU統合: ZED2i内蔵IMU（200Hz）との融合により、急な動きにも対応

設計の想い:
「センサーの能力を信じ、シンプルに構成する」という哲学です。ZED2iのVisual SLAMは非常に高精度であり、わざわざAMCLで二重に位置推定する必要がないと判断しました。

1.2.3 デュアルLiDARによる360°障害物検知

なぜ前後2つのLiDARを使うのか:
単一LiDARではなく、前後に2つのLiDARを配置した理由：

完全な360°カバレッジ: 前方180° + 後方180° = 360°の死角なし検知
メカナムホイールの特性: 全方位移動が可能なため、後方への移動時も障害物検知が必須
安全性の向上: 人が後ろから近づいた場合も即座に検知、衝突回避
scan_toolsパッケージで統合: 2つのスキャンを1つの360°スキャンに統合し、Nav2に供給

設計の想い:
「どの方向に動いても安全」を最優先しました。人がいる環境で動くロボットは、常に周囲に気を配る必要があります。

1.2.4 Nav2 MPPI Controllerによる全方位移動

なぜMPPIコントローラーを選んだのか:
DWB（Dynamic Window Approach）ではなく、MPPI（Model Predictive Path Integral）コントローラーを採用：

メカナムホイール対応: motion_model: "Omni"でオムニディレクショナル移動を完全サポート
サンプルベース最適化: 2000個の軌道候補から最適解を選択、滑らかな動き
リアルタイム障害物回避: 予測的制御により、動的障害物にも柔軟に対応
パラメータチューニング: 速度、加速度、障害物回避のバランスを細かく調整可能

設計の想い:
「人の近くで動くロボットは、予測可能で滑らかな動きをすべき」という考えです。MPPIの予測的制御により、急な動きを避け、人が安心して近づけるロボットを目指しました。

1.2.5 PS5コントローラーとBody Trackingの二重インターフェース

なぜ2つの操作方法を用意したのか:

PS5コントローラー: デバッグ時の遠隔操作、パラメータ調整時の細かい動作確認
Body Tracking（手を挙げる）: 自然なインタラクション、ロボットを「呼ぶ」

設計の想い:
「開発とユーザー体験の両立」を重視しました。PS5コントローラーは開発時のデバッグ性向上のために不可欠です。システムの動作確認、センサーのキャリブレーション、パラメータ調整時には細かい操作が必要です。一方、実際の使用時には手を挙げるだけの自然な操作で使える、という両立を目指しています。

デバッグ性の重要性:
個人開発では「問題が起きた時に素早く原因を特定できる」ことが何より重要です。PS5コントローラーによる手動制御があることで、自律機能に問題が起きても即座に介入でき、ログを確認しながら原因を切り分けられます。

1.3 ハードウェア構成

コンポーネント	型番・仕様	役割	選定理由
プラットフォーム	NVIDIA Jetson (Linux 5.15.148-tegra)	計算プラットフォーム	Isaac ROSの完全サポート、GPUによる並列処理
ロボット	メカナムホイール	全方位移動	狭い空間での機動性、その場回転が可能
ステレオカメラ	ZED2i	Visual Odometry + 深度画像 + Body Tracking	オールインワンセンサー、Body Tracking標準搭載
LiDAR	前方LiDAR + 後方LiDAR	360°障害物検知	全方位移動に対応した完全な周囲認識
コントローラー	PS5 DualSense	デバッグ・開発用遠隔操作	直感的な操作性、低遅延のBluetooth接続

1.4 使用している主要外部パッケージ

本システムは、オープンソースのROS2パッケージを活用して構築されています。GitHubリポジトリのセットアップスクリプトで自動インストール可能です。

外部パッケージ一覧

カテゴリ	パッケージ名	提供元	役割
3Dマッピング	nvblox	NVIDIA Isaac ROS	GPU加速3D TSDF マッピング + Occupancy Grid生成
ナビゲーション	Navigation2	ROS2公式	経路計画（Planner）、経路追従（Controller）、Behavior Tree
センサードライバ	zed_wrapper	Stereolabs	ZED2iカメラ統合（Visual SLAM, 深度, Body Tracking）
LiDARドライバ	ldlidar_stl_ros2	LDROBOT	前後LiDARドライバ（前方・後方とも同一パッケージ）
センサー統合	laser_scan_merger	scan_tools	複数LiDARスキャンの360°統合
センサー処理	laser_filters	ROS2公式	スキャンフィルタリング（範囲、強度、欠測除去）

自作パッケージ（統合レイヤー）

本リポジトリには、外部パッケージを統合するための薄いラッパーパッケージが含まれています：

パッケージ名	役割	備考
bringup	統合launchファイル、パラメータ設定	システム全体の起動を1コマンドで実現
zed_goal_publisher	Body Tracking→Nav2ゴール変換	ジェスチャー認識を位置指令に変換
joy_mecanum_controller	PS5コントローラー→速度指令変換	メカナムホイール用全方位制御
mark3_urdf	ロボットモデル定義	TF静的配信、ロボット形状
zed_zupt_wrapper	ZEDオドメトリZUPTフィルタ	静止時のドリフト抑制

設計思想: 外部パッケージは「そのまま使う」ことを基本とし、統合のための薄いラッパーのみを自作。これにより外部パッケージの更新を容易に取り込め、デバッグの切り分けも明確になります。

2. 起動コマンド

2.1 メインシステム起動

ros2 launch bringup zed2i_nvblox_nav2_launch.py

このコマンドで以下が起動します：

ZED2iノード - Visual Odometry + 深度画像 + Body Tracking（人物追従の核）
zed_goal_publisherノード - Body Trackingから目標地点を自動生成
nvbloxノード - 3Dマッピング + Occupancy Grid生成（環境理解）
LiDARマージ・フィルタリングノード - 360°障害物検知
Nav2ナビゲーションスタック - Planner + Controller + Behavior Tree（自律ナビゲーション）
PS5コントローラーインターフェース - デバッグ・開発用手動制御
RViz2可視化 - デバッグ・モニタリング

設計の意図:
たった1つのコマンドで、センサー統合からナビゲーションまでの全システムが起動します。これは「ユーザーが複雑な起動手順を意識しなくて良い」という思想に基づいています。内部では複雑な依存関係がありますが、外からは「シンプルに使える」ことを重視しました。

2.2 起動ファイル構成

zed2i_nvblox_nav2_launch.py (メイン)
  └─ zed2i_nvblox_fixed.launch.py (センサー統合 + Body Tracking + zed_goal_publisher)
  └─ bringup_launch.py (Nav2スタック) [8秒遅延起動]

なぜ8秒の遅延を入れたのか:
Nav2は/mapトピック（Occupancy Grid）が配信されていることを前提に動作します。nvbloxがマップを生成するまでに数秒かかるため、Nav2を先に起動するとエラーで停止します。この8秒の遅延により、以下のシーケンスを確実に実行：

0秒: ZED2i起動（特徴点検出開始）
2秒: ZED2iオドメトリが安定
4秒: nvblox起動（ZED2iポーズを受信してマッピング開始）
6秒: マップ生成が安定
8秒: Nav2起動（マップが確実に利用可能）

「起動の失敗を防ぐための確実なタイミング設計」です。

3. システムアーキテクチャ

3.1 パッケージ統合とノード構成の全体像

本システムは、外部パッケージ（NVIDIA Isaac ROS、Nav2、ZED SDK）と自作パッケージ（統合レイヤー）を組み合わせた階層構造で設計されています。

3.1.1 パッケージ階層図

┌─────────────────────────────────────────────────────────────┐
│  Hardware Layer (Physical Sensors & Actuators)              │
│  ZED2i Camera | Front LiDAR | Back LiDAR | Mecanum Wheels   │
└─────────────────────────────────────────────────────────────┘
                          ↓
┌─────────────────────────────────────────────────────────────┐
│  Driver Layer (External Packages)                           │
│  ┌──────────────┐  ┌──────────────────────────────────┐   │
│  │ zed_wrapper  │  │ ldlidar_stl_ros2 (LDROBOT)       │   │
│  │ (Stereolabs) │  │ 前方・後方LiDAR共通ドライバ      │   │
│  └──────────────┘  └──────────────────────────────────┘   │
└─────────────────────────────────────────────────────────────┘
                          ↓
┌─────────────────────────────────────────────────────────────┐
│  Perception & Mapping Layer (Isaac ROS + Custom)            │
│  ┌─────────────────────────────────────────────────────┐   │
│  │  nvblox_container (Component Container)             │   │
│  │  ┌──────────┐  ┌─────────┐  ┌──────────────────┐  │   │
│  │  │ ZED2i    │→ │ nvblox  │→ │ zed_goal_        │  │   │
│  │  │ Node     │  │ Node    │  │ publisher        │  │   │
│  │  │ (外部)   │  │ (Isaac) │  │ (自作)           │  │   │
│  │  └──────────┘  └─────────┘  └──────────────────┘  │   │
│  └─────────────────────────────────────────────────────┘   │
│  ┌──────────────┐  ┌─────────────────────────────────┐   │
│  │ LiDAR Merge  │→ │ Scan Filter                     │   │
│  │ (scan_tools) │  │ (laser_filters)                 │   │
│  └──────────────┘  └─────────────────────────────────┘   │
└─────────────────────────────────────────────────────────────┘
                          ↓
┌─────────────────────────────────────────────────────────────┐
│  Navigation Layer (Nav2 Stack)                              │
│  ┌──────────────┐  ┌──────────────┐  ┌────────────────┐   │
│  │ Planner      │  │ Controller   │  │ BT Navigator   │   │
│  │ Server       │  │ Server       │  │                │   │
│  │ (Nav2)       │  │ (Nav2)       │  │ (Nav2)         │   │
│  └──────────────┘  └──────────────┘  └────────────────┘   │
└─────────────────────────────────────────────────────────────┘
                          ↓
┌─────────────────────────────────────────────────────────────┐
│  Control Layer (Custom Integration)                         │
│  ┌──────────────────────┐  ┌────────────────────────────┐  │
│  │ joy_mecanum_         │→ │ /cmd_vel → Wheel Control   │  │
│  │ controller (自作)    │  │                            │  │
│  └──────────────────────┘  └────────────────────────────┘  │
└─────────────────────────────────────────────────────────────┘

凡例:

(外部): 外部提供パッケージ（そのまま使用）
(Isaac): NVIDIA Isaac ROSパッケージ（ソースビルド）
(Nav2): ROS2公式Navigation2パッケージ
(自作): 本リポジトリで開発した統合レイヤー

3.1.2 データフロー付き詳細ノード構成図

┌───────────────────────────────────────────────────────────────┐
│  nvblox_container (Component Container - Low Latency)         │
│                                                               │
│  /zed/zed_node (zed_wrapper)                                 │
│  ├─ Visual Odometry → /zed/zed_node/odom (60Hz)             │
│  ├─ 6DOF Pose → /zed/zed_node/pose (60Hz)                   │
│  ├─ Depth Image → /zed/zed_node/depth/depth_registered (15Hz)│
│  ├─ RGB Image → /zed/zed_node/rgb/image_rect_color (15Hz)   │
│  ├─ Body Tracking → /zed/zed_node/body_trk/skeletons (15Hz) │
│  └─ TF: odom → zed_camera_origin (60Hz)                     │
│         ↓                    ↓                    ↓           │
│  /nvblox_node (nvblox_ros - Isaac ROS)                       │
│  ├─ Input: Pose + Depth + RGB + /scan_filtered              │
│  ├─ 3D TSDF Mapping (GPU Accelerated)                       │
│  └─ Output: /map (OccupancyGrid, 5Hz)                       │
│         ↓                                        ↓           │
│  /zed_goal_publisher (Custom)                                │
│  ├─ Input: Body Tracking Skeletons                          │
│  ├─ Gesture Recognition (Hand Raised Detection)             │
│  └─ Output: /goal_pose (PoseStamped)                        │
└───────────────────────────────────────────────────────────────┘
                          ↓
┌───────────────────────────────────────────────────────────────┐
│  LiDAR Processing Pipeline (External Packages)                │
│                                                               │
│  /front_lidar_node (ldlidar_stl_ros2)                        │
│  └─ /front_scan (LaserScan, 30Hz, 180°)                     │
│                        ↓                                      │
│  /back_lidar_node (ldlidar_stl_ros2)                         │
│  └─ /back_scan (LaserScan, 30Hz, 180°)                      │
│                        ↓                                      │
│  /lidar_merge_node (laser_scan_merger)                       │
│  ├─ Input: /front_scan + /back_scan                         │
│  └─ Output: /merged_scan (LaserScan, 30Hz, 360°)            │
│                        ↓                                      │
│  /scan_filter_node (laser_filters)                           │
│  ├─ Range Filter: 0.1m - 20m                                │
│  ├─ Intensity Filter + Nan Removal                          │
│  └─ Output: /scan_filtered (LaserScan, 30Hz)                │
└───────────────────────────────────────────────────────────────┘
                          ↓
┌───────────────────────────────────────────────────────────────┐
│  Nav2 Navigation Stack (nav2_*)                               │
│                                                               │
│  /planner_server                                             │
│  ├─ Plugin: SmacPlannerHybrid (Hybrid A*)                   │
│  ├─ Input: /map + /goal_pose + odom TF                      │
│  └─ Output: /plan (Path, 20Hz)                              │
│                        ↓                                      │
│  /controller_server                                          │
│  ├─ Plugin: MPPIController (Omni Motion Model)              │
│  ├─ Input: /plan + /odom + /scan_filtered + odom TF         │
│  ├─ Samples: 2000 trajectories per cycle                    │
│  └─ Output: /cmd_vel (Twist, 30Hz)                          │
│                        ↓                                      │
│  /bt_navigator                                               │
│  ├─ Behavior Tree: navigate_to_pose_w_replanning            │
│  ├─ Recovery: ClearCostmap → Spin → BackUp                  │
│  └─ Goal Status Management                                  │
└───────────────────────────────────────────────────────────────┘
                          ↓
┌───────────────────────────────────────────────────────────────┐
│  Robot Control Layer (Custom + Hardware Interface)           │
│                                                               │
│  /ps5_controller_node (joy_mecanum_controller)               │
│  ├─ Input: PS5 DualSense (Bluetooth)                        │
│  ├─ Mecanum Wheel Kinematics (Vx, Vy, Wz)                  │
│  └─ Output: /cmd_vel (Manual Override)                      │
│                        ↓                                      │
│  /cmd_vel → Hardware Driver (Mecanum Wheel Controller)       │
│  ├─ FL, FR, BL, BR Wheel Speed Commands                     │
│  └─ Mecanum Inverse Kinematics                              │
└───────────────────────────────────────────────────────────────┘

3.1.3 アーキテクチャの設計思想

このシステムは5層構造で設計されています：

Hardware Layer（最下層）: 物理センサーとアクチュエータ
- ZED2i、LiDAR、メカナムホイール
Driver Layer: センサードライバ（外部パッケージ）
- zed_wrapper、ldlidar_stl_ros2（前後LiDAR共通）
- 役割分担: ハードウェア固有の通信・データ取得に専念
Perception & Mapping Layer: 環境認識と自己位置推定
- nvblox (Isaac ROS): GPU加速3Dマッピング
- ZED2i Visual SLAM: 高精度オドメトリ
- LiDAR統合: 360°障害物検知
- zed_goal_publisher (自作): ジェスチャー→ゴール変換
Navigation Layer: 経路計画と制御（Nav2）
- Planner: 大域経路計画（Hybrid A*）
- Controller: 局所経路追従（MPPI）
- BT Navigator: 高レベル判断とリカバリ
Control Layer: 実行制御（自作統合）
- joy_mecanum_controller: PS5コントローラー統合
- cmd_vel統合: ナビゲーション/手動制御の切り替え

なぜComponent Containerを使うのか:
ZED2i、nvblox、zed_goal_publisherを同一プロセス（nvblox_container）内で動作させることで：

低レイテンシ: プロセス間通信が不要、共有メモリで高速データ転送
リソース効率: Jetsonの限られたメモリ・CPUを効率的に使用
同期性: センサーデータ→マッピング→ゴール生成の一連の処理がスムーズ

外部パッケージと自作パッケージの役割分担:

外部パッケージ: 汎用的な機能（センサードライバ、マッピング、ナビゲーション）
自作パッケージ: 統合レイヤー（ジェスチャー認識、パラメータ調整、起動管理）
利点: 外部パッケージの更新を容易に取り込め、デバッグの切り分けが明確

3.2 主要ノード一覧と設計意図

ノード名	パッケージ	役割	なぜこのノードが必要か
`/zed/zed_node`	zed_wrapper	Visual Odometry、深度画像、RGB画像、IMU、Body Tracking、TF配信	本システムの中核センサー。1つのノードで位置推定、環境認識、人物認識の3役をこなす。ZED2iを選んだ最大の理由。
`/zed/zed_zupt_filter_node`	zed_wrapper	Zero-velocity UPdaTe filter（オドメトリ安定化）	ロボットが静止中にオドメトリがドリフトするのを防ぐ。「止まっている時は本当に止まっている」ことを保証し、マップのずれを防止。
`/nvblox_node`	nvblox_ros	3D TSDF構築、Occupancy Grid生成、ESDF計算	3D SLAMの心臓部。深度画像とLiDARを統合し、Nav2が使える2D Occupancy Gridを生成。Jetson GPUを活かすために選択。
`/zed_goal_publisher`	bringup	Body Trackingジェスチャーから目標位置生成	人物追従機能の実現。手を挙げる→ゴール生成の橋渡し役。このノードがあることで「手を挙げるだけで呼べる」が実現。
`/planner_server`	nav2_planner	SmacPlannerHybridによる大域経路計画	Hybrid A*で滑らかな経路を生成。メカナムホイールの動きに適した経路計画。障害物を避けつつ最短経路を探索。
`/controller_server`	nav2_controller	MPPIコントローラーによる局所経路追従	メカナムホイール専用の全方位制御。2000個の軌道候補から最適解を選び、人が近くにいても安全で予測可能な動き。
`/bt_navigator`	nav2_bt_navigator	Behavior Treeによるナビゲーション制御	「ゴール到達できない時の再計画」「障害物回避」などの高レベル判断。ロボットが「賢く振る舞う」ための脳。
`/lidar_merge_node`	scan_tools	前後LiDAR統合（360°スキャン生成）	2つのLiDARスキャンを1つの360°スキャンに統合。Nav2は単一スキャンを期待するため、この統合が必須。
`/scan_filter_node`	laser_filters	スキャンフィルタリング（範囲・強度・欠測除去）	LiDARのノイズ除去。床反射、ガラス反射などの誤検出を除去し、Nav2に「きれいなデータ」を供給。
`/ps5_controller_node`	ps5_controller	PS5コントローラー入力処理	デバッグ性の向上。開発中のパラメータ調整、動作確認、問題発生時の即座の介入。個人開発では素早い問題切り分けが重要。

4. TFツリー構成と配信責任

4.1 TFツリー全体像

map (nvbloxがグローバル座標系として使用)
 ↓ [ZED2i Visual SLAM: odom → zed_camera_origin]
odom (ZED2iが配信するオドメトリフレーム)
 ↓ [ZED2i: zed_camera_origin → zed_camera_link]
zed_camera_origin (中間オドメトリフレーム)
 ↓
zed_camera_link (カメラ原点・ロボット基準点)
 ├─ base_link (static TF: ロボット幾何中心)
 ├─ front_lidar (static TF: 前方LiDAR位置)
 ├─ back_lidar (static TF: 後方LiDAR位置)
 ├─ zed_left_camera_frame (ZED左カメラ)
 ├─ zed_right_camera_frame (ZED右カメラ)
 └─ zed_imu_link (ZED IMU)

4.2 TF配信責任の詳細

変換	配信ノード	配信タイプ	パラメータ設定	説明
`odom` → `zed_camera_origin`	`/zed/zed_node`	動的TF	`map_frame: 'odom'` `odometry_frame: 'zed_camera_origin'` `publish_tf: true`	ZED2i Visual Odometry カメラ移動を計測してオドメトリを配信
`zed_camera_origin` → `zed_camera_link`	`/zed/zed_node`	静的TF	(自動配信)	カメラ内部フレーム変換
`zed_camera_link` → `base_link`	`static_transform_publisher`	静的TF	`xyz: [0, 0, 0]` `rpy: [0, 0, 0]`	ロボット幾何中心への変換
`zed_camera_link` → `front_lidar`	`static_transform_publisher`	静的TF	`xyz: [0.15, 0, 0.1]`	前方LiDAR位置
`zed_camera_link` → `back_lidar`	`static_transform_publisher`	静的TF	`xyz: [-0.15, 0, 0.1]` `rpy: [0, 0, 3.14159]`	後方LiDAR位置（180°回転）

4.3 重要な設計ポイント

4.3.1 ZED2iのフレーム設定（critical）

# common_stereo_isaac.yaml
pos_tracking:
  publish_tf: true                    # TF配信を有効化
  publish_map_tf: true                # map→odom TFを配信（★重要★）
  map_frame: 'odom'                   # ★★★最重要★★★
  odometry_frame: 'zed_camera_origin' # 中間オドメトリフレーム

この設定の意味:

ZED2iは map_frame: 'odom' に設定されているため、内部的に「odom」フレームをグローバル基準点として扱う
ZED2iはVisual SLAMで推定した「odomからzed_camera_originへの変換」を配信
publish_map_tf: true により、ZED2iは実際には odom → zed_camera_origin のTFを配信する
nvbloxは別途 global_frame: 'map' を使用してOccupancy Gridを生成

4.3.2 なぜ `map_frame: 'odom'` なのか？ - ドリフト抑制の重要性

通常のZED2i使用方法との違い:

本システムの設定は、ZED2iの一般的な使い方とは明らかに異なります。通常は以下のように設定することが多いです：

# 一般的な設定（本システムでは採用していない）
pos_tracking:
  map_frame: 'map'                    # ZED2i内部でmapを生成
  odometry_frame: 'odom'              # odomフレームを直接配信
  publish_map_tf: true                # map→odomを配信

しかし、本システムではあえて異なる設定を採用しています：

# 本システムの設定（ドリフト抑制のため）
pos_tracking:
  map_frame: 'odom'                   # ★ZED2i内部の「map」を'odom'として扱う
  odometry_frame: 'zed_camera_origin' # ★中間フレームを生成
  publish_map_tf: true                # odom→zed_camera_originを配信

この設計の技術的理由:

odometry_frame: 'zed_camera_origin' の問題点
- ZED2iのzed_camera_originフレームは、純粋なVisual Odometryの原点
- このフレームを直接使うと、ドリフトが非常に大きい
- 累積誤差が時間とともに増大し、マップとのずれが発生
map_frame: 'odom' による解決
- ZED2i内部で生成される「map」（Visual SLAMで補正されたフレーム）を、外部に対してodomという名前で配信
- ZED2i内部のループクロージャ機能により、原点のドリフトが大幅に抑制される
- ZED2i内部で自己位置推定が完結し、高精度なodomフレームを外部に提供
zed_camera_origin は中間フレーム
- zed_camera_originは、ZED2i内部で使われる中間的なフレーム
- odom（ZED2i内部のmap） → zed_camera_origin（Visual Odometry原点）の変換で、ドリフト補正が適用される
- 外部パッケージ（nvblox、Nav2）は、この補正済みのodomフレームを使用

なぜこの設計が重要か:

他パッケージへの受け渡しが容易
- nvbloxは odom フレームを基準に動作できる
- Nav2も odom フレームを基準に経路追従できる
- すべてのパッケージが同じodomフレームを共有し、整合性が保たれる
原因の切り分けが容易
- ZED2iの自己位置推定が独立して完結
- 問題発生時、「ZED2iのオドメトリ自体に問題があるのか」「Nav2の経路追従に問題があるのか」を明確に切り分けられる
- デバッグ時の効率が大幅に向上
自己位置精度の向上
- ZED2i内部のループクロージャ機能が有効に働く
- zed_camera_originの生のオドメトリよりも、odom（ZED2i内部のmap）の方が精度が高い
- 長時間運用でもドリフトが少ない

この設計により実現される分離:

ZED2i: odom フレーム（内部mapを'odom'として配信）
- ドリフト抑制済み（ループクロージャ適用後）
- 高周波数（30Hz）
- 滑らかな動き
- 自己完結した高精度な位置推定
nvblox: map フレームを基準にしたグローバルマップを生成
- ZED2iのodomを基準に3Dマッピング
- Occupancy Gridを生成（Nav2互換）
- TF配信の責任なし（純粋なマッピングに専念）
Nav2: map フレームでゴール指定、odom フレームで経路追従
- Planner: map フレームで大域経路計画
- Controller: odom フレーム（ZED2i補正済み）で局所経路追従
- 高精度なオドメトリによる滑らかな動き

4.4 mapフレームの扱い

重要: 現在の構成では、map フレームはnvbloxが Occupancy Grid生成時の基準座標系 として使用しますが、map→odomのTF変換は配信されていません。

つまり：

nvbloxは global_frame: 'map' でOccupancy Gridを /map トピックに配信
しかし、nvbloxは map → odom のTF変換を配信しない
Nav2は map フレーム上でゴールを受け取り、odom フレームで経路を追従

この設計の利点:

ZED2i Visual Odometryの高周波数・高精度を活かせる
nvbloxは純粋にマッピングに専念（TF配信の責任なし）
Nav2は odom フレームでリアルタイムに経路追従可能

この設計の注意点:

map と odom の原点が一致していることが前提
初期位置が重要（システム起動時にロボットが原点にいる必要がある）
長時間運用するとZED2iのオドメトリドリフトが蓄積する可能性

5. 重要な設計思想: nvbloxをSLAMとして使用

5.1 設計の核心

本システムの最大の特徴は、nvbloxを純粋なSLAM（Simultaneous Localization and Mapping）として使用している点です。

従来のROS2ナビゲーション:

SLAM Toolbox → /map (Occupancy Grid) + map→odom TF配信
AMCL → 自己位置推定（パーティクルフィルタ）
Nav2 → 経路計画・追従

本システム:

nvblox → /map (Occupancy Grid) ※TF配信なし
ZED2i Visual Odometry → odom→zed_camera_origin TF配信
Nav2 → 経路計画・追従（AMCLは未使用）

5.2 nvbloxの役割

5.2.1 入力データ

nvbloxは以下のデータを統合してマップを構築：

ZED2i深度画像 (/zed/zed_node/depth/depth_registered)
- 解像度: 720x404
- レート: 15 Hz
- 範囲: 0.3m～20m
ZED2i RGB画像 (/zed/zed_node/rgb/image_rect_color)
- 色情報によるTSDF品質向上
ZED2iポーズ (/zed/zed_node/pose)
- Visual SLAMによる6DOFポーズ
- nvbloxはこのポーズを使ってセンサーデータを統合
LiDARスキャン (/scan_filtered)
- デュアルLiDAR統合（360°カバレッジ）
- レート: 30 Hz
- 範囲: 0.1m～20m

5.2.2 出力データ

Occupancy Grid (/map)
- 解像度: 5cm (voxel_size: 0.05)
- 更新頻度: 5 Hz
- 形式: nav_msgs/OccupancyGrid（Nav2互換）
- 座標系: map フレーム
TSDF点群 (/nvblox_node/mesh_visualization)
- 3D再構成結果の可視化
ESDF点群 (/nvblox_node/esdf_pointcloud)
- Euclidean Signed Distance Field
- 障害物からの距離情報

5.2.3 nvbloxの設定（重要）

# nvblox_nav2_integration.yaml
global_frame: "map"  # Occupancy Gridの座標系
voxel_size: 0.05     # 5cm解像度
mapping_type: "static_tsdf"  # 静的環境マッピング

# LiDAR統合
use_lidar: true
lidar_width: 720
integrate_lidar_rate_hz: 30.0

# Occupancy Grid生成
publish_occupancy_layer: true
publish_layer_rate_hz: 5.0

# Occupancy確率設定
static_mapper:
  free_region_occupancy_probability: 0.3     # 空き領域
  occupied_region_occupancy_probability: 0.9  # 占有領域
  unobserved_region_occupancy_probability: 0.5  # 未観測

5.3 ZED2i Visual Odometryの役割

ZED2iは 自己位置推定（localization） を担当：

# common_stereo_isaac.yaml
pos_tracking:
  pos_tracking_enabled: true
  pos_tracking_mode: 'GEN_3'  # 最新アルゴリズム
  imu_fusion: true            # IMU統合で精度向上

  # フレーム設定（★重要★）
  map_frame: 'odom'                   # 親フレーム
  odometry_frame: 'zed_camera_origin' # 子フレーム
  publish_tf: true                    # TF配信
  publish_map_tf: true                # 実際はodom→zed_camera_originを配信

  # SLAM機能
  area_memory: true                   # ループクロージャ検出
  save_area_memory_on_closing: true   # エリアメモリ保存
  reset_odom_with_loop_closure: false # オドメトリリセット無効

ZED2i Visual SLAMの特徴:

ステレオ視覚情報とIMUを融合
特徴点マッチングによる高精度オドメトリ
ループクロージャ検出（既知エリアへの復帰を認識）
map_frame: 'odom' 設定により、内部mapを外部に'odom'として配信し、ドリフトを大幅に抑制

重要な設計上の工夫 - ドリフト抑制:

ZED2iのodometry_frame: 'zed_camera_origin'（純粋なVisual Odometry原点）はドリフトが非常に大きいという問題があります。そこで本システムでは：

ZED2i内部で自己位置推定を完結させる
- map_frame: 'odom' により、ZED2i内部のSLAM補正済みフレーム（本来の'map'）を外部に対して'odom'として配信
- ループクロージャ機能により原点のドリフトが抑制される
- 外部パッケージは、この高精度な'odom'フレームを使用
zed_camera_originは中間フレームとして扱う
- odom → zed_camera_origin のTF変換に、ZED2iのSLAM補正が含まれる
- nvbloxやNav2は、この補正済みのodomフレームを基準に動作
- デバッグ時に「ZED2iの位置推定」と「Nav2の経路追従」を独立して評価可能

この設計により、他パッケージへのデータ受け渡しが容易になり、原因の切り分けが明確になります。

5.4 この設計のメリット

リアルタイム性
- ZED2iの高周波数オドメトリ（30Hz）でスムーズな経路追従
- nvbloxは低周波数（5Hz）でマップ更新（計算負荷軽減）
高精度マッピング
- nvbloxのTSDF手法により滑らかな3Dマップ
- 深度画像とLiDARの統合で環境全体をカバー
ドリフト抑制
- map_frame: 'odom' 設定により、ZED2i内部のSLAM補正を活用
- zed_camera_originの生オドメトリよりも高精度
- 長時間運用でもドリフトが少ない
シンプルな構成
- AMCLが不要（パーティクルフィルタの計算コスト削減）
- SLAM Toolboxが不要（nvbloxがマッピングを担当）
柔軟性とデバッグ性
- ZED2iとnvbloxの役割が明確に分離
- 各コンポーネントを独立して調整可能
- 問題発生時の原因切り分けが容易（ZED2iのオドメトリ自体を評価可能）

5.5 この設計の制約

初期位置の重要性
- システム起動時にロボットが原点（map座標系の原点≈odom座標系の原点）にいる必要
- 起動後に大きく移動してからの再起動は位置ずれの原因
長時間運用のドリフト
- map_frame: 'odom' 設定によりドリフトは大幅に抑制されているが、完全にゼロではない
- 数時間の連続運用では若干の位置ずれが発生する可能性
- 対策: ループクロージャ機能（area_memory: true）により、既知エリアへの復帰時に自動補正
map↔odom変換の不在
- nvbloxは map → odom のTFを配信しない
- Nav2のAMCLのような「地図上での自己位置補正」は行われない
- 前提: map と odom の原点が常に一致

6. パッケージ詳細

6.1 センサー統合（zed2i_nvblox_fixed.launch.py）

6.1.1 ZED2i Wrapper

パッケージ: zed_wrapper
ノード: /zed/zed_node

機能:

Visual Odometry（odom → zed_camera_origin TF配信）
深度画像生成（720x404, 15Hz）
RGB画像生成（720x404, 15Hz）
IMUデータ配信
Body Tracking（ジェスチャー認識）

主要トピック:

/zed/zed_node/odom - Visual Odometry
/zed/zed_node/depth/depth_registered - 深度画像
/zed/zed_node/rgb/image_rect_color - RGB画像
/zed/zed_node/pose - 6DOFポーズ
/zed/zed_node/imu/data - IMUデータ
/zed/zed_node/body_trk/skeletons - Body Tracking結果

設定ファイル:

common_stereo_isaac.yaml - 共通設定
zed2i.yaml - ZED2iモデル固有設定

6.1.2 ZED ZUPT Filter

パッケージ: zed_wrapper
ノード: /zed/zed_zupt_filter_node

機能:

Zero-velocity UPdaTe (ZUPT) フィルタ
静止時のオドメトリドリフト抑制
IMUバイアス推定

主要トピック:

入力: /zed/zed_node/odom
出力: /zed/zed_node/odom_zupt

6.1.3 nvblox Node

パッケージ: nvblox_ros
ノード: /nvblox_node

機能:

TSDF（Truncated Signed Distance Field）ベース3Dマッピング
深度画像 + LiDARの統合マッピング
Occupancy Grid生成（Nav2互換）
ESDF（Euclidean Signed Distance Field）計算

主要トピック:

入力:
- /zed/zed_node/depth/depth_registered
- /zed/zed_node/rgb/image_rect_color
- /zed/zed_node/pose
- /scan_filtered
出力:
- /map - Occupancy Grid (nav_msgs/OccupancyGrid)
- /nvblox_node/mesh_visualization - メッシュ可視化
- /nvblox_node/esdf_pointcloud - ESDF点群

設定ファイル:

nvblox_base.yaml - 基本設定
nvblox_zed.yaml - ZED統合設定
nvblox_extended_range.yaml - 拡張範囲設定
nvblox_nav2_integration.yaml - Nav2統合設定

6.1.4 LiDAR Processing

マージノード:

パッケージ: scan_tools
ノード: /lidar_merge_node
機能: 前後LiDARを360°スキャンに統合

フィルタノード:

パッケージ: laser_filters
ノード: /scan_filter_node
機能: 範囲フィルタ、強度フィルタ、欠測除去

トピック:

入力: /front_scan, /back_scan
中間: /merged_scan
出力: /scan_filtered

6.1.5 zed_goal_publisher (Body Tracking→ゴール変換)

パッケージ: bringup
ノード: /zed_goal_publisher

機能:

ZED2i Body Trackingのジェスチャー認識
ジェスチャーを目標位置に変換
/goal_pose トピックに配信

設計の意図:
このノードは、ZED2iの骨格認識データから人物の位置とジェスチャーを解析し、Nav2が理解できるgeometry_msgs/PoseStamped形式のゴール位置に変換します。「手を挙げるだけでロボットが近づいてくる」という直感的なインタラクションを実現する中核ノードです。

6.2 ナビゲーション（bringup_launch.py）

6.2.1 Planner Server

パッケージ: nav2_planner
ノード: /planner_server

機能:

大域経路計画（start → goal）
SmacPlannerHybrid使用（Hybrid A*アルゴリズム）
Occupancy Gridベースの計画

主要トピック:

入力: /map, /goal_pose
出力: /plan

設定:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0  # 計画頻度
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      tolerance: 0.5  # ゴール許容誤差
      downsample_costmap: false
      downsampling_factor: 1

6.2.2 Controller Server

パッケージ: nav2_controller
ノード: /controller_server

機能:

局所経路追従
MPPIコントローラー（Model Predictive Path Integral）
リアルタイム障害物回避

主要トピック:

入力: /plan, /odom, /scan_filtered
出力: /cmd_vel

設定:

controller_server:
  ros__parameters:
    controller_frequency: 30.0  # 制御頻度30Hz
    controller_plugins: ["FollowPath"]

    FollowPath:
      plugin: "nav2_mppi_controller/MPPIController"
      motion_model: "Omni"  # メカナムホイール用
      batch_size: 2000  # サンプル数
      time_steps: 56    # 予測ステップ数
      iteration_count: 2  # イテレーション回数

6.2.3 BT Navigator

パッケージ: nav2_bt_navigator
ノード: /bt_navigator

機能:

Behavior Treeによる高レベルナビゲーション制御
エラーリカバリ（再計画、回転、後退）
ゴール達成判定

Behavior Tree XML:

navigate_to_pose_w_replanning_and_recovery.xml
  └─ FollowPath
      ├─ ComputePathToPose（経路計画）
      ├─ FollowPath（経路追従）
      └─ Recovery Behaviors（エラー時）
          ├─ ClearCostmap（コストマップクリア）
          ├─ Spin（その場回転）
          └─ BackUp（後退）

7. トピック構成

7.1 センサートピック

7.1.1 ZED2i関連

トピック	型	頻度	説明
`/zed/zed_node/odom`	`nav_msgs/Odometry`	60 Hz	Visual Odometry（生）
`/zed/zed_node/odom_zupt`	`nav_msgs/Odometry`	60 Hz	ZUPT適用後オドメトリ
`/zed/zed_node/pose`	`geometry_msgs/PoseStamped`	60 Hz	6DOFポーズ（nvblox入力）
`/zed/zed_node/depth/depth_registered`	`sensor_msgs/Image`	15 Hz	深度画像（RGB整合済み）
`/zed/zed_node/rgb/image_rect_color`	`sensor_msgs/Image`	15 Hz	RGB画像（歪み補正済み）
`/zed/zed_node/imu/data`	`sensor_msgs/Imu`	200 Hz	IMUデータ
`/zed/zed_node/body_trk/skeletons`	`zed_msgs/SkeletonArray`	15 Hz	Body Tracking結果

7.1.2 LiDAR関連

トピック	型	頻度	説明
`/front_scan`	`sensor_msgs/LaserScan`	30 Hz	前方LiDAR生データ
`/back_scan`	`sensor_msgs/LaserScan`	30 Hz	後方LiDAR生データ
`/merged_scan`	`sensor_msgs/LaserScan`	30 Hz	360°統合スキャン
`/scan_filtered`	`sensor_msgs/LaserScan`	30 Hz	フィルタ済みスキャン

7.2 マッピング・ナビゲーショントピック

7.2.1 nvblox関連

トピック	型	頻度	説明
`/map`	`nav_msgs/OccupancyGrid`	5 Hz	Occupancy Grid（Nav2入力）
`/nvblox_node/mesh_visualization`	`visualization_msgs/Marker`	1 Hz	メッシュ可視化
`/nvblox_node/esdf_pointcloud`	`sensor_msgs/PointCloud2`	5 Hz	ESDF点群
`/nvblox_node/static_occupancy_grid`	`nav_msgs/OccupancyGrid`	5 Hz	元のOccupancy Grid

7.2.2 Nav2関連

トピック	型	説明
`/goal_pose`	`geometry_msgs/PoseStamped`	ナビゲーションゴール
`/plan`	`nav_msgs/Path`	計画された経路
`/cmd_vel`	`geometry_msgs/Twist`	ロボット速度指令
`/local_costmap/costmap`	`nav_msgs/OccupancyGrid`	局所コストマップ
`/global_costmap/costmap`	`nav_msgs/OccupancyGrid`	大域コストマップ

7.3 制御トピック

トピック	型	説明
`/cmd_vel`	`geometry_msgs/Twist`	最終速度指令（Nav2 or PS5）
`/ps5_controller/cmd_vel`	`geometry_msgs/Twist`	PS5コントローラー入力
`/ps5_controller/joy`	`sensor_msgs/Joy`	PS5生入力

8. 起動シーケンスとタイミング

8.1 起動フロー

t=0s:  zed2i_nvblox_nav2_launch.py開始
  └─ zed2i_nvblox_fixed.launch.py起動
     ├─ t=0s:   nvblox_container起動
     ├─ t=0s:   ZED2iノード読み込み
     ├─ t=2s:   ZED ZUPTフィルタ読み込み
     ├─ t=4s:   nvbloxノード読み込み
     ├─ t=5s:   Goal Selectorノード読み込み
     ├─ t=0s:   LiDARマージ・フィルタ起動
     ├─ t=0s:   Static TF Publishers起動
     ├─ t=0s:   PS5コントローラー起動
     └─ t=6s:   RViz2起動

t=8s:  bringup_launch.py起動（Nav2スタック）
  ├─ Planner Server起動
  ├─ Controller Server起動
  ├─ BT Navigator起動
  ├─ Behavior Server起動
  ├─ Waypoint Follower起動
  ├─ Velocity Smoother起動
  └─ Lifecycle Manager起動

8.2 タイミングの理由

遅延	ノード	理由
0s	ZED2i	基盤となるセンサー、最優先起動
2s	ZUPT Filter	ZED2iオドメトリが安定してから起動
4s	nvblox	ZED2iポーズ・深度画像が供給されてから起動
5s	Goal Selector	ZED2i Body Trackingが有効になってから起動
6s	RViz2	すべてのトピックが出揃ってから可視化
8s	Nav2	マップ（/map）が配信されてから起動

まとめ

本システムは、nvbloxをSLAMとして使用し、ZED2i Visual Odometryで自己位置推定を行うという独自の設計により、高精度かつリアルタイムな自律ナビゲーションを実現しています。

設計の核心ポイント

ZED2i map_frame: 'odom' 設定
- ZED2iは odom → zed_camera_origin のTFを配信
- 高周波数（60Hz）の滑らかなオドメトリ
nvblox global_frame: 'map' 設定
- nvbloxは map フレーム基準でOccupancy Gridを生成
- TF配信はせず、純粋にマッピングに専念
Nav2の活用
- map フレームでゴール指定
- odom フレームで経路追従
- AMCLは未使用（ZED2i Visual Odometryで十分）

この設計により、計算コストを抑えつつ高精度なナビゲーションを実現しています。

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up

ZED2i + nvblox + Nav2 自律ナビゲーションシステム技術書

目次

使用環境

本プロジェクトについて

AI支援開発による効率化

ソースコードについて

1. このロボットの特徴

1.1 最大の特徴: 手を挙げるだけでロボットが近づいてくる

1.2 技術的な特徴

1.2.1 NVIDIA Isaac ROS nvbloxによる高精度3Dマッピング

1.2.2 ZED2i Visual Odometryによる自己位置推定

1.2.3 デュアルLiDARによる360°障害物検知

1.2.4 Nav2 MPPI Controllerによる全方位移動

1.2.5 PS5コントローラーとBody Trackingの二重インターフェース

1.3 ハードウェア構成

1.4 使用している主要外部パッケージ

外部パッケージ一覧

自作パッケージ（統合レイヤー）

2. 起動コマンド

2.1 メインシステム起動

2.2 起動ファイル構成

3. システムアーキテクチャ

3.1 パッケージ統合とノード構成の全体像

3.1.1 パッケージ階層図

3.1.2 データフロー付き詳細ノード構成図

3.1.3 アーキテクチャの設計思想

3.2 主要ノード一覧と設計意図

4. TFツリー構成と配信責任

4.1 TFツリー全体像

4.2 TF配信責任の詳細

4.3 重要な設計ポイント

4.3.1 ZED2iのフレーム設定（critical）

4.3.2 なぜ map_frame: 'odom' なのか？ - ドリフト抑制の重要性

4.4 mapフレームの扱い

5. 重要な設計思想: nvbloxをSLAMとして使用

5.1 設計の核心

5.2 nvbloxの役割

5.2.1 入力データ

5.2.2 出力データ

5.2.3 nvbloxの設定（重要）

5.3 ZED2i Visual Odometryの役割

5.4 この設計のメリット

5.5 この設計の制約

6. パッケージ詳細

6.1 センサー統合（zed2i_nvblox_fixed.launch.py）

6.1.1 ZED2i Wrapper

6.1.2 ZED ZUPT Filter

6.1.3 nvblox Node

6.1.4 LiDAR Processing

6.1.5 zed_goal_publisher (Body Tracking→ゴール変換)

6.2 ナビゲーション（bringup_launch.py）

6.2.1 Planner Server

6.2.2 Controller Server

6.2.3 BT Navigator

7. トピック構成

7.1 センサートピック

7.1.1 ZED2i関連

7.1.2 LiDAR関連

7.2 マッピング・ナビゲーショントピック

7.2.1 nvblox関連

7.2.2 Nav2関連

7.3 制御トピック

8. 起動シーケンスとタイミング

8.1 起動フロー

8.2 タイミングの理由

まとめ

設計の核心ポイント

4.3.2 なぜ `map_frame: 'odom'` なのか？ - ドリフト抑制の重要性