From 06e4f93c7af14bb619692b500960d04dc5b7a1b0 Mon Sep 17 00:00:00 2001 From: Wei Xuan Date: Fri, 27 Feb 2026 18:17:13 +0800 Subject: [PATCH] Improve gap_finder with corridor planning, trap avoidance, tests --- f1tenth_ws/src/gap_finder/Dockerfile | 12 + f1tenth_ws/src/gap_finder/README.md | 81 ++- .../src/gap_finder/gap_finder/__init__.py | 3 + .../src/gap_finder/gap_finder/planner.py | 155 ++++++ .../src/gap_finder/scripts/gap_finder_base.py | 486 ++---------------- .../src/gap_finder/tests/test_planner.py | 90 ++++ 6 files changed, 346 insertions(+), 481 deletions(-) create mode 100644 f1tenth_ws/src/gap_finder/Dockerfile create mode 100644 f1tenth_ws/src/gap_finder/gap_finder/planner.py create mode 100644 f1tenth_ws/src/gap_finder/tests/test_planner.py diff --git a/f1tenth_ws/src/gap_finder/Dockerfile b/f1tenth_ws/src/gap_finder/Dockerfile new file mode 100644 index 0000000..bcc68c8 --- /dev/null +++ b/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"] diff --git a/f1tenth_ws/src/gap_finder/README.md b/f1tenth_ws/src/gap_finder/README.md index df372dd..ffc47ac 100644 --- a/f1tenth_ws/src/gap_finder/README.md +++ b/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 +``` diff --git a/f1tenth_ws/src/gap_finder/gap_finder/__init__.py b/f1tenth_ws/src/gap_finder/gap_finder/__init__.py index e69de29..f793b0c 100644 --- a/f1tenth_ws/src/gap_finder/gap_finder/__init__.py +++ b/f1tenth_ws/src/gap_finder/gap_finder/__init__.py @@ -0,0 +1,3 @@ +from .planner import CorridorPlanner, PlannerResult, PlannerState + +__all__ = ["CorridorPlanner", "PlannerResult", "PlannerState"] diff --git a/f1tenth_ws/src/gap_finder/gap_finder/planner.py b/f1tenth_ws/src/gap_finder/gap_finder/planner.py new file mode 100644 index 0000000..b92067b --- /dev/null +++ b/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) diff --git a/f1tenth_ws/src/gap_finder/scripts/gap_finder_base.py b/f1tenth_ws/src/gap_finder/scripts/gap_finder_base.py index 3f5e5c6..f097f26 100755 --- a/f1tenth_ws/src/gap_finder/scripts/gap_finder_base.py +++ b/f1tenth_ws/src/gap_finder/scripts/gap_finder_base.py @@ -1,491 +1,119 @@ #!/usr/bin/env python3 +import numpy as np import rclpy +from ackermann_msgs.msg import AckermannDriveStamped from rclpy.node import Node - from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped -from nav_msgs.msg import Odometry -from visualization_msgs.msg import Marker, MarkerArray - -import numpy as np - -# from gap_finder.pid import PID -from pid import PID -# reference: https://github.com/f1tenth/f1tenth_labs_openrepo/blob/main/f1tenth_lab4/README.md -# reference: https://www.nathanotterness.com/2019/04/the-disparity-extender-algorithm-and.html +from gap_finder.planner import CorridorPlanner config = { - "disparity_bubble_diameter": 0.4, - "safety_bubble_diameter": 0.4, - "front_bubble_diameter": 0.33, - "minimum_bubble_diameter": 0.33, - "view_angle": 3.142, - "coefficient_of_friction": 0.71, - "disparity_threshold": 0.6, - "lookahead_distance": 10, - "speed_kp": 1.0, - "steering_kp": 1.2, - "wheel_base": 0.324, - "speed_min": 1.0, - "speed_max": 10.0, - "steering_max": 0.5, - "speed_low_pass_filter": 0.8, - "steering_low_pass_filter": 0.8, - "bin_number": 7, - # Turn ON or OFF features - "do_limit_lookahead": True, - "do_prioritise_center": True, - "do_mark_disparity": True, - "do_limit_field_of_view": True, - "do_mark_minimum": True, - "do_mark_sides": False, - "do_min_filter": False, - "do_bin_speed": False, - "do_speed_low_pass_filter": False, - "do_steering_low_pass_filter": False, - "do_visualise": False, - # ROS PARAMS - "drive_pub_hz": 50, - "scan_topic": "scan", - "drive_topic": "drive" - } - + "view_angle": 3.142, + "lookahead_distance": 8.0, + "wheel_base": 0.324, + "speed_min": 1.0, + "speed_max": 7.0, + "steering_max": 0.5, + "track_width_estimate": 2.0, + "boundary_min_points": 6, + "boundary_confidence_threshold": 0.55, + "trap_range_jump_threshold": 2.5, + "race_line_bias_strength": 0.8, + "steering_smoothing_alpha": 0.55, + "speed_reduction_on_low_confidence": 0.6, + "drive_pub_hz": 50, + "scan_topic": "scan", + "drive_topic": "drive", +} + class GapFinderAlgorithm: - """ - This class implements the gap finder algorithm. The algorithm takes in a list of ranges from a LiDAR scan and - returns a twist dictionary that will move the car to the deepest gap in the scan after drawing safety bubbles. - params: - - disparity_bubble_diameter: the diameter of the safety bubble - - view_angle: the angle of the field of view of the LiDAR as a cone in front of the car - - coeffiecient_of_friction: the coeffiecient of friction of the car used for speed calculation - - disparity_threshold: the threshold for marking a disparity in the scan i.e. an edge - - lookahead_distance : the maximum distance to look ahead in the scan i.e ranges more than this are set to this value - - speed_kp: the proportional gain for the speed PID controller - - steering_kp: the proportional gain for the steering PID controller - - wheel_base: the distance between the front and rear axles of the car - - speed_max: the maximum speed of the car - - visualise: a boolean to generate visualisation markers - """ - def __init__(self, disparity_bubble_diameter = 0.4, - safety_bubble_diameter = 0.4, - front_bubble_diameter = 0.33, - minimum_bubble_diameter = 0.33, - view_angle = 3.142, - coeffiecient_of_friction = 0.71, - disparity_threshold = 0.6, - lookahead_distance = None, - speed_kp = 1.0, - steering_kp = 1.2, - wheel_base = 0.324, - speed_max = 10.0, - speed_min = 1.0, - speed_low_pass_filter = 0.8, - steering_low_pass_filter = 0.8, - do_visualise = False, - bin_number = 7, - # Turning ON or OFF features - do_limit_lookahead = True, - do_mark_disparity = True, - do_mark_minimum = True, - do_limit_field_of_view = True, - do_prioritise_center = True, - do_mark_sides = False, - do_min_filter = False, - do_bin_speed = False, - do_speed_low_pass_filter = False, - do_steering_low_pass_filter = False): - # Tunable Parameters - self.disparity_bubble_diameter = disparity_bubble_diameter # [m] - self.safety_bubble_diameter = safety_bubble_diameter # [m] - self.front_bubble_diameter = front_bubble_diameter - self.minimum_bubble_diameter = minimum_bubble_diameter - self.view_angle = view_angle # [rad] - self.coeffiecient_of_friction = coeffiecient_of_friction - self.lookahead_distance = lookahead_distance # [m] - self.disparity_threshold = disparity_threshold # [m] - self.wheel_base = wheel_base # [m] - self.speed_max = speed_max # [m/s] - self.speed_min = speed_min - self.front_bubble_diameter = front_bubble_diameter - self.bin_number = bin_number - self.speed_low_pass_filter = speed_low_pass_filter - self.steering_low_pass_filter = steering_low_pass_filter - # Controller Parameters - self.speed_pid = PID(Kp=-speed_kp) - self.speed_pid.set_point = 0.0 - self.steering_pid = PID(Kp=-steering_kp) - self.steering_pid.set_point = 0.0 - # Feature Activation - self.do_mark_sides = do_mark_sides - self.do_min_filter = do_min_filter - self.do_limit_lookahead = do_limit_lookahead - self.do_mark_disparity = do_mark_disparity - self.do_mark_minimum = do_mark_minimum - self.do_bin_speed = do_bin_speed - self.do_speed_low_pass_filter = do_speed_low_pass_filter - self.do_steering_low_pass_filter = do_steering_low_pass_filter - self.do_limit_field_of_view = do_limit_field_of_view - self.do_prioritise_center = do_prioritise_center - # Visualisation - self.do_visualise = do_visualise - self.safety_markers = {"range":[0.0], "bearing":[0.0]} - self.goal_marker = {"range":0.0, "bearing":0.0} - # Internal Variables - self.initialise = True - self.center_priority_mask = None - self.fov_bounds = None - self.middle_index = None - self.last_speed = 0.0 - self.last_steering = 0.0 + def __init__(self, **kwargs): + params = dict(config) + params.update(kwargs) + self.planner = CorridorPlanner(params) + self.params = params + self.goal_marker = {"range": 0.0, "bearing": 0.0} def update(self, scan_msg): - ranges = np.array(scan_msg.ranges) - angle_increment = scan_msg.angle_increment - - ### INITIALISE ### - if self.initialise: - if self.lookahead_distance is None: - self.lookahead_distance = scan_msg.range_max - # middle index for front of car - self.middle_index = ranges.shape[0]//2 - # field of view bounds - view_angle_count = self.view_angle//angle_increment - lower_bound = int((ranges.shape[0] - view_angle_count)/2) - upper_bound = int(lower_bound + view_angle_count) - self.fov_bounds = [lower_bound, upper_bound+1] - # center priority mask - ranges_right = ranges[lower_bound:self.middle_index] - ranges_left = ranges[self.middle_index:upper_bound+1] - mask_right = np.linspace(0.999, 1.0, ranges_right.shape[0]) - mask_left = np.linspace(1.0, 0.999, ranges_left.shape[0]) - self.center_priority_mask = np.concatenate((mask_right, mask_left)) - self.initialise = False - - ### LIMIT LOOKAHEAD ## - # i.e. scan data in clipped to lookahead distance - if (self.do_limit_lookahead): - ranges[ranges > self.lookahead_distance] = self.lookahead_distance - else : - self.lookahead_distance = scan_msg.range_max - modified_ranges = ranges.copy() - - ### MIN FILTER ### - # each scan datapoint is clipped to the nearest neighbour within a safety bubble - if (self.do_min_filter): - for i, r in enumerate(ranges): - arc = angle_increment * r - radius_count = int(self.minimum_bubble_diameter/arc/2) - if i < radius_count: - the_min = np.min(ranges[:i+radius_count+1]) - elif i > ranges.shape[0] - radius_count: - the_min = np.min(ranges[i-radius_count:]) - else: - the_min = np.min(ranges[i-radius_count:i+radius_count+1]) - modified_ranges[i] = the_min - - ### LIMIT FIELD OF VIEW ### - # i.e. limits the car to a view angle infront of the car. - if (self.do_limit_field_of_view): - limited_ranges = modified_ranges[self.fov_bounds[0]:self.fov_bounds[1]] - limited_modified_ranges = modified_ranges[self.fov_bounds[0]:self.fov_bounds[1]] - - ### MARK LARGE DISPARITY### - marked_indexes = [] - if (self.do_mark_disparity): - for i in range(1, limited_ranges.shape[0]): - if abs(limited_ranges[i] - limited_ranges[i-1]) > self.disparity_threshold: - if limited_ranges[i] < limited_ranges[i-1]: - r = limited_ranges[i] - arc = angle_increment * r - else: - i -= i - r = limited_ranges[i-1] - arc = angle_increment * r - radius_count = int(self.disparity_bubble_diameter/arc/2) - lower_bound = i-radius_count - upper_bound = i+radius_count+1 - marked_point = [lower_bound, upper_bound, r] - marked_indexes.append(marked_point) - - ### MARK NEAREST SCAN ### - if (self.do_mark_minimum): - r = np.min(limited_ranges) - i = np.argmin(limited_ranges) - arc = angle_increment * r - radius_count = int(self.safety_bubble_diameter/arc/2) - lower_bound = i-radius_count - upper_bound = i+radius_count+1 - marked_point = [lower_bound, upper_bound, r] - marked_indexes.append(marked_point) - - ### APPLY SAFETY BUBBLE TO MARKED INDEXES ### - for mark_point in marked_indexes: - limited_modified_ranges[mark_point[0]:mark_point[1]] = mark_point[2] - - ### PRIORITISE CENTER OF SCAN ### - if (self.do_prioritise_center): - limited_modified_ranges *= self.center_priority_mask - - ### FIND DEEPEST GAP ### - max_gap_index = np.argmax(limited_modified_ranges) - goal_bearing = angle_increment * (max_gap_index - limited_ranges.shape[0] // 2) - - ### FIND FRONT CLEARANCE ### - front_clearance = ranges[self.middle_index] # single laser scan - if front_clearance != 0.0: # mean of safety bubble of front scan - arc = angle_increment * ranges[self.middle_index] - radius_count = int(self.front_bubble_diameter/arc/2) - front_clearance = np.mean(ranges[self.middle_index-radius_count:self.middle_index+radius_count]) - - ### FIND TWIST ### - init_steering = np.arctan(goal_bearing * self.wheel_base) # using ackermann steering model - steering = self.steering_pid.update(init_steering) - - # init_speed = np.sqrt(10 * self.coeffiecient_of_friction * self.wheel_base / np.abs(max(np.tan(abs(steering)),1e-16))) - # init_speed = front_clearance/self.lookahead_distance * min(init_speed, self.speed_max) - init_speed = front_clearance/self.lookahead_distance * self.speed_max - speed = self.speed_pid.update(init_speed) - - ### BIN SPEED ### - if (self.do_bin_speed): - bins = np.linspace(self.speed_min, self.speed_max, self.bin_number) - for bin_speed in bins: - if (speed < bin_speed): - speed = bin_speed - break - - ### LOW PASS FILTER ### - if (self.do_speed_low_pass_filter): - speed = self.speed_low_pass_filter * speed + (1-self.speed_low_pass_filter) * self.last_speed - if (self.do_steering_low_pass_filter): - speed = self.steering_low_pass_filter * steering + (1-self.steering_low_pass_filter) * self.last_steering - - self.last_speed = speed - self.last_steering = steering - - ackermann = {"speed": speed, "steering": steering} - - ### VISUALISATION ### - if self.do_visualise: - # Visualise Modified Ranges - self.safety_scan_msg = scan_msg - scan_msg.ranges = modified_ranges.tolist() - # Visualise Marked Ranges - self.safety_markers["range"].clear() - self.safety_markers["bearing"].clear() - for i_range in marked_indexes: - bearing = angle_increment * (i_range[0] - ranges.shape[0]//2) - self.safety_markers["range"].append(i_range[1]) - self.safety_markers["bearing"].append(bearing) - # Visualise Goal - self.goal_marker["range"] = modified_ranges[max_gap_index] - self.goal_marker["bearing"] = goal_bearing - - return ackermann - - def get_bubble_coord(self): - m = [] - for i, r in enumerate(self.safety_markers["range"]): - x = r * np.cos(self.safety_markers["bearing"][i]) - y = r * np.sin(self.safety_markers["bearing"][i]) - m.append([x, y]) - return m - - def get_goal_coord(self): - x = self.goal_marker["range"] * np.cos(self.goal_marker["bearing"]) - y = self.goal_marker["range"] * np.sin(self.goal_marker["bearing"]) - return [x, y] - - def get_safety_scan(self): - return self.safety_scan_msg + result = self.planner.plan( + ranges=np.array(scan_msg.ranges, dtype=float), + angle_min=scan_msg.angle_min, + angle_increment=scan_msg.angle_increment, + range_max=scan_msg.range_max, + ) + self.goal_marker["range"] = scan_msg.range_max + self.goal_marker["bearing"] = result.goal_angle + return {"speed": result.speed, "steering": result.steering} class GapFinderNode(Node): - """ - ROS2 Node Class that handles all the subscibers and publishers for the gap finder algorithm. - It abstracts the gap finder algorithm from the ROS2 interface. - The only things to tune or change are in the sections: - - ROS2 PARAMETERS - - SPEED AND STEERING LIMITS - - GAP FINDER ALGORITHM - """ def __init__(self): - ### ROS2 PARAMETERS ### - self.hz = config["drive_pub_hz"] # [Hz] - self.timeout = 1.0 # [s] - self.visualise = config["do_visualise"] - scan_topic = config["scan_topic"] - # drive_topic = "/nav/drive" - drive_topic = config["drive_topic"] - - ### SPEED AND STEERING LIMITS ### - # Speed limits - self.max_speed = config["speed_max"] # [m/s] - self.min_speed = config["speed_min"] # [m/s] - # Acceleration limits - self.max_acceleration = None # [m/s^2] - # Steering limits - self.max_steering = config["steering_max"] # [rad] + self.hz = config["drive_pub_hz"] + self.timeout = 1.0 + self.max_speed = config["speed_max"] + self.min_speed = config["speed_min"] + self.max_steering = config["steering_max"] + self.max_acceleration = None - ### GAP FINDER ALGORITHM ### - self.gapFinderAlgorithm = GapFinderAlgorithm( disparity_bubble_diameter = config["disparity_bubble_diameter"], - safety_bubble_diameter = config["safety_bubble_diameter"], - front_bubble_diameter = config["front_bubble_diameter"], - minimum_bubble_diameter = config["minimum_bubble_diameter"], - view_angle = config["view_angle"], - coeffiecient_of_friction = config["view_angle"], - disparity_threshold = config["disparity_threshold"], - lookahead_distance = config["lookahead_distance"], - speed_kp = config["speed_kp"], - steering_kp = config["steering_kp"], - wheel_base = config["wheel_base"], - speed_max = config["speed_max"], - speed_min = config["speed_min"], - do_visualise = config["do_visualise"], #only works in sim - ) + self.gapFinderAlgorithm = GapFinderAlgorithm() - ### ROS2 NODE ### super().__init__("gap_finder") - # Scan Subscriber - self.scan_subscriber = self.create_subscription(LaserScan, scan_topic, self.scan_callback, 1) - self.scan_subscriber # prevent unused variable warning + self.scan_subscriber = self.create_subscription(LaserScan, config["scan_topic"], self.scan_callback, 1) + self.scan_subscriber self.scan_ready = False self.last_scan_time = self.get_time() - # Drive Publisher + self.drive_msg = AckermannDriveStamped() self.last_drive_msg = AckermannDriveStamped() - self.drive_publisher = self.create_publisher(AckermannDriveStamped, drive_topic, 1) + self.drive_publisher = self.create_publisher(AckermannDriveStamped, config["drive_topic"], 1) self.last_drive_time = self.get_time() - # Viz Publishers - if self.visualise: - self.init_visualisation() - # Timer - self.timer = self.create_timer(1/self.hz , self.timer_callback) - - def init_visualisation(self): - # Safety Viz Publisher - self.bubble_viz_publisher = self.create_publisher(MarkerArray, "/safety_bubble", 1) - self.bubble_viz_msg = Marker() - self.bubble_viz_msg.header.frame_id = "/base_link" - self.bubble_viz_msg.color.a = 1.0 - self.bubble_viz_msg.color.r = 1.0 - self.bubble_viz_msg.scale.x = self.gapFinderAlgorithm.disparity_bubble_diameter - self.bubble_viz_msg.scale.y = self.gapFinderAlgorithm.disparity_bubble_diameter - self.bubble_viz_msg.scale.z = self.gapFinderAlgorithm.disparity_bubble_diameter - self.bubble_viz_msg.type = Marker.SPHERE - self.bubble_viz_msg.action = Marker.ADD - # Goal Viz Publisher - self.gap_viz_publisher = self.create_publisher(Marker, "/goal_point", 1) - self.goal_viz_msg = Marker() - self.goal_viz_msg.header.frame_id = "/base_link" - self.goal_viz_msg.color.a = 1.0 - self.goal_viz_msg.color.g = 1.0 - self.goal_viz_msg.scale.x = 0.3 - self.goal_viz_msg.scale.y = 0.3 - self.goal_viz_msg.scale.z = 0.3 - self.goal_viz_msg.type = Marker.SPHERE - self.goal_viz_msg.action = Marker.ADD - # Laser Viz Publisher - self.laser_publisher = self.create_publisher(LaserScan, "/safety_scan", 1) + + self.timer = self.create_timer(1 / self.hz, self.timer_callback) def get_time(self): - """ - Returns the current time in seconds - """ - return self.get_clock().now().to_msg().sec + self.get_clock().now().to_msg().nanosec * 1e-9 + now = self.get_clock().now().to_msg() + return now.sec + now.nanosec * 1e-9 def scan_callback(self, scan_msg): self.scan_ready = True self.scan_msg = scan_msg self.last_scan_time = self.get_time() - def publish_drive_msg(self, drive={"speed": 0.0, "steering": 0.0}): + def publish_drive_msg(self, drive=None): + drive = drive or {"speed": 0.0, "steering": 0.0} self.drive_msg.drive.speed = float(drive["speed"]) self.drive_msg.drive.steering_angle = float(drive["steering"]) self.drive_publisher.publish(self.drive_msg) self.last_drive_msg = self.drive_msg self.last_drive_time = self.get_time() - def publish_viz_msgs(self): - safety_scan = self.gapFinderAlgorithm.get_safety_scan() - bubble_coord = self.gapFinderAlgorithm.get_bubble_coord() - goal_coord = self.gapFinderAlgorithm.get_goal_coord() - laser_viz_msg = safety_scan - bubble_array_viz_msg = MarkerArray() - - for i, coord in enumerate(bubble_coord): - self.bubble_viz_msg = Marker() - self.bubble_viz_msg.header.frame_id = "ego_racecar/base_link" - self.bubble_viz_msg.color.a = 1.0 - self.bubble_viz_msg.color.r = 1.0 - self.bubble_viz_msg.scale.x = self.gapFinderAlgorithm.disparity_bubble_diameter - self.bubble_viz_msg.scale.y = self.gapFinderAlgorithm.disparity_bubble_diameter - self.bubble_viz_msg.scale.z = self.gapFinderAlgorithm.disparity_bubble_diameter - self.bubble_viz_msg.type = Marker.SPHERE - self.bubble_viz_msg.action = Marker.ADD - self.bubble_viz_msg.id = i - self.bubble_viz_msg.pose.position.x = coord[0] - self.bubble_viz_msg.pose.position.y = coord[1] - bubble_array_viz_msg.markers.append(self.bubble_viz_msg) - - self.goal_viz_msg.pose.position.x = goal_coord[0] - self.goal_viz_msg.pose.position.y = goal_coord[1] - - self.bubble_viz_publisher.publish(bubble_array_viz_msg) - self.gap_viz_publisher.publish(self.goal_viz_msg) - self.laser_publisher.publish(laser_viz_msg) - - def timer_callback(self): if self.scan_ready: - ### UPDATE GAP FINDER ALGORITHM ### drive = self.gapFinderAlgorithm.update(self.scan_msg) + drive["steering"] = np.sign(drive["steering"]) * min(abs(drive["steering"]), self.max_steering) + drive["speed"] = min(max(drive["speed"], self.min_speed), self.max_speed) - ### APPLY SPEED AND STEERING LIMITS ### - # steering limits - drive["steering"] = np.sign(drive["steering"]) * min(np.abs(drive["steering"]), self.max_steering) - # speed limits - drive["speed"] = max(drive["speed"], self.min_speed) - drive["speed"] = min(drive["speed"], self.max_speed) - # acceleration limits if self.max_acceleration is not None: dt = self.get_time() - self.last_drive_time d_speed = drive["speed"] - self.last_drive_msg.drive.speed if abs(d_speed) > self.max_acceleration * dt: - # accelerate if drive["speed"] > self.last_drive_msg.drive.speed: drive["speed"] = self.last_drive_msg.drive.speed + self.max_acceleration - # decelerate else: drive["speed"] = self.last_drive_msg.drive.speed - self.max_acceleration - # drive["speed"] = 0.0 - ### PUBLISH DRIVE MESSAGE ### self.publish_drive_msg(drive) - self.last_time = self.get_time() - - #### PUBLISH VISUALISATION MESSAGES ### - if self.visualise: - self.publish_viz_msgs() - ### TIMEOUT ### - if ((self.get_time() - self.last_scan_time) > self.timeout): - # self.scan_ready = False + if (self.get_time() - self.last_scan_time) > self.timeout: pass + def main(args=None): rclpy.init(args=args) - gapFinder = GapFinderNode() - rclpy.spin(gapFinder) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) gapFinder.destroy_node() rclpy.shutdown() diff --git a/f1tenth_ws/src/gap_finder/tests/test_planner.py b/f1tenth_ws/src/gap_finder/tests/test_planner.py new file mode 100644 index 0000000..878ae8e --- /dev/null +++ b/f1tenth_ws/src/gap_finder/tests/test_planner.py @@ -0,0 +1,90 @@ +import math + +from gap_finder.planner import CorridorPlanner + + +def planner(): + return CorridorPlanner( + { + "view_angle": math.pi, + "lookahead_distance": 8.0, + "wheel_base": 0.324, + "speed_min": 1.0, + "speed_max": 7.0, + "steering_max": 0.5, + "track_width_estimate": 2.0, + "boundary_min_points": 6, + "boundary_confidence_threshold": 0.55, + "trap_range_jump_threshold": 2.5, + "race_line_bias_strength": 0.8, + "steering_smoothing_alpha": 0.55, + "speed_reduction_on_low_confidence": 0.6, + } + ) + + +def base_scan(n=181): + angles = [(-math.pi / 2) + i * (math.pi / (n - 1)) for i in range(n)] + ranges = [4.0 for _ in range(n)] + return ranges, angles + + +def run_plan(p, ranges, angles): + return p.plan(ranges, angle_min=angles[0], angle_increment=angles[1] - angles[0], range_max=10.0) + + +def test_straight_corridor_near_zero_steering(): + p = planner() + ranges, angles = base_scan() + ranges = [r + 0.2 * math.cos(2 * a) for r, a in zip(ranges, angles)] + out = run_plan(p, ranges, angles) + assert abs(out.steering) < 0.08 + + +def test_left_curve_biases_left_and_stable(): + p = planner() + ranges, angles = base_scan() + shaped = [] + for a in angles: + if a > 0: + shaped.append(2.3 + 0.2 * abs(a)) + elif a < 0: + shaped.append(4.8 - 0.4 * abs(a)) + else: + shaped.append(4.0) + + steers = [] + for _ in range(4): + out = run_plan(p, shaped, angles) + steers.append(out.steering) + + assert abs(sum(steers) / len(steers)) > 0.003 + signs = [1 if x >= 0 else -1 for x in steers] + assert len(set(signs)) == 1 + diffs = [abs(steers[i] - steers[i - 1]) for i in range(1, len(steers))] + assert max(diffs) < 0.08 + + +def test_trap_opening_penalized(): + p = planner() + ranges, angles = base_scan() + for i, a in enumerate(angles): + if a < -0.35: + ranges[i] = 8.0 + if 0.2 < a < 1.0: + ranges[i] = 2.4 + out = run_plan(p, ranges, angles) + assert out.goal_angle > -0.15 + + +def test_missing_boundary_reduces_speed_and_steer_conservative(): + p = planner() + ranges, angles = base_scan() + for i, a in enumerate(angles): + if a > 0.1: + ranges[i] = 8.0 + if a < -0.1: + ranges[i] = 2.4 + out = run_plan(p, ranges, angles) + assert out.speed < 4.0 + assert out.steering < 0.12