Corridor-aware planner: boundary tracking, trap-avoidance, race-line heuristic, tests & Docker

f1tenth_ws/src/gap_finder/Dockerfile

@@ -0,0 +1,12 @@
+FROM python:3.11-slim
+
+WORKDIR /workspace
+
+RUN pip install --no-cache-dir numpy pytest
+
+COPY gap_finder ./gap_finder
+COPY tests ./tests
+
+ENV PYTHONPATH=/workspace
+
+CMD ["pytest", "-q", "tests"]

f1tenth_ws/src/gap_finder/README.md

@@ -1,65 +1,42 @@
 # Gap Finder
 
-This package contains "follow the gap" methods
+`gap_finder_base.py` now uses a corridor-aware planner that scores candidate steering directions instead of simply picking the deepest LiDAR gap.
 
-## gap_finder_base
+## Algorithm summary
 
--   Disparity extender ++
+1. **Sanitize + crop LiDAR** to front FOV.
+2. **Boundary extraction** computes left/right wall proximity and confidence from scan continuity.
+3. **Safe corridor model** constrains valid goal angles to lie between left/right detected boundaries (when confidence is high).
+4. **Trap detection** penalizes directions where one side suddenly opens with low confidence while the opposite wall remains stable.
+5. **Race-line heuristic** applies a filtered outside-inside-outside bias from local curvature asymmetry.
+6. **Goal scoring** combines clearance, corridor validity, trap penalty, smoothness to previous steering, and race-line bias.
+7. **Conservative fallback** (missing boundary): reduced speed and bounded steering toward the known-safe side.
 
-### Description
+## Tunable parameters
 
-This method borrows hevily from the disparity extender method but employs additional signal processing on the laser scans to produce a more stable planner.
+Defined in `scripts/gap_finder_base.py`:
 
-### Components
+- `track_width_estimate`
+- `boundary_min_points`
+- `boundary_confidence_threshold`
+- `trap_range_jump_threshold`
+- `race_line_bias_strength`
+- `steering_smoothing_alpha`
+- `speed_reduction_on_low_confidence`
 
-1. Disparity Extender
-2. Safety Bubble
+## Unit tests (local)
 
--   bubble of a certain diameter e.g. width of car where all the points are set to the same value as the middle range.
--   Calculates the number of ranges within the safety bubble by assuming the arc=angle_increment\*range is a stright line colinear with the diameter.
+From repo root:
 
-3. lookahead distance
+```bash
+PYTHONPATH=f1tenth_ws/src/gap_finder pytest -q f1tenth_ws/src/gap_finder/tests
+```
 
--   Ranges are clipped at this value.
--   Increases the priority of nearer end points
--   prevents jitter in steering by smoothing the max distances
+## Docker test run
 
-4. field of view
+From repo root:
 
