From 220923b71b2dca4ab1f659aa5a4cfbac019e3098 Mon Sep 17 00:00:00 2001 From: Axel Peytavin Date: Mon, 24 Aug 2026 14:05:14 -0700 Subject: [PATCH] perf(search): let coverage dominate sweep route cost --- .../test/test_household_resident_roster.py | 4 ++-- workspace/innate_skills/find_next_person.py | 14 ++++++-------- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/ros2_ws/src/brain/brain_client/test/test_household_resident_roster.py b/ros2_ws/src/brain/brain_client/test/test_household_resident_roster.py index 5dfb3e7f7..74b3b4d1e 100644 --- a/ros2_ws/src/brain/brain_client/test/test_household_resident_roster.py +++ b/ros2_ws/src/brain/brain_client/test/test_household_resident_roster.py @@ -2509,11 +2509,11 @@ def visible(_plan, row, col, _theta): assert (view.row, view.col) == same_side -def test_sweep_information_utility_prefers_efficient_gain_over_long_detour(): +def test_sweep_information_utility_lets_extra_coverage_outweigh_a_detour(): nearby = search_module._coverage_travel_utility(40, 2.0, sweeping=True) distant = search_module._coverage_travel_utility(50, 8.0, sweeping=True) - assert nearby > distant + assert distant > nearby assert search_module._coverage_travel_utility(50, 0.0, sweeping=True) == 50 diff --git a/workspace/innate_skills/find_next_person.py b/workspace/innate_skills/find_next_person.py index 2b94418af..8e20bd408 100644 --- a/workspace/innate_skills/find_next_person.py +++ b/workspace/innate_skills/find_next_person.py @@ -44,9 +44,8 @@ MAX_VIEWPOINTS = 240 MIN_NEW_CELLS = 6 INITIAL_TRAVEL_COST_CELLS_PER_M = 12.0 -SWEEP_INFORMATION_DECAY_PER_M = 0.35 -SWEEP_NOVELTY_BONUS_CELLS_PER_M = 2.0 -SWEEP_NOVELTY_BONUS_MAX_M = 2.0 +SWEEP_TRAVEL_COST_CELLS_PER_M = 1.0 +SWEEP_NOVELTY_BONUS_CELLS_PER_M = 10.0 SWEEP_BACKTRACK_PENALTY_CELLS = 12.0 HANDLED_PERSON_ESTIMATED_DISTANCE_M = 1.5 HANDLED_PERSON_VIEW_PENALTY_CELLS = 220.0 @@ -395,10 +394,9 @@ def _distance_from_observations(x: float, y: float, observations: list[dict]) -> def _coverage_travel_utility(gain: int, route_distance_m: float, *, sweeping: bool) -> float: - """Score information against actual route cost without rewarding long trips.""" - if not sweeping: - return gain - INITIAL_TRAVEL_COST_CELLS_PER_M * route_distance_m - return gain / (1.0 + SWEEP_INFORMATION_DECAY_PER_M * route_distance_m) + """Keep the first view local, then let coverage gain dominate route length.""" + travel_cost = SWEEP_TRAVEL_COST_CELLS_PER_M if sweeping else INITIAL_TRAVEL_COST_CELLS_PER_M + return gain - travel_cost * route_distance_m def _backtrack_penalty(x: float, y: float, observations: list[dict]) -> float: @@ -534,7 +532,7 @@ def score_candidates(candidates: list[tuple[int, int]]) -> _View | None: ) score = ( _coverage_travel_utility(gain, evaluation_distance, sweeping=bool(observations)) - + SWEEP_NOVELTY_BONUS_CELLS_PER_M * min(novelty, SWEEP_NOVELTY_BONUS_MAX_M) + + SWEEP_NOVELTY_BONUS_CELLS_PER_M * novelty - _backtrack_penalty(evaluation_x, evaluation_y, observations) - 0.5 * _angular_distance(theta, pose.theta) - person_view_penalty