Safety net and overtaking

pure_pursuit_node runs a set of independent checks every tick, after it has computed its steering and speed. Each one can override both. Overtaking sits among them: it only nudges the steering target, and the checks below it still win. All of this applies whichever steering law is selected, so Stanley, LQR and MPC get the same protection as pure pursuit.

The code is in pure_pursuit_node.pyGitHub (the checks and the opponent tracker) and racing_math.pyGitHub (the geometry). Code excerpts below are from racing_math.py, with docstrings and input checks removed.

Topics

The safety net and overtaking run inside pure_pursuit_node, so their topics are that node’s: see Pure pursuit. Four of its inputs exist mainly for this page:

After a change of localizer, opponent detection waits 6.0 s (opponent_detection_settle_sec) before trusting the map comparison, because a pose that is still converging makes walls look like obstacles.

Safety layers

The checks are listed from the one that must never be overridden down to the least critical. A stop is published at once, never through the speed or steering rate limits.

Body contact checks every direction, so it can see a wall the car is driving alongside, which a forward cone cannot. The emergency stop and the body-contact check are measured from the car’s padded rectangle, like gap_follow’s. The crawl-out states (emergency_escape, body_contact_escape, off_racing_line_recovery) move toward a visible way out instead of holding a stop the car could never leave.

Until the map is available, reactive avoidance uses a raw distance of 0.6 m (avoidance_fallback_trigger_distance) instead of the map comparison.

Detect another car

The detector compares each LiDAR beam with the distance the saved map predicts for it. A return at least 0.4 m (map_subtraction_margin) shorter than the prediction is something the map does not contain. This removes walls before any further test, including an opponent parked against a wall, which simple clustering handles worst.

The prediction comes from ray casting: for each beam, the node starts at the LiDAR’s pose on the map and steps along the beam’s direction through the occupancy grid until it reaches an occupied cell. The distance travelled is the range the map expects. A wall gives a measured range equal to the expected one, to within noise. A car in front of the wall gives a measured range shorter by at least the gap between them. Every fourth beam is ray-cast (map_beam_step) to limit the processing cost.

def dynamic_beam_mask(measured, expected, margin, range_min=0.0):
    measured = np.asarray(measured, dtype=np.float64)
    expected = np.asarray(expected, dtype=np.float64)
    valid = np.isfinite(measured) & (measured >= range_min)
    return valid & (measured < (expected - margin))

An invalid beam is never flagged: it is unknown, not evidence of an object.

The remaining returns are grouped into clusters of neighbouring beams, split wherever the range jumps by more than 0.3 m. Each cluster’s size is its chord, the straight-line distance between its first and last points:

Each end point comes from converting the beam’s polar reading to Cartesian coordinates: $x=r\cos \theta$, $y=r\sin \theta$. The chord is a better measure of size than the cluster’s angle, which makes anything near look large and anything far look small.

def cluster_geometry(ranges, angle_min, angle_increment, start_idx, end_idx):
    angles = angle_min + np.arange(start_idx, end_idx + 1) * angle_increment
    segment = ranges[start_idx:end_idx + 1]
    xs = segment * np.cos(angles)
    ys = segment * np.sin(angles)
    physical_width = float(np.hypot(xs[-1] - xs[0], ys[-1] - ys[0]))
    centroid_idx = (start_idx + end_idx) // 2
    centroid_range = float(ranges[centroid_idx])
    centroid_angle = float(angle_min + centroid_idx * angle_increment)
    return physical_width, centroid_range, centroid_angle

With the map, a cluster counts as a car if its chord is between 0.15 m and 0.7 m (opponent_min_width, opponent_max_width) and it is within 5.0 m (opponent_engagement_range). The size test here only rejects noise and merged objects, because the map comparison has already removed the walls. The closest qualifying cluster wins.

Without a map, or with opponent_detection_mode: heuristic, the clustering runs on the raw scan, and there is no map to tell a car from a section of wall. The heuristic adds one more test: the five beams on each side of the cluster must read at least 0.5 m (opponent_open_side_margin) further away than the cluster does, so the object stands out in open track rather than lying along a wall.

