Follow the gap

gap_follow is the workspace’s reactive controller. It runs once per LiDAR scan, about 40 times a second, and keeps no map and no memory of the track, so every decision comes from the scan in front of it. Each scan produces one steering angle and one speed on /drive. It drives the mapping laps in map_course, and it is the template for writing a new driving node.

The method is the follow-the-gap algorithm from Lecture 5 and Lab 4 of the course: a safety bubble, a disparity extender and a target inside a gap. The car’s version adds what the simulator exercise leaves out. It models the body as a rectangle instead of a point, limits speed by braking distance and cornering grip, and brakes on time to collision, the check built in Lab 2.

The code is in two files. gap_logic.pyGitHub holds the mathematics and imports no ROS, so its tests run on any computer with Python and NumPy. gap_follow_node.pyGitHub is the ROS node that feeds it scans and publishes its commands. The code excerpts on this page are copied from gap_logic.py with docstrings and input checks removed.

Topics

Subscribes to

Publishes

fused_speed:=true, on map_course or on gap_follow_launch.py, points odom_topic at /odom/fused, the IMU-fused speed from imu_bridge, instead of /odom. The IMU features add subscriptions to /imu/data, /imu/status, /steering/measured and /odom/fused only when they are turned on. All subscriptions and publishers use a queue depth of 10 with the default reliable QoS.

/drive is published from the scan callback, not from a timer. If scans stop arriving, /drive stops too. ackermann_mux publishes no zero when its input goes quiet, so the motor stops only when the VESC’s own command timeout expires. The node reports this as scan_stale after 0.5 s (scan_timeout_sec).

How it works

The LiDAR gives the car a fan of distances, one per beam, from its right to its left. The node looks for a run of beams that are all far away and wide enough for the body, points the wheels at it, and drives as fast as the turn’s curvature and the free distance ahead allow.

Each scan goes through these steps in order:

1. Check the driver. If LB is not held, or /joy is older than 0.5 s, the node publishes speed 0 and does nothing else with the scan.
2. Clean the scan. It keeps the forward 180° and marks unreadable beams.
3. Check the body clearance. If the body touches something, or the time to collision is under 0.30 s, the car stops or creeps.
4. Extend obstacle edges by half the car’s width, so the scan describes where the car’s centre can go.
5. Draw a safety bubble around the closest obstacle.
6. Pick a gap, then pick a beam inside it to aim at.
7. Compute the steering from the aim angle, then limit how fast it can change.
8. Compute the speed as the lowest of several caps, then limit how fast it can rise.

A worked example

The four figures below follow one scan through steps 4 to 6. They come from running the workspace’s own gap_logic.py functions on car 1’s parameters, so every number in them is what the code computes. To keep the drawing readable they use one beam every 0.5°, half as many as the real LiDAR. The script that draws them is in the website repository at scripts/docs-figures/gap_follow_scan.py.

The scene

The car is in a corridor 2 m wide. A box 0.4 m deep and 0.5 m wide sits 1.6 m ahead, slightly to the left. The LiDAR sends out a beam every 0.25°, and each beam returns the distance to the first thing it hits.

The scan as the node sees it

The node never sees the picture above. It receives a list of distances, one per beam, ordered from the car’s right to its left. Drawing that list as a chart gives the shape every later step works on.

Edge extension and the safety bubble

The dip left by the box has two sharp edges. A beam can pass right beside an edge, but the car is 0.31 m wide, so the node lowers the beams next to each edge to the box’s distance (step 4). It then sets to zero the beams around the closest point in the scan, here the right wall straight beside the car (step 5).

The gap and the aim

A beam counts as free when its processed distance is more than 2.0 m (step 6). Consecutive free beams form a gap. In this scan only one run of beams is free: the space right of the box. Both edges of that gap are real obstacles (the box on one side, the right wall on the other), so the node aims at the gap’s middle beam.

Step by step

1. Check the driver

Before it looks at the scan, the node asks whether a person is holding LB. It keeps the time of the last /joy message and the state of button 4, and it refuses to drive in four cases, each with its own state name in the log:

In each case the node keeps publishing speed 0 at the scan rate rather than going silent, because a silent /drive would leave the motor to the VESC’s timeout. A stale /joy counts as released because joy_node crashing while LB is held would otherwise freeze the last “held” reading. Every driving node in the workspace has its own copy of this check, as described in the safety model.

2. Clean the scan

The Hokuyo reports three kinds of reading, and the node treats each differently:

• A normal distance is kept, clipped to max_range (10 m).
• NaN, zero, or a reading below the sensor’s range_min means the beam got no usable return. The stop checks ignore it, because counting it as an obstacle at 0 m would brake on every dropout. Gap selection treats it as blocked (range 0), because steering into a blind spot that only looks open is worse than stopping.
• $+\infty$ means nothing was in reach. That is a real measurement of open space, so it becomes 10 m.

def sanitize_ranges(ranges, max_range: float, range_min: float = 0.0):
    raw = np.asarray(ranges, dtype=np.float64)
    valid = np.isfinite(raw) & (raw >= range_min) & (raw > 0.0)
    clean = np.where(valid, raw, 0.0)
    clean[np.isposinf(raw)] = max_range
    return np.clip(clean, 0.0, max_range), valid

The function returns two arrays: clean, the ranges for gap selection, and valid, a true or false flag per beam for the stop checks. The node then keeps only the beams within ±90° of straight ahead (forward_fov_deg 180). Beams further back describe where the car has been, not where it can go.

3. Stop and creep checks

Three checks run before any gap is chosen. All three measure from the car’s body, a rectangle 0.31 m wide and 0.58 m long, not from the LiDAR. The LiDAR sits 0.26 m ahead of the rear axle, so a wall 0.20 m from the LiDAR can be much closer to a corner of the bumper.

For each beam, the node first computes $b_i$: the distance from the LiDAR to the edge of the body along that beam. A beam’s clearance is then $r_i-b_i$, the free space between the body and whatever the beam hit.

Time to collision is how long the car would take to reach an obstacle if nothing changed. A car moving straight ahead at speed $v$ approaches a point in the direction ${\theta }_i$ at only part of that speed: the component of its velocity along the beam, $v\cos {\theta }_i$. Straight ahead ($\theta =0$) that is all of $v$. At 60° to the side it is half. At 90° it is zero, because the car is moving past the point, not toward it. Dividing the clearance by that closing speed gives the time:

A beam whose closing speed is 0.05 m/s or less gets ${\tau }_i=\infty$: the car is not approaching it. For example, a wall 1.0 m clear straight ahead at 3.0 m/s gives $\tau =0.33$ s, 0.03 s above the 0.30 s threshold. At 3.5 m/s it gives 0.29 s and the car brakes.

closing_speed = speed * np.cos(beam_angles)
approaching = (
    validity
    & np.isfinite(boundaries)
    & (closing_speed > min_closing_speed)
)
# ... drop beams outside the corridor the car will sweep ...
ttc = np.full(ranges.shape, np.inf, dtype=np.float64)
clearances = np.maximum(0.0, ranges - boundaries)
ttc[approaching] = clearances[approaching] / closing_speed[approaching]

The formula is only right for things in the car’s path. A wall 0.5 m to the side, seen at 80°, still has a small positive closing speed, so on a narrow track the plain formula braked for walls the car was going to pass. The node therefore counts only beams that land inside the corridor the body will sweep: the body plus 0.05 m (ttc_path_margin) on each side. On a straight that is a band of fixed width. While turning it is a ring around the turning centre.

The speed $v$ is the magnitude of the /odom speed. When /odom reads almost zero (0.10 m/s or less) but the node commanded a speed in the last 0.5 s, it uses the commanded speed instead. Both choices make the brake err toward stopping. An odometry source with the wrong sign once read −1.8 m/s while the car drove forward at 1.8 m/s, so the brake saw no approach and released on every scan.

4. Extend obstacle edges

A LiDAR beam is a line with no width, but the car is 0.31 m wide. A beam can skim past the edge of a box that would hit the car’s side. The disparity extender fixes this by making every obstacle edge look wider by half a car width plus a margin.

