fix(crane_world_model_publisher): Vision単独検出時のボール速度0固定を修正

[HansRobo]

概要

crane_world_model_publisher において、Tracker 未検出かつ Vision 検出のみの状況でボール速度が 0 に固定されてしまう不具合を修正します。Vision 連続フレームの有限差分による速度推定フォールバックを実装しました。

問題

world_model_data_provider.cpp の integrateBallInfo() では、Tracker が未検出で Vision のみ検出している場合に ball_info_.velocity と ball_info_.velocity_norm を 0 に固定していました。

その直後のボール状態判定 velocity_norm < 0.1 により、実際にはボールが高速移動していても常に STOPPED と判定されてしまいます。これにより Tracker 障害時に下流スキルからボールの移動情報が失われ、ルーズボール検知失敗などの問題を招いていました。

原因

Vision 単独経路に速度推定のフォールバックが存在せず、速度を無条件に 0 固定していたためです。前回 Vision 位置・時刻を保持する仕組みも無く、有限差分による速度算出ができませんでした。

修正内容

• world_model_data_provider.hpp に前回 Vision ボール位置・時刻・有効フラグを保持するメンバ (prev_vision_ball_position_, prev_vision_ball_stamp_, prev_vision_ball_valid_) を追加。
• integrateBallInfo() の Vision 単独検出経路で、(現在pos - 前回pos) / dt による有限差分で速度を推定するフォールバックを実装。
  • 前回値が無い初回、または dt <= 0 の場合のみ速度 0 とする。
  • Vision ノイズ・誤検出による非現実的な速度を抑制するため、SSL ボール最高速 6.5 m/s を上限とするクランプ（スケールダウン）と非有限値ガードを追加。
• Vision 生データ更新時に、次回推定のため前回 Vision 位置・時刻を更新。

検証

cwm の独立オーバーレイ worktree 上で対象パッケージの colcon build (--no-rdeps) を実施し、コンパイルが正常に完了することを確認しました（Build complete.）。状態推定ロジックの変更のため、実機・シミュレーションでの挙動確認は別途推奨します。

レビュー観点

• 状態推定（ボール速度）の変更です。前回 Vision 位置・時刻保持メンバの追加が伴うためレビュー必須です。
• 有限差分の dt 算出に Vision のタイムスタンプ (last_detect_time) を用いており、フレーム間隔が正しく反映されるか。
• 速度クランプ上限値 (6.5 m/s) の妥当性。

本PRはソースコード監査ワークフローで検出・敵対的検証されたバグに対する単一修正です。