--   limits the search space for the goal point
--   prevents the car from selecting goal points behind the car
-
-5. Nearest point safety bubble
-
--   draws a safety bubble at the neearest point of the car
--   used for obstacle avoidance
-
-6. Mean filter
-
--   Window rolling mean filter with the windows size the same as a safety bubble drawn at that point
--   It smooths the readings so that the edges of the safety bubble is slightly longer if the edges next to it are stretched
-
-7. Priortise the center of scan
-
--   multiplies all the ranges with a linspace of 0.999 to 1.000 so that within a scan of equal ranges, the car moves in a stright line.
-
-8. Selection of the max range
-
--   the max range after filtering is selected as the goal point
-
-9. Speed controller
-
--   the speed is proportional to the ratio of the mean ranges to the lookahead distance, this is to slow the car in tight spaces (i.e. high uncertainty) this can be changed to the range or mean range in front of the car. 
-- the speed is a function of the steering angle using the simple bicycle model. The equation is  
-$\sqrt{\frac{g * u * b}{tan{|d|}}}$  
-where:  
-g is gravitational acceleration,  
-u is the coefficient of friction,  
-b is the wheelbase and  
-d is the steering angle. 
-
-### Resources
-
--   [Unifying F1TENTH Autonomous Racing: Survey, Methods and Benchmarks](https://arxiv.org/pdf/2402.18558)
--   [Disparity Extender](https://www.nathanotterness.com/2019/04/the-disparity-extender-algorithm-and.html)
--   [A novel obstacle avoidance algo- rithm:“follow the gap method”](https://www.sciencedirect.com/science/article/abs/pii/S0921889012000838)
+```bash
+docker build -t gap-finder-tests -f f1tenth_ws/src/gap_finder/Dockerfile f1tenth_ws/src/gap_finder
+docker run --rm gap-finder-tests
+```

f1tenth_ws/src/gap_finder/gap_finder/__init__.py

@@ -0,0 +1,3 @@
+from .planner import CorridorPlanner, PlannerResult, PlannerState
+
+__all__ = ["CorridorPlanner", "PlannerResult", "PlannerState"]

f1tenth_ws/src/gap_finder/gap_finder/planner.py

@@ -0,0 +1,155 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from math import atan, exp, isfinite
+from typing import Dict, List, Tuple
+
+
+@dataclass
+class PlannerState:
+    last_goal_angle: float = 0.0
+    last_steering: float = 0.0
+    race_phase: float = 0.0
+
+
+@dataclass
+class PlannerResult:
+    steering: float
+    speed: float
+    goal_angle: float
+    confidence: float
+    corridor: Tuple[float, float]
+
+
+class CorridorPlanner:
+    def __init__(self, params: Dict[str, float]):
+        self.params = params
+        self.state = PlannerState()
+
+    def sanitize_ranges(self, ranges, range_max) -> List[float]:
+        lookahead = float(self.params["lookahead_distance"] or range_max)
+        out = []
+        for r in ranges:
+            v = float(r)
+            if (not isfinite(v)) or v <= 0.0:
+                v = lookahead
+            v = max(0.0, min(v, lookahead))
+            out.append(v)
+        return out
+
+    def crop_fov(self, ranges: List[float], angle_min: float, angle_increment: float):
+        half = self.params["view_angle"] / 2.0
+        kept_ranges, kept_angles = [], []
+        for i, r in enumerate(ranges):
+            a = angle_min + i * angle_increment
+            if -half <= a <= half:
+                kept_ranges.append(r)
+                kept_angles.append(a)
+        return kept_ranges, kept_angles
+
+    def _percentile(self, values: List[float], q: float) -> float:
+        if not values:
+            return 0.0
+        s = sorted(values)
+        idx = int((q / 100.0) * (len(s) - 1))
+        return float(s[idx])
+
+    def _boundary_features(self, ranges, angles):
+        left = [(r, a) for r, a in zip(ranges, angles) if a > 0.03]
+        right = [(r, a) for r, a in zip(ranges, angles) if a < -0.03]
+
+        def side_stats(points):
+            if len(points) < 3:
+                return None
+            rs = [p[0] for p in points]
+            near = self._percentile(rs, 20)
+            grads = [abs(rs[i] - rs[i - 1]) for i in range(1, len(rs))]
+            continuity = sum(1 for g in grads if g < self.params["trap_range_jump_threshold"]) / max(len(grads), 1)
+            valid_ratio = sum(1 for r in rs if r < self.params["lookahead_distance"] * 0.98) / len(rs)
+            conf = max(0.0, min(1.0, 0.7 * valid_ratio + 0.3 * continuity))
+            imin = min(range(len(rs)), key=lambda i: rs[i])
+            wall_angle = points[imin][1]
+            return {
+                "near": near,
+                "conf": conf,
+                "wall_angle": wall_angle,
+                "max": max(rs),
+                "median": self._percentile(rs, 50),
+            }
+
+        return side_stats(left), side_stats(right)
+
+    def _corridor_bounds(self, left, right):
+        default = (-self.params["view_angle"] / 2.0, self.params["view_angle"] / 2.0)
+        if left is None or right is None:
+            return default
+        if left["conf"] < self.params["boundary_confidence_threshold"] or right["conf"] < self.params["boundary_confidence_threshold"]:
+            return default
+        lo, hi = right["wall_angle"] + 0.05, left["wall_angle"] - 0.05
+        return default if lo >= hi else (lo, hi)
+
+    def _race_bias_angle(self, left, right):
+        if left is None or right is None:
+            self.state.race_phase *= 0.8
+            return 0.0
+        denom = max(left["near"] + right["near"], 1e-3)
+        curvature = max(-1.0, min(1.0, (left["near"] - right["near"]) / denom))
+        self.state.race_phase = 0.85 * self.state.race_phase + 0.15 * curvature
+        return self.params["race_line_bias_strength"] * 0.30 * self.state.race_phase
+
+    def _trap_side(self, left, right):
+        if left and right:
+            if left["max"] - left["median"] > self.params["trap_range_jump_threshold"] and right["conf"] > self.params["boundary_confidence_threshold"] and left["conf"] < right["conf"]:
+                return "left"
+            if right["max"] - right["median"] > self.params["trap_range_jump_threshold"] and left["conf"] > self.params["boundary_confidence_threshold"] and right["conf"] < left["conf"]:
+                return "right"
+        return None
+
+    def plan(self, ranges, angle_min, angle_increment, range_max):
+        scan = self.sanitize_ranges(ranges, range_max)
+        scan, angles = self.crop_fov(scan, angle_min, angle_increment)
+        left, right = self._boundary_features(scan, angles)
+        corridor = self._corridor_bounds(left, right)
+        trap_side = self._trap_side(left, right)
+        bias_angle = self._race_bias_angle(left, right)
+
+        best_score, goal_angle = -1e18, 0.0
+        for r, a in zip(scan, angles):
+            if a < corridor[0] or a > corridor[1]:
+                continue
+            clearance = r / max(self.params["lookahead_distance"], 1e-3)
+            smooth = abs(a - self.state.last_goal_angle) / max(self.params["view_angle"], 1e-3)
+            race_term = exp(-((a - bias_angle) ** 2) / (2 * 0.12 ** 2))
+            score = 1.0 * clearance + 0.45 * race_term - 0.35 * smooth
+            if trap_side == "left" and a > 0:
+                score -= 5.0
+            if trap_side == "right" and a < 0:
+                score -= 5.0
+            if score > best_score:
+                best_score, goal_angle = score, a
+
+        left_conf = left["conf"] if left else 0.0
+        right_conf = right["conf"] if right else 0.0
+        confidence = min(1.0, min(left_conf, right_conf) + 0.2)
+        if left_conf < self.params["boundary_confidence_threshold"] <= right_conf:
+            goal_angle = min(goal_angle, 0.05)
+        elif right_conf < self.params["boundary_confidence_threshold"] <= left_conf:
+            goal_angle = max(goal_angle, -0.05)
+
+        steering_raw = atan(goal_angle * self.params["wheel_base"])
+        alpha = self.params["steering_smoothing_alpha"]
+        steering = alpha * steering_raw + (1.0 - alpha) * self.state.last_steering
+
+        front_idx = min(range(len(angles)), key=lambda i: abs(angles[i]))
+        front_clearance = scan[front_idx]
+        speed = self.params["speed_min"] + (front_clearance / self.params["lookahead_distance"]) * (self.params["speed_max"] - self.params["speed_min"])
+        if confidence < self.params["boundary_confidence_threshold"]:
+            speed *= self.params["speed_reduction_on_low_confidence"]
+
+        self.state.last_goal_angle = goal_angle
+        self.state.last_steering = steering
+
+        steering = max(-self.params["steering_max"], min(self.params["steering_max"], steering))
+        speed = max(self.params["speed_min"], min(self.params["speed_max"], speed))
+
+        return PlannerResult(float(steering), float(speed), float(goal_angle), float(confidence), corridor)