An edge shows up as a jump between neighbouring beams: one beam hits the box at 1.6 m, the next misses it and reaches the wall at 4 m. Wherever two neighbours differ by more than the threshold $d$ (disparity_threshold, 0.4 m), the nearer one marks an edge.

How many beams to lower comes from a right-angled triangle. The edge is at distance $r_{\text{near}}$ along the beam. The car’s centre has to stay $h$ to the side of it, where

with $w$ the car width and $m$ the safety margin. The side of length $r_{\text{near}}$ lies along the beam and the side of length $h$ is at right angles to it, so the angle the protected strip covers, seen from the LiDAR, is

where $\Delta \theta$ is the angle between beams (0.25°) and $n$ is the number of beams to lower, rounded up. At 1.0 m, $\phi =18.5°$ and $n=75$ beams. At 3.0 m, $\phi =6.4°$ and $n=26$. A near edge needs a wide angle and a far one a narrow angle, because the same 0.335 m looks smaller from further away.

The beams on the far side of the edge are lowered to $r_{\text{near}}$. Values only go down, never up, so this step can never invent free space. Afterwards each range describes where the car’s centre can go, and every later step can treat the car as a point.

def disparity_extend(clean, angle_increment, disparity_threshold, extend_width_m):
    extended = clean.copy()
    n = len(clean)
    jumps = np.abs(np.diff(clean))
    for i in np.nonzero(jumps > disparity_threshold)[0]:
        near = min(clean[i], clean[i + 1])
        if near <= 0.0:
            continue
        num_beams = int(math.ceil(math.atan2(extend_width_m, near) / angle_increment))
        if clean[i] < clean[i + 1]:
            lo, hi = i + 1, min(n, i + 1 + num_beams)
        else:
            lo, hi = max(0, i + 1 - num_beams), i + 1
        extended[lo:hi] = np.minimum(extended[lo:hi], near)
    return extended

np.diff gives the jump between each pair of neighbours. The if picks the side to lower: if beam i is the near one, the far side is to the right of it in the array (i + 1 onward), otherwise to the left. extend_width_m is $h$.

The exact angle a circle of radius $h$ at distance $r$ covers is $\arcsin (h/r)$, slightly more than $atan2(h,r)$: by 1.05° at 1 m and 0.13° at 2 m. The code uses $atan2$ because it stays defined when an obstacle is closer than $h$, where $\arcsin$ has no answer. The 0.18 m margin covers the difference.

5. Safety bubble

The node finds the closest valid beam, at distance $r_c$, and sets every beam within $atan2(h,r_c)$ of it to 0, the same angle the edge extender uses. At 1.0 m the bubble is ±18.5° wide. At 3.0 m it is ±6.4°. A bubble sized by distance is wide when the obstacle is near and narrow when it is far, where a fixed angle would give too little clearance at 0.3 m and waste most of the scan at 5 m.

def safety_bubble(window, closest_idx, closest_dist, angle_increment, bubble_width_m):
    out = window.copy()
    if closest_dist <= 0.0:
        radius_idx = out.size  # contact distance: everything is too close
    else:
        radius_idx = int(math.ceil(math.atan2(bubble_width_m, closest_dist) / angle_increment))
    radius_idx = max(1, radius_idx)
    lo = max(0, closest_idx - radius_idx)
    hi = min(out.size, closest_idx + radius_idx + 1)
    out[lo:hi] = 0.0
    return out

6. Choose a gap

A beam is free when its processed range is more than the gap depth $t$ (min_gap_distance, 2.0 m), and a gap is a run of consecutive free beams. Two tests then decide which gap the car takes.

Is it wide enough? A gap of $N$ beams spans the angle $N\Delta \theta$. Its narrowest point is at its nearest beam, at distance $r_{\min }$. The two rays from the LiDAR to the gap’s edges at that distance form an isosceles triangle with two sides of length $r_{\min }$ and the angle $N\Delta \theta$ between them. Splitting it down the middle gives two right-angled triangles, each with half the angle at the LiDAR and half the opening opposite it, so the opening is

