fix(crane_local_planner): 未指令の味方RVOエージェント残留(幽霊障害物)を修正

[HansRobo]

概要

crane_local_planner の RVO2 プランナにおいて、指令が止まった味方ロボットが RVO シミュレータ上に前回位置のまま残り、他の味方の経路計画に対する「幽霊障害物」となる不具合を修正します。

問題

rvo2_planner.cpp の reflectWorldToRVOSim は、味方エージェント (0-19) のうち msg.robot_commands に含まれるものだけを更新していました。一方、敵エージェント (20-39) は available() が false の場合に (20,20) へ退避して速度0にする処理が存在します。

味方側にはこの退避処理が無いため、指令が止まった (退場・通信断などで robot_commands から外れた、あるいは available() が false になった) 味方エージェントが RVO シミュレータ内に前回位置のまま残留し、他の味方ロボットの経路計画に影響する幽霊障害物となっていました。

原因

RVO シミュレータは永続的に保持されるため、各サイクルで更新されなかったエージェントは前回状態を維持します。敵側には未活性エージェントの退避処理があるのに対し、味方側には同等の処理が欠けており、処理が非対称でした。

修正内容

reflectWorldToRVOSim に、敵側と対称な味方エージェントの退避処理を追加しました。

• 味方コマンド処理ループ内で、今サイクルで指令された味方ロボット ID を std::array<bool, 20> で記録。
• コマンド処理後、world_model->ours().robots を走査し、今サイクルで指令が無い、または available() が false の味方ロボットを敵側と同様に位置 (20,20)・速度0へ退避。

これにより、指令の止まった味方エージェントが幽霊障害物として残ることを防ぎます。

検証

cwm の独立オーバーレイ worktree 上で colcon build（--no-rdeps）による crane_local_planner のコンパイル確認を実施し、ビルド成功 (Build complete.) を確認しました。

レビュー観点

RVO 挙動の変更を含むため、レビュー必須です。特に以下を確認してください。

• 味方退避条件（未指令 / available() が false）が意図通りか。
• 退避先座標 (20,20)・速度0 が敵側と対称で妥当か。
• 指令が再開された際に正しく通常の更新経路へ戻るか。

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