fix(crane_local_planner): 配置エリア回避でセグメント上のNaNをガード

[HansRobo]

概要

crane_local_planner の配置エリア回避処理において、ロボットの現在位置が配置ラインセグメント上にある場合に目標位置が NaN となり、出力コマンドへ伝播する不具合を修正します。

問題

rvo2_planner.cpp の adjustForPlacementAvoidance() 内（旧 837-840 行付近）で、getClosestPointAndDistance(placement_area.segment, current_pos) は current_pos が配置ラインセグメント上にあるとき closest_point == current_pos を返します。

このとき (current_pos - closest_point).normalized() はゼロ長ベクトルの正規化となり NaN を生成し、target_position 全体が NaN になります。NaN はそのまま target_pos に代入され、出力されるロボットコマンドに伝播してしまいます。

原因

• Eigen::Vector2d::normalized() がゼロ長ベクトルに対して未保護であること。
• INPUT_VALIDATION は調整前の入力のみを検証しており、配置エリア回避の調整処理内部で生成される NaN を捕捉できないこと。

修正内容

normalized() を呼ぶ前にゼロ長判定（norm() > 1e-6）を導入しました。

• 脱出方向ベクトル escape_diff = current_pos - closest_point の長さがしきい値より大きい場合は従来どおり escape_diff.normalized() を脱出方向に用いる。
• ゼロ長（current_pos がセグメント上）の場合は、配置ラインセグメントの法線方向（getVerticalVec((segment.second - segment.first).normalized())）を安全な脱出方向としてフォールバックする。

これにより escape_dir は常に有限の単位ベクトルとなり、target_position への NaN 伝播を防ぎます。逆方向（フィールド外候補が無効な場合）の算出も、同じ escape_dir を符号反転して再利用することで同様に NaN を防止しています。

検証

cwm 独立オーバーレイ worktree 上で colcon build（--no-rdeps）によるコンパイル確認を実施しました。crane_local_planner のビルドが成功（Build complete.）し、本修正によるコンパイルエラーが無いことを確認しました（出力された stderr は CMake の互換性に関する deprecation warning のみ）。

レビュー観点

crane_sessions/include/crane_sessions/placement_avoidance_session.hpp の 84-87 行付近にも、normalized() をゼロ長未保護で用いる同一パターンが存在します。本 PR では対象バグに限定して修正していますが、同様の NaN 生成リスクがあるため別途の対応をご検討ください。

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