Track the car along the track

The tracker measures the opponent’s position as distance along the racing line, the same quantity used for the car’s own progress. Each line point has a cumulative arc length $s$ from the start. The opponent’s progress is the arc length of its nearest line point, and its speed along the track is smoothed between ticks:

with $\beta$ = opponent_velocity_smoothing (0.3). This is an exponential moving average: each new measurement moves the estimate 30% of the way toward it. A single noisy detection therefore moves the speed estimate only a little, and a real change shows through within a few ticks. It is a simple prediction in the Frenet frame, the frame measured along a reference path: it follows the track’s bends because it is expressed in track distance. If the opponent is not detected again within 1.0 s (opponent_lost_timeout_sec), the tracker forgets it.

On a closed loop, distance along the track wraps at the start line, so “how far ahead” needs care. With lap length $S$, the distance ahead is $(s_{extopp}-s_{extego})\mathrm{mod}S$, always between 0 and $S$. That cannot tell “3 m behind” from ”$S-3$ m ahead”, so a second function picks the shorter way round, for deciding when a pass is finished:

def track_progress_gap(ego_arc_length, other_arc_length, total_length):
    if total_length <= 0.0:
        return 0.0
    return float((other_arc_length - ego_arc_length) % total_length)

def track_lead_distance(ego_arc_length, other_arc_length, total_length):
    if total_length <= 0.0:
        return 0.0
    half = total_length / 2.0
    behind = (other_arc_length - ego_arc_length + half) % total_length - half
    return float(-behind)

Using the first for the end of a pass once caused a collision in the simulator: the test read “at least 1 m past” but meant “at most 1 m past”, and was true the moment the car’s nose edged in front while the two cars were still side by side.

Overtake

A pass starts when both are true:

• The opponent is within overtake_trigger_gap (3.0 m) ahead, measured along the track.
• The car’s profiled speed exceeds the opponent’s tracked speed by at least overtake_closing_margin (0.3 m/s).

The side is chosen once, from the same scan: whichever side of the opponent’s cluster is more open, provided it has at least overtake_min_side_clearance (0.70 m). The pass does not change the racing line. It moves the steering target 0.35 m sideways (overtake_lateral_offset), and the same pure pursuit geometry steers to it.

The shifted point is taken 4.0 m ahead (overtake_lookahead_distance), not at the normal lookahead. The pure pursuit curvature $\kappa =2y/L_d^2$ shows why. A target 1.5 m ahead and 0.35 m to the side asks for $\kappa =2\times 0.35/1.5^2=0.31{\text{m}}^{-1}$, and the off-line curvature limit $\sqrt{a_{\text{lat}}/\kappa }=\sqrt{2.5/0.31}$ caps the car at 2.8 m/s, in the middle of the pass. The same offset 4.0 m ahead asks for $\kappa =0.044{\text{m}}^{-1}$, a cap of 7.5 m/s, above the 4.0 m/s top speed. The node refuses to start if overtake_lookahead_distance is less than max_lookahead.

The pass ends once the car is 1.0 m (overtake_clear_margin) ahead of the opponent’s last known position, measured along the track with track_lead_distance. That test does not need a fresh detection, because an opponent alongside often leaves the forward cone. A pass that has seen no detection for 3.0 s (overtake_max_blind_sec) is abandoned. The emergency stop still applies throughout.

Read the decision log

The decision line names the state that set the command. Normal racing shows the steering law’s name, with any overtake reported in the line’s detail. The states from this page are:

A run that stops unexpectedly is easiest to explain from a recording: the run analyzer names the state behind every stop.

Limitations

• One opponent at a time, with no identity tracking through occlusions.
• Map subtraction removes mapped walls only while localization is accurate. An unmapped object of car size still counts as a car.
• No defensive driving: a car closing from behind changes nothing.
• Overtaking has been tested only in the simulator. Test it on the floor before relying on it.

The full design, including two overtaking defects found and fixed in simulation, is in docs/racing-autonomy.mdGitHub.