A gap narrower than $w_{\min }$ (min_centerline_gap_width, 0.10 m) is dropped. This is measured after edge extension, so 0.10 m is extra room for the car’s centre beyond the half-width already added on each side. Using the nearest beam rather than the average stops a wedge-shaped opening from passing because its far end is wide.

Which is best? Each remaining gap is scored by its width in beams times its average depth $\bar{r}$:

Width alone would prefer a wide doorway alcove 1 m deep over a narrower corridor that continues for 8 m. Multiplying by depth makes a gap win only if it stays open. To stop the aim jumping between two similar gaps on scan noise, the node keeps the previous scan’s gap unless another one scores at least gap_switch_margin (1.2) times higher.

def score(run):
    start, end = run
    segment = window[start:end + 1]
    width = end - start + 1
    avg_depth = float(np.mean(segment))
    return width * avg_depth

best = max(candidates, key=score)
if previous_target_idx is not None and switch_margin > 1.0:
    sticky = next((run for run in candidates
                   if run[0] <= previous_target_idx <= run[1]), None)
    if (sticky is not None and sticky != best
            and score(best) < score(sticky) * switch_margin):
        best = sticky

If no gap reaches 2.0 m, which is common immediately before a corner that hides the track beyond it, the node searches again at the fallback depth of 0.8 m (fallback_min_gap_distance). It drives a fallback gap at up to corner_speed and reports the state corner_fallback. If even the fallback finds nothing, the state is no_safe_gap and the car stops.

7. Choose the aim beam

When both edges of the gap are real obstacles, the node aims at the middle beam, which centres the car between them. When one edge is only the ±90° limit of what the node looks at, the middle beam carries no information about the track. A gap from −90° to +3° has its middle at −44°, pointing at the wall beside the car. In that case the node aims at the deepest beam instead, which is where the course continues. Among the beams within 5% of the deepest, it takes the one closest to straight ahead, so in open space, where every beam reads 10 m, the car goes straight instead of hard over.

def aim_within_gap(window, gap_start, gap_end, beam_angles):
    if gap_start != 0 and gap_end != len(window) - 1:
        return (gap_start + gap_end) // 2
    depths = window[gap_start:gap_end + 1]
    deepest = float(np.max(depths))
    near_deepest = np.nonzero(depths >= deepest - max(1e-6, 0.05 * deepest))[0]
    angles = beam_angles[gap_start:gap_end + 1][near_deepest]
    return gap_start + int(near_deepest[int(np.argmin(np.abs(angles)))])

8. Steering

The aim beam’s angle $\alpha$ (positive to the left) becomes a steering angle:

$K$ is steering_gain (1.0), so at low speed the wheels point straight at the aim. $b$ is a small bias, bounded to 0.08 rad from centring and 0.10 rad from corner set-up: it centres the car in a corridor on straights and moves it to the outside before a corner. ${\delta }_{\max }$ is the steering limit, 0.33 rad on car 1 and 0.349 rad on car 2.

Why the gain falls with speed. The bicycle model from Lecture 6 (notes) says a car steering at $\delta$ turns at the yaw rate

for small angles, with $L$ the wheelbase (0.36 m). Steering at $\delta =K\alpha$ therefore turns the car toward the aim at a rate proportional to $vK/L$. That product is how fast the heading loop responds, in radians per second. At 3 m/s with $K=1$ it is $3/0.36=8.3$ rad/s.

The loop also has a delay. The servo takes time to move, and the next scan arrives 25 ms after this one, about 0.175 s together as measured on the car. A delay of $T$ at a response rate $\omega$ shifts the correction by $\omega T$ radians, and once that shift approaches 90° the correction arrives so late it pushes the car the wrong way. At 8.3 rad/s, $8.3\times 0.175=1.46$ rad, which is 84° of the 90° available, and the car weaved about once a second on straights.

Above $v_{\text{ref}}$ (steering_gain_speed_ref, 1.8 m/s), $K(v)$ falls as $1/v$, so $vK(v)/L$ stays at $1.8/0.36=5$ rad/s at any speed. The delay then costs $5\times 0.175=0.875$ rad, or 50°, leaving 40° of margin. Below 1.8 m/s the full gain is kept, because tight corners need it. At 3.0 m/s, $K(v)=0.6$.

