fix(crane_physics): 単一ロボット割当でコストを無視してtarget0を固定する不具合を修正

[HansRobo]

概要

crane_physics の位置割当ロジック (position_assignments.hpp) において、ロボットが1台のみのケースで距離・コストを一切評価せず無条件にターゲットインデックス {0} を返していた不具合を修正します。

問題

getOptimalAssignments（距離ベース）および getOptimalAssignmentsWithCost（カスタムコストベース）の early-return 分岐で、ロボットが1台のとき常に {0} を返していました。複数ターゲットが存在する場合でも最適ターゲットが index0 とは限らないため、より遠い（あるいはコストの高い）ターゲットへ誤って割り当てられる可能性がありました。

原因

1台時の early-return で argmin を計算せず、ハードコードした {0} を返していたためです。

修正内容

ロボットが1台のとき、ターゲット集合に対して最小コストとなるインデックスを argmin で選択して返すように変更しました。

• getOptimalAssignments: bg::distance(robot_positions[0], targets[j]) が最小となるターゲットインデックスを選択。
• getOptimalAssignmentsWithCost: cost_func(0, j) が最小となるターゲットインデックスを選択。

ターゲットが1個の場合は従来通り {0} を返します（argmin の結果が0になるため動作は変わりません）。Hungarian アルゴリズムを呼ぶ複数ロボット時のパスは変更していません。

検証

• cwm 独立オーバーレイ worktree 上で colcon build（cwm ws build --no-rdeps）を実行し、crane_physics のコンパイルが成功することを確認（Build complete.、エラーなし）。
• 当該パッケージのユニットテストを実行し、全テストが成功することを確認（118 tests, 0 errors, 0 failures, 0 skipped。gtest 10 ケースを含む）。
• 既存テスト SingleRobot はロボット (0,0) に対し target0 (1,1) が最近傍であるため、argmin の結果も {0} となり従来通りパスすることを確認。

レビュー観点

• test/test_position_assignments.cpp の SingleRobot 期待値（最近傍が target0 のケース）が引き続き成立すること。
• 1台かつ複数ターゲット時に argmin が正しく最小コスト/距離のインデックスを選ぶこと。
• ターゲット1個時に従来同様 {0} を返すこと。

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