crane_world_model_publisher¶

概要¶

**crane_world_model_publisher**は、Craneロボットサッカーシステムの中核となる知覚・状態推定コンポーネントです。SSL Visionカメラからの生データを処理し、ボール位置、ロボット位置、フィールド幾何学を含む統一世界モデルを生成する中央ハブとして機能します。EKFベースの高精度ボールトラッキング、3D物理モデル、リアルタイム処理により、全ての高次自律動作の基盤となる信頼性の高い世界状態情報を提供します。

主要機能¶

SSL Vision処理: カメラデータからの物体認識・位置推定
3Dボール物理モデル: 空気抵抗・重力を考慮した高精度ボール軌道予測
ボールトラッキング: 状態依存（STOPPED/ROLLING/FLYING）の追跡システム
ロボット状態推定: 位置・姿勢・速度の統合推定
統合世界モデル配信: システム全体への状態情報提供

アーキテクチャ上の役割¶

Craneシステムの**認識層の中核**として、生のセンサーデータから構造化された世界状態を生成し、上位の戦略・制御層に情報を提供します。リアルタイム制約下で高精度な状態推定を実現する重要コンポーネントです。

コンポーネント構成¶

メインノード¶

WorldModelPublisherComponent: 統合世界モデル配信コンポーネント

コアクラス¶

BallPhysicsModel: 3D物理ボールモデル
- 放物運動シミュレーション
- 空気抵抗・重力計算
- 状態遷移管理（停止→転がり→飛行）

BallTracker: ボール追跡システム
- カルマンフィルター実装
- 予測・更新サイクル
- 状態依存追跡

VisionDataProcessor: Vision データ前処理
- カメラデータ統合
- ノイズフィルタリング
- 座標系変換

TrackerDataProcessor: 追跡データ処理
- トラッキング結果統合
- 状態履歴管理

VisualizationDataHandler: 可視化データ生成
- デバッグ情報配信
- 軌道予測表示

WorldModelDataProvider: 世界モデルデータ提供
- 統合データ生成
- メッセージ配信管理

ボール物理モデル詳細¶

状態定義¶

enum BallState {
  STOPPED,   // 静止状態（摩擦による完全停止）
  ROLLING,   // 転がり状態（地面接触・摩擦あり）
  FLYING     // 飛行状態（3D放物運動・空気抵抗）
};

3D物理方程式¶

重力: g = 9.81 m/s²
空気抵抗: F_drag = -k * v * |v|（速度二乗比例）
地面摩擦: F_friction = μ * N（転がり摩擦）

予測アルゴリズム¶

// 飛行状態での3D軌道予測
Vector3 predictPosition(double dt) {
  // 重力・空気抵抗を考慮した数値積分
  // 地面衝突検出・バウンド計算
  // 状態遷移判定
}

依存関係¶

コア依存¶

crane_geometry: 幾何学計算ライブラリ
crane_physics: 物理計算・ボールモデル
crane_msg_wrappers: メッセージ変換ユーティリティ
crane_msgs: システムメッセージ定義
robocup_ssl_msgs: SSL公式プロトコルメッセージ

システム依存¶

boost: 高性能C++ライブラリ
eigen: 線形代数ライブラリ（一部使用）
libgoogle-glog-dev: ログ出力
diagnostic_updater: システム診断

使用方法¶

基本起動¶

# 単体起動
ros2 run crane_world_model_publisher world_model_publisher_node

# データパイプライン起動
ros2 launch crane_bringup data.launch.py

トピック構成¶

# 購読トピック
/vision: robocup_ssl_msgs/msg/SSL_WrapperPacket
/referee: robocup_ssl_msgs/msg/Referee

# 配信トピック  
/world_model: crane_msgs/msg/WorldModel
/ball_info: crane_msgs/msg/BallInfo
/robot_info: crane_msgs/msg/RobotInfo

パラメータ設定例¶

world_model_publisher:
  ros__parameters:
    ball_physics:
      gravity: 9.81
      air_resistance: 0.01
      bounce_factor: 0.7
    tracking:
      position_noise: 0.01
      velocity_noise: 0.1

更新頻度: 60Hz（SSL Vision同期）
予測範囲: 最大5秒先までの軌道予測
レイテンシ: <10ms（認識→配信）

精度特性¶

位置精度: ±1cm（静止物体）
速度精度: ±0.1m/s（移動物体）
予測精度: 1秒後±5cm（飛行ボール）

トラブルシューティング¶

よくある問題¶

Vision信号なし: カメラ接続・ネットワーク確認
トラッキングロスト: パラメータ調整・環境光確認
予測精度低下: 物理パラメータ再キャリブレーション

関連パッケージ: crane_geometry | crane_physics | crane_game_analyzer | robocup_ssl_comm