fix(crane_world_model_publisher): ボール較正の速度計算で平滑化/生位置混在を修正

[HansRobo]

概要

crane_world_model_publisher のボール較正（オフラインキャリブレーションツール）における Vision 生データからの速度計算で、平滑化済みの現在位置と生の前回位置を混在させて差分していた問題を修正します。

問題

ball_calibration_data_extractor.cpp の速度計算（rosbag からの抽出処理）では、現在位置に 5 点移動平均（applySmoothingFilter / applySmoothingFilterScalar）を適用した smoothed_pos と、平滑化前の生の前回位置 prev_ball.pos の差分を取って速度を算出していました。

差分を取る 2 端点の平滑化状態が一致していないため、速度にバイアスが入ります。検証では符号反転かつ約 0.2 倍の過小評価となるケースが確認されました。この誤差はキック検出の速度閾値や較正データの品質ゲートに影響します。

原因

applySmoothingFilter は「現在位置 + 過去 4 点」の重み付き移動平均であり、戻り値の現在位置は過去側へ引っ張られます。この過去側に引っ張られた現在位置と、平滑化を施していない前回位置を混在させて差分していたことが原因です。速度差分の両端点で平滑化の有無が非対称になっていました。

修正内容

速度差分の両端点の平滑化状態を一致させるため、現在位置・前回位置とも生の Vision 位置同士で速度を計算するようにしました。

• 速度差分を vision_ball.pos - prev_ball.pos（生の現在位置 − 生の前回位置）に変更
• z 方向も同様に vision_ball.pos_z - prev_ball.pos_z に変更
• 速度計算に用いていた applySmoothingFilter / applySmoothingFilterScalar の呼び出しを除去

差分計算後の妥当性チェック・外れ値除去（validateAndFilterVelocity 等）はそのまま維持しています。修正は当該速度計算箇所に限定しています。

検証

cwm の独立オーバーレイ worktree 上で、対象パッケージ crane_world_model_publisher の colcon build（--no-rdeps）によりコンパイルが通ることを確認しました（Build complete.、エラーなし）。

レビュー観点

• 本変更はオフラインの較正ツールに対する修正です。
• 速度の両端点を一貫させたことで、キック検出の速度閾値・品質ゲートへ与えていたバイアス（符号反転・過小評価）が解消されることを確認してください。

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