fix(crane_geometry): ボール接近計算のゼロ長normalized()によるNaNをガード

[HansRobo]

概要

crane_geometry のボール回り込みアプローチ計算において、退化入力（ボールとロボット・目標が同位置等）でゼロ長ベクトルを正規化し NaN が発生する不具合を修正します。

問題

utility/crane_geometry/include/crane_geometry/geometry_operations.hpp の以下の箇所で、ゼロ長ベクトルに対して Eigen::Vector2d::normalized() を呼び出すと NaN が生成され、progress 計算やアプローチ目標位置の算出が破綻します。

• computeAroundBallApproachTarget（239行付近）: (ball - desired_opposite).normalized() がボールと目標の同位置で 0/0 = NaN。
• computeAroundBallApproachTargetDynamic（275, 276行付近）: (desired_opposite - ball).normalized() および (from - ball).normalized() が同位置入力で NaN となり、progress が破綻。

原因

Eigen::Vector2d::normalized() はノルム 0 に対する保護を行わず、0/0 = NaN を返します。これらの呼び出し前にゼロ長判定が存在しませんでした。

修正内容

対象の各 normalized() 呼び出しの前にノルムのゼロ判定（しきい値 1e-9）を追加し、退化入力時に安全なフォールバックを返すようにしました。

• computeAroundBallApproachTarget:
  • (ball - desired_opposite) がゼロ長の場合、from 方向へオフセットした点を返す。from 方向もゼロ長であれば既定方向 (offset, 0) を返す。
  • 非ゼロ時は既存処理どおり手動正規化した方向を使用。
  • なお 243行の (result.closest_point - ball).normalized() は result.distance > epsilon の分岐内でのみ実行されるため、ノルムが epsilon（既定 1e-4）を超えることが保証されており追加ガードは不要です。
• computeAroundBallApproachTargetDynamic:
  • (desired_opposite - ball) と (from - ball) のいずれかがゼロ長の場合、progress = 0（初期状態 = max_offset 側）にフォールバックし、dot 計算を回避。
  • 両者が非ゼロのときのみ手動正規化して内積・進捗を計算。

なお getIntersections（135, 142行）と getSeparatedPoints（212行）は別PRで対応するため本PRでは変更していません。

検証

• cwm の独立オーバーレイ worktree 上で colcon build（--no-rdeps）を実行し、crane_geometry がコンパイル成功することを確認（Build complete.、エラーなし）。
• 当該パッケージのユニットテストを実行し、全テストがパスすることを確認。
  • colcon test --packages-select crane_geometry: 100% tests passed, 0 tests failed out of 4（gtest 2件・copyright・xmllint）。
  • colcon test-result --verbose: 23 tests, 0 errors, 0 failures, 0 skipped。

レビュー観点

• フォールバック挙動の妥当性。
  • computeAroundBallApproachTarget: 目標方向が定義できないとき from 方向、さらに不定なら既定方向 (offset, 0) を使う妥当性。
  • computeAroundBallApproachTargetDynamic: 退化入力時に progress = 0（初期=大回り側）とみなす妥当性。

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