fix(crane_local_planner): ボール回避でtarget_pos==ball_pos時のNaNをガード

[HansRobo]

概要

crane_local_planner のボール回避調整 RVO2Planner::adjustForBallAvoidance において、目標位置とボール位置が一致する場合に NaN が発生し、target_pos が汚染される不具合を修正します。

問題

rvo2_planner.cpp のボール回避処理で、目標位置 target_pos とボール位置 ball_pos が一致(またはほぼ一致)している場合に NaN が生成され、補正後の target_pos が NaN に汚染されます。

attacker など「ボールそのものを目標位置にするスキル」では、ボール回避を有効にしたままボール位置へ到達しうるため、この条件に容易に到達します。下流に再 NaN チェックが無いため、汚染された target_pos がそのまま伝播します。

原因

if ((target_pos - ball_pos).norm() < MIN_BALL_DISTANCE) {
  target_pos = ball_pos + (target_pos - ball_pos).normalized() * MIN_BALL_DISTANCE;
}

条件 (target_pos - ball_pos).norm() < MIN_BALL_DISTANCE は norm() == 0(完全一致)を含みます。このとき (target_pos - ball_pos) はゼロ長ベクトルとなり、Eigen::normalized() がゼロ長ベクトルに対して各成分 0/0 を計算するため NaN を返します。結果として target_pos が NaN になります。

修正内容

normalized() を呼び出す前にノルムのゼロ判定(> 1e-9)を追加しました。ノルムがゼロ近傍(目標位置とボール位置がほぼ一致)の場合は、方向が定義できないためボール回避調整をスキップし target_pos を変更しません(フォールバック挙動)。これにより NaN の発生を防ぎます。

if (const double diff_norm = (target_pos - ball_pos).norm(); diff_norm < MIN_BALL_DISTANCE) {
  // target_pos と ball_pos が一致(diff_norm==0)する場合、normalized() がゼロ長ベクトルに対して
  // NaN を生成し target_pos を汚染するため、ゼロ近傍ではボール回避調整をスキップする
  if (diff_norm > 1e-9) {
    target_pos = ball_pos + (target_pos - ball_pos).normalized() * MIN_BALL_DISTANCE;
  }
}

修正は当該 1 箇所のみに限定し、無関係な整形やリファクタは行っていません。

検証

cwm の独立オーバーレイ worktree 上で colcon build(--no-rdeps)を実行し、crane_local_planner パッケージが正常にコンパイルされることを確認しました(Build complete.)。

レビュー観点

• フォールバック挙動の妥当性: 目標位置とボール位置がほぼ一致する場合、ボール回避調整をスキップして target_pos を変更しない方針としています。この挙動が意図に沿うかご確認ください。
• 閾値 1e-9 の妥当性。

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