def speed_scheduled_gain(speed: float, reference_speed: float) -> float:
    if reference_speed <= 0.0 or speed <= reference_speed:
        return 1.0
    return reference_speed / speed

Two checks then adjust the angle:

• The arc guard sweeps the car’s body along the arc this steering angle would drive. The arc must be clear for the larger of 1.0 m (arc_guard_length) and the stopping distance plus the 0.20 m reserve. A direction says nothing about the path the body sweeps on the way: approaching an inside corner, the aim leans into the turn and the rear of the car, which tracks inside the LiDAR, clips the corner. If the arc is blocked, the guard tries angles within 0.165 rad on car 1 (0.1745 rad on car 2) of the request, nearest first, and takes the first clear one. Near an inside corner that is a slightly straighter arc, so the car turns in later. If none is clear, it keeps the request (arc_guard_blocked_fallback: request). Choosing the “freest” arc instead once unwound the steering in a tight corner and drove the car into the outer wall.
• The rate limit holds the change in steering to max_steering_rate (2.0 rad/s) times the time since the last command, so the servo never gets a step it cannot follow.

def slew_rate_limit(target, previous, dt, increase_rate, decrease_rate=None):
    if decrease_rate is None:
        decrease_rate = increase_rate
    lower = previous - decrease_rate * dt
    upper = previous + increase_rate * dt
    return float(np.clip(target, lower, upper))

9. Speed

The target speed is the lowest of four caps:

The cap that is lowest is named in /drive_intent, so the dashboard shows why the car is going the speed it is.

Cornering cap. A car on a circle of radius $R$ at speed $v$ needs a sideways (centripetal) acceleration $v^2/R$, and the tyres can supply only so much before they slide. With curvature $\kappa =1/R$, holding $v^2\kappa$ under the grip limit $a_{\text{lat}}$ (max_lateral_accel, 2.5 m/s²) gives

The curvature comes from the steering angle through the bicycle model, $\kappa =\tan \delta /L$. At car 1’s 0.33 rad limit, $\kappa =\tan (0.33)/0.36=0.95{\text{m}}^{-1}$ and $v_{\text{curve}}=\sqrt{2.5/0.95}=1.62$ m/s. The floor $v_{\min }$ (min_speed, 1.429 m/s) keeps a tight corner from stalling the car.

def curvature_speed_limit(curvature, max_lateral_accel, max_speed):
    if abs(curvature) < 1e-9:
        return max_speed
    return min(max_speed, math.sqrt(max_lateral_accel / abs(curvature)))

Braking cap. The car must be able to stop inside the free distance $c$ along the path it is about to sweep, keeping a reserve $r$ (0.20 m). Stopping happens in two parts:

1. The delay. A command takes $\tau$ (braking_latency_sec, 0.25 s) to reach the wheels. During it the car keeps going at $v$ and covers $v\tau$.
2. The braking. From $v$ to rest at a steady deceleration $a$ (max_braking_decel, 3.0 m/s²), the constant-acceleration equation $v_{\text{end}}^2=v^2-2ad$ with $v_{\text{end}}=0$ gives a distance $d=v^2/(2a)$.

The two must fit in the room available:

Multiplying by $2a$ and moving everything to one side gives a quadratic in $v$:

The quadratic formula gives two roots, and only the positive one is a speed:

With 2.0 m clear: $a\tau =0.75$, $2a(c-r)=2\times 3.0\times 1.8=10.8$, and $v_{\text{brake}}=-0.75+\sqrt{0.5625+10.8}=2.62$ m/s. Without the delay term the answer would be 3.29 m/s, so the 0.25 s of latency costs 0.67 m/s.

def braking_speed_limit(clearance, reserve_distance, max_braking_decel, max_speed, latency=0.0):
    if clearance == math.inf:
        return max_speed
    usable_distance = max(0.0, clearance - reserve_distance)
    a_t = max_braking_decel * latency
    v = -a_t + math.sqrt(a_t * a_t + 2.0 * max_braking_decel * usable_distance)
    return min(max_speed, v)

