fix(navigation): signed momentum term so U-turns cost

dimos/navigation/frontier_exploration/test_wavefront_frontier_scoring.py

@@ -0,0 +1,87 @@
+"""Scoring tests for the direction momentum term of the wavefront selector.
+
+Three candidate frontiers are placed at the same distance from the robot, with
+the same cluster size, on an empty map with no explored goals, so every term of
+the score except direction momentum is identical between them: one straight
+ahead of the current exploration direction, one sideways, one straight behind.
+"""
+
+import numpy as np
+import pytest
+
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid
+from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import (
+    WavefrontConfig,
+    WavefrontFrontierExplorer,
+)
+
+RESOLUTION = 0.05
+FRONTIER_SIZE = 40
+
+ROBOT = Vector3(5.0, 5.0, 0.0)
+AHEAD = Vector3(7.0, 5.0, 0.0)  # dot product +1 with the exploration direction
+SIDEWAYS = Vector3(5.0, 7.0, 0.0)  # dot product 0
+BEHIND = Vector3(3.0, 5.0, 0.0)  # dot product -1
+
+
+def open_costmap() -> OccupancyGrid:
+    """A 10 m x 10 m free map, no obstacle within any candidate's safe radius."""
+    grid = np.full((200, 200), CostValues.FREE, dtype=np.int8)
+    return OccupancyGrid(grid=grid, resolution=RESOLUTION, frame_id="world")
+
+
+def score_candidates(**config_kwargs) -> dict[str, float]:
+    """Score the three candidates with a selector heading in +x."""
+    explorer = WavefrontFrontierExplorer.__new__(WavefrontFrontierExplorer)
+    explorer.config = WavefrontConfig(**config_kwargs)
+    explorer.explored_goals = []
+    explorer.exploration_direction = Vector3(1.0, 0.0, 0.0)
+
+    costmap = open_costmap()
+    return {
+        name: explorer._compute_comprehensive_frontier_score(
+            frontier, FRONTIER_SIZE, ROBOT, costmap
+        )
+        for name, frontier in (("ahead", AHEAD), ("sideways", SIDEWAYS), ("behind", BEHIND))
+    }
+
+
+def test_signed_momentum_makes_a_u_turn_cost():
+    scores = score_candidates()
+    weight = WavefrontConfig().momentum_weight
+
+    assert scores["ahead"] > scores["sideways"] > scores["behind"]
+    # The candidates differ only in direction, so the whole gap is the momentum
+    # term: +weight straight ahead against -weight straight behind.
+    assert scores["ahead"] - scores["behind"] == pytest.approx(2 * weight)
+
+
+def test_clamped_momentum_prices_a_u_turn_like_a_sideways_move():
+    """The previous behavior, still reachable through the config."""
+    scores = score_candidates(min_momentum_score=0.0)
+
+    assert scores["behind"] == scores["sideways"]
+    assert scores["ahead"] > scores["behind"]
+
+
+def test_momentum_weight_scales_the_direction_gap():
+    quiet = score_candidates(momentum_weight=0.0)
+    loud = score_candidates(momentum_weight=0.5)
+
+    assert quiet["ahead"] == quiet["behind"]
+    assert loud["ahead"] - loud["behind"] == pytest.approx(1.0)
+
+
+def test_zeroed_direction_neutralizes_momentum_after_a_timeout():
+    """The timeout branch zeroes exploration_direction, so the ranking right
+    after a timeout must carry no directional preference at all."""
+    explorer = WavefrontFrontierExplorer.__new__(WavefrontFrontierExplorer)
+    explorer.config = WavefrontConfig()
+    explorer.explored_goals = []
+    explorer.exploration_direction = Vector3(0.0, 0.0, 0.0)
+
+    costmap = open_costmap()
+    ahead = explorer._compute_comprehensive_frontier_score(AHEAD, FRONTIER_SIZE, ROBOT, costmap)
+    behind = explorer._compute_comprehensive_frontier_score(BEHIND, FRONTIER_SIZE, ROBOT, costmap)
+    assert ahead == pytest.approx(behind)

dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py

@@ -91,6 +91,8 @@ class WavefrontConfig(ModuleConfig):
     info_gain_threshold: float = 0.03
     num_no_gain_attempts: int = 2
     goal_timeout: float = 15.0
+    momentum_weight: float = 0.3
+    min_momentum_score: float = -1.0
 
 
 class WavefrontFrontierExplorer(Module):
@@ -463,8 +465,11 @@ def _compute_direction_momentum_score(self, frontier: Vector3, robot_pose: Vecto
             + self.exploration_direction.y * frontier_direction.y
         )
 
-        # Return momentum score (higher for same direction, lower for opposite)
-        return max(0.0, dot_product)  # Only positive momentum, no penalty for different directions
+        # Return momentum score (higher for same direction, lower for opposite).
+        # Signed: +1 straight ahead, 0 sideways, -1 a full U-turn. Clamping at
+        # 0.0 priced a reversal exactly like a sideways move, so turning around
+        # was free. Set min_momentum_score to 0.0 to restore that behavior.
+        return max(self.config.min_momentum_score, dot_product)
 
     def _compute_distance_to_explored_goals(self, frontier: Vector3) -> float:
         """Compute distance from frontier to the nearest explored goal."""
@@ -557,20 +562,22 @@ def _compute_comprehensive_frontier_score(
         else:
             obstacles_score = obstacles_distance / self.config.safe_distance  # Linear penalty
 
-        # 5. Direction momentum (already in 0-1 range from dot product)
+        # 5. Direction momentum (in -1 to 1 range from the signed dot product)
         momentum_score = self._compute_direction_momentum_score(frontier, robot_pose)
 
         logger.info(
             f"Distance score: {distance_score:.2f}, Info gain: {info_gain_score:.2f}, Explored goals: {explored_goals_score:.2f}, Obstacles: {obstacles_score:.2f}, Momentum: {momentum_score:.2f}"
         )
 
-        # Combine scores with consistent scaling
+        # Combine scores with consistent scaling. Frontiers are only ever ranked
+        # against each other (see _rank_frontiers), never against an absolute
+        # threshold, so the weights do not have to sum to 1.
         total_score = (
             0.3 * info_gain_score  # 30% information gain
             + 0.3 * explored_goals_score  # 30% distance from explored goals
             + 0.2 * distance_score  # 20% distance optimization
             + 0.15 * obstacles_score  # 15% distance from obstacles
-            + 0.05 * momentum_score  # 5% direction momentum
+            + self.config.momentum_weight * momentum_score  # signed direction momentum
         )
 
         return total_score
@@ -825,7 +832,15 @@ def _run_exploration_loop(self) -> None:
                 if goal_reached:
                     logger.info("Goal reached, finding next frontier")
                 else:
-                    logger.warning("Goal timeout after 30 seconds, finding next frontier anyway")
+                    logger.warning(
+                        f"Goal timeout after {self.config.goal_timeout:g} seconds, "
+                        "finding next frontier anyway"
+                    )
+                    # A goal we failed to reach must not keep steering the next
+                    # ranking: zero the exploration direction so the momentum
+                    # term is neutral for the selection right after a timeout.
+                    # It re-establishes itself on the next chosen goal.
+                    self.exploration_direction = Vector3(0.0, 0.0, 0.0)
             else:
                 consecutive_failures += 1