Narrow corridor and corners. $v_{\text{narrow}}$ falls linearly from max_speed to 3.571 m/s (narrow_corridor_speed) as the body’s gap to the nearer side wall closes from 0.15 m to 0.05 m. $v_{\text{corner}}$ applies only on a fallback gap.

With the understeer check on, its cap is applied after these four.

Finally, the commanded speed can rise by at most max_acceleration (5.0 m/s²) times the time since the last command, using the same slew_rate_limit. It can fall immediately: braking is never rate limited. With traction control on, a hold replaces this ramp while the wheels spin.

Parameters

Every parameter is declared in gap_follow_node.py and set in gap_follow.yamlGitHub, which both cars share. Each car’s steering limits are loaded on top of it from its profile’s calibration/gap_follow.yaml. The YAML overrides the code’s defaults, so change the YAML; with a --symlink-install build, restarting the launch picks up the change, and otherwise the package needs colcon build --symlink-install --packages-select gap_follow first. Many of them can also be changed while driving, from the dashboard.

The two cars share every value in the table except these, set in each car’s calibration/gap_follow.yaml (car 1Car 1, car 2Car 2):

arc_guard_max_correction is half of max_steering_angle on each car, and the node refuses to start if it is more than half. enable_adaptive_width (on) narrows the safety margin on narrow straights and sets the fallback corner speed between corner_speed (2.143 m/s) and corner_speed_wide (2.714 m/s) from the sensed width. enable_opponent_awareness (on, tested only in the simulator) tracks other cars and follows a slower one. The IMU features’ parameters are listed with each feature under IMU features.

Run it

Start bringup in the first terminal and leave it running.

Terminal 1:

ros2 launch f1tenth_stack bringup_launch.py

Start gap_follow in a second terminal, without teleop_launch.py. Teleop’s commands win at the mux even when LB is released, so the car would not move.

Terminal 2:

ros2 launch gap_follow gap_follow_launch.py

To set the top speed for one run, add max_speed:=. The value must be a positive number no higher than 5.0 m/s, and any of min_speed, corner_speed and corner_speed_wide above it are lowered to it. Anything else is refused and nothing starts.

ros2 launch gap_follow gap_follow_launch.py max_speed:=1.0

Opponent awareness is on by default, so every car on a shared track runs it. To drive without it:

ros2 launch gap_follow gap_follow_launch.py opponent_awareness:=false

The launch also starts the IMU stack for steering damping, so keep the car still for about 3 s after it starts while the gyro bias is measured. imu:=false starts without it. The other IMU flags are under IMU features.

Hold LB to drive. Run every change with the wheels off the ground first, then on the floor at low speed in open space.

Check it

In a third terminal:

ros2 topic hz /scan                      # about 40 Hz
ros2 topic hz /drive                     # the same rate as /scan
ros2 topic echo /drive --field drive     # steering_angle and speed
ros2 param get /gap_follow_node max_speed

With LB released, /drive still publishes at the scan rate, with speed 0.0.

Read the decision log

Terminal 2 prints one decision line per second (decision_log_period_sec), and one immediately whenever the state changes:

DRIVE [gap_follow] selected preferred depth 2.00m gap -29.5deg to -7.5deg; target=4.12m at -18.5deg, …; command: steering=-0.323rad, speed=1.45m/s
STOP [deadman_released] LB deadman button is not held; command: steering=-0.323rad, speed=0.00m/s

Each line has three parts. DRIVE or STOP says whether the car is moving. The word in brackets is the state. The rest explains the decision, and ends with the command that was published.

For a run that needs explaining afterwards, record it with race_diagnostics and read the report: see Record a run.

IMU features

gap_follow can use the IMU in six ways. Steering damping is on by default on both cars. The other five ship off, because they have been run only in the simulator and have not yet been through a floor trial. All six read the IMU stack’s topics from imu_bridge (see IMU fusion), and the launch starts that stack once, whichever and however many are on.

Both cars run the same code, so all six features and the flags below exist on both. The thresholds of the last four were measured on car 1 and are labelled CAR 1'S VALUES in gap_follow.yaml. Keep the understeer check, traction control, impact stop and fused speed off on car 2 until car 2 has recorded its own values and its BNO085’s mount yaw has been measured: a wrong mount yaw swaps forward and sideways acceleration.

Launch flags

imu_all:=true turns on fused speed, the understeer check, traction control and impact stop. An explicit false on any one of them wins, so imu_all:=true imu_traction:=false runs the other three. Adaptive grip is not part of imu_all: replaying car 1’s recorded runs showed it would raise the lateral limit by at most 0.09 m/s², about 1% of corner speed, and the simulator was slower with it on.

imu:=false starts the node without the IMU stack, and turns steering damping off. The launch refuses to start if imu:=false is combined with any IMU feature, including one turned on by imu_all, and names the feature in its error. imu_hardware chooses the IMU source: auto, the default, uses the simulator’s IMU topics when gym_bridge_node is running and the real sensors otherwise. true and false force the real and the simulated IMU.

Every feature that reads the IMU first checks that it can be trusted. The check passes only when /imu/status says ok, the latest /imu/data sample is less than 0.1 s old (imu_timeout_sec) and its yaw rate is a finite number. When the check fails, each feature falls back on the same control tick to exactly what the node commands with that feature off, and names the reason in /drive_intent, for example imu_understeer_off: imu stale. Impact stop is the one exception, described below.

Fused speed

With fused_speed:=true the node reads its speed from /odom/fused, the wheel speed corrected by the BNO085’s forward accelerometer so that wheelspin does not read as speed. Position and heading are unchanged. If no usable accelerometer samples arrive, /odom/fused carries exactly the wheel speed, and the commands are the same as with the flag off. /drive_intent reports fused_speed_imu: ok while the fused speed is in use, or waiting, fault, stale or no imu naming why the node is on raw /odom instead.

With fused speed in use, the node still subscribes to the wheel odometry on /odom (wheel_odom_topic), and every safety check uses whichever of the two speeds is more cautious:

• Time to collision is computed separately from the wheel speed and from the fused speed, and the higher closing speed is used. Each source keeps its own fallback to the last command, so neither can switch off the other’s.
• Stale odometry stops the car if either /odom or /odom/fused is older than 0.5 s (odom_timeout_sec), even with LB held. The odometry_stale reason names the topic that went stale, so a stopped wheel stream is not blamed on the fusion node.

Fused speed can therefore never weaken the time-to-collision brake or the stale-odometry stop.

Understeer check

The check compares the turn the car makes with the turn it was asked for. /steering/measured is the steering angle implied by the gyro’s yaw rate and the car’s speed, and the command is taken from 0.15 s earlier because the car responds late:

The ratio is the fraction of the commanded turn the car makes, so 1.0 is the full turn and 0.6 is 60% of it. After a 0.3 s low-pass filter, a ratio that stays below 0.75 for 0.2 s caps speed at 85% of the measured speed (1.70 m/s at 2.0 m/s). The cap applies at once, before the acceleration ramp. It releases above 0.85, and the gap between the two thresholds keeps it from switching on and off around a single value.

A turn is judged only at 1.0 m/s or more, with at least 0.05 rad of commanded steering that has changed by at most 0.05 rad over the 0.15 s lag. Counter-steering, where the measured turn is in the opposite direction, is not judged. /drive_intent names an active cap as imu understeer cap 1.70m/s (ratio 0.60).

The thresholds come from replaying car 1’s runs 20261026-194514 and 20261026-194913, where normal ratios were mostly 0.84 to 0.97. The 0.15 s lag is the steering lag measured on run 20261026-194514.

Traction control

Traction control compares the wheel speed from /odom with the fused speed from /odom/fused. Their difference is the slip:

When $s$ exceeds 0.246 m/s, the node holds the last commanded speed instead of accelerating. The hold lasts at most 0.5 s and ends early when $s$ falls below 0.123 m/s, and another hold needs at least one tick below that release value. It only ever lowers a command: a lower cap from clearance or cornering still applies immediately, and the command can never be higher than with traction control off on the same tick. Below 0.3 m/s on either speed, or when reversing, no slip is judged. /drive_intent names a hold as imu traction hold (slip 0.30m/s). If the traction-off acceleration ramp is the limit that binds instead, the intent names the traction-off ceiling.

The 0.246 m/s threshold is 1.5 times the 99th percentile of the positive slip measured on runs 20261026-194514 and 20261026-194913, 0.164 m/s, and stays below the estimator’s 0.3 m/s bound so that it can be reached.

Impact stop

Impact stop commands zero speed after a large horizontal acceleration spike, such as a collision. It reads the calibrated accelerometer on /imu/data, in car axes, and uses only the horizontal part, so gravity on $+z$ is excluded:

A single trusted sample strictly above the threshold sets the stop. The stop latches: it stays set after the spike ends. On every scan it is checked straight after the LB deadman, before any other driving decision, and the published speed is zero with the steering left where it was. /drive_intent shows the state imu_impact_stop, with the force and its time.

To resume, release LB and then press it again. Holding LB, or a press without a release received after the impact, does not clear the stop. An IMU failure never clears it either, unlike the fallbacks of the other features, so unplugging the sensor or restarting imu_bridge cannot make a stopped car drive off. Restarting gap_follow starts with the latch clear. An untrusted IMU cannot set the latch.

The threshold is provisional. No recorded run contains a clear impact, so it is 1.5 times the largest spike found in the recordings, 22.48 m/s² on run 20261029-132902. It will be set from a recorded wheels-off bump.

Every numeric parameter of the three features above can be changed while the node runs, through the ROS parameter services. A value out of range is refused, both at startup and on a live change. The feature flags and topic names need a restart.

Try an IMU feature

Run each one with the wheels off the ground first, then on the floor at low speed in open space, with the run recorded and LB held whenever the car moves. In terminal 2, with bringup running in terminal 1:

ros2 launch gap_follow gap_follow_launch.py imu_traction:=true max_speed:=1.0

The terminal prints IMU source: real IMU hardware and the IMU stack starts once. Then check, in a third terminal:

ros2 topic echo /imu/status --once                         # "state": "ok"
ros2 param get /gap_follow_node enable_imu_traction        # True

With fused_speed:=true, ros2 param get /gap_follow_node odom_topic prints /odom/fused. On a stand the understeer check cannot judge, because the car is not turning, so do not expect it to engage. To check impact stop, hold LB with the car secure on its stand and tap the chassis: the wheels stop, the stop stays while LB is held, and releasing and pressing LB resumes. A tap that does not trigger it is a reason to read the recorded horizontal force, not to test with a collision.

Tests

The package’s tests are in src/gap_follow/test/GitHub. The gap_logic tests call the functions on this page directly and check each against a closed form or an invariant: for example, that a car at the braking cap stops within the room it was given, and that edge extension never raises a range. The node tests start a real gap_follow_node, publish scans and /joy messages, and check the published command, including the four LB cases from step 1. How to run them, and how to read the results, is on Run the tests.

Troubleshooting

Limitations

• gap_follow keeps no map, so it cannot plan a racing line or brake early for a corner it cannot see yet. Racing a saved line on a mapped track is the job of pure pursuit.
• On a track about 1.2 m wide, opponent awareness mostly follows the car ahead instead of passing. It has been tested only in the simulator.
• The IMU features have been run only in the simulator, whose tyres never slide, so fused speed equals wheel speed there and real understeer and wheelspin cannot occur. Each feature ships off until a recorded floor trial. In the simulator the understeer check and traction control still engaged on some ticks, from the simulator’s added steering friction and wheel-speed noise, so those runs check the wiring, not the thresholds.
• Impact stop can miss a slow push or a spike below its threshold, and tilting the chassis puts gravity into the horizontal axes. Its threshold is provisional until a wheels-off bump is recorded.
• The geometry in gap_follow.yaml was measured on car 2. Car 1 uses the same values.