Pure pursuit

pure_pursuit_node drives a saved racing line on a mapped course. Forty times a second it finds where the car is on the line, picks a point a short distance further along, steers along the circle that reaches that point, and sets the speed from the line’s profile. Pure pursuit is the default of the node’s four steering laws and the method taught in Lecture 10 (notes) of the course. Stanley, LQR and MPC replace only the steering step, and everything else on this page applies to them too.

The node needs two things running beneath it: bringup, and a localizer that publishes the car’s pose on the map. race_launch.py starts the particle filter and the node together. The ROS side is pure_pursuit_node.pyGitHub. The geometry is in racing_math.pyGitHub and the steering laws in tracking_laws.pyGitHub, both free of ROS so their tests run anywhere. Code excerpts on this page are from those two files, with docstrings and input checks removed.

Topics

Subscribes to

The last three exist only with opponent_detection_mode: map, the default. “Latched” means the publisher keeps the last message (transient-local durability) and the node subscribes the same way, so it still receives a map that was published once, long before it started.

Publishes

One tick, in order

The controller runs on a timer, not on incoming poses. If localization stopped, a loop driven by poses would stop running and leave its last command in force. A timer keeps running, keeps checking how old its data is, and stops the car when the pose goes stale.

Each tick goes through these checks and steps. The first one that fails stops the car for that tick and names the reason in the log:

1. LB. No live LB hold means speed 0, before anything else is looked at (deadman_released, joy_stale).
2. A racing line. In a mapping run the node starts without one and waits (waiting_for_profile).
3. A fresh pose. Until the first pose arrives the state is waiting_for_pose. After that the pose must be newer than 0.5 s (pose_timeout_sec) by both its own timestamp and its arrival time (pose_stale). The timestamp catches an old transform being republished at full rate. The arrival time catches the publisher going silent.
4. A pose that moves. If /odom says the car is moving at 0.3 m/s or more but the pose has not moved for 0.5 s, localization has stalled while the car keeps rolling (pose_frozen). A stalled SLAM put the car into a wall on July 27, 2026, which is why this check exists.
5. The nearest point on the line, and the car’s distance from it, the cross-track error.
6. Still on the line. Further than 1.0 m (max_cross_track_error) from the line, the node first searches the whole line once in case localization jumped. Between 1.0 and 2.0 m it crawls back at 0.30 m/s, aiming straight at the nearest point, for up to 5 s. Beyond 2.0 m, or after 5 s, it stops (off_racing_line).
7. Steering, from the chosen law.
8. Speed, from the profile at the nearest point, then the speed caps.
9. Overtaking, which may move the target to pass a slower car. See Safety net and overtaking.
10. The reactive safety net, which has the final say over everything above.
11. Rate limits on steering and speed, then publish. Stops skip the rate limit and go out at once.
12. Intent, published last, so a failure in it can never delay the drive command.

The geometry

From the LiDAR to the rear axle

The particle filter estimates the pose of the LiDAR, because it ray-casts each particle’s scan from the sensor. The bicycle model and the racing line both describe the rear axle, 0.26 m behind it (laser_offset_x). With pose_frame: laser the node moves the pose back along the car’s heading $\psi$:

def laser_pose_to_base_link(x, y, yaw, laser_offset_x, laser_offset_y):
    cos_yaw = math.cos(yaw)
    sin_yaw = math.sin(yaw)
    return (x - (laser_offset_x * cos_yaw - laser_offset_y * sin_yaw),
            y - (laser_offset_x * sin_yaw + laser_offset_y * cos_yaw),
            yaw)

Getting this wrong puts every target 0.26 m off, and the car tracks a parallel line beside the real one. With /slam_pose, which is already a rear-axle pose, pose_frame must be base_link.

Find the nearest point

The racing line is a list of points about 0.15 m apart. The node finds the point closest to the car, but it searches only the 40 points on either side of last tick’s answer (nearest_search_window). On a course that passes close to itself, such as a hairpin, the closest point overall can be on the other leg of the hairpin. A windowed search stays on the leg the car is on, and it is cheaper too.

Choose the lookahead

The lookahead distance $L_d$ grows with speed:

with $k$ = lookahead_speed_gain (0.15 s), $L_{\min }$ = 0.6 m and $L_{\max }$ = 1.5 m. At 4.0 m/s, $L_d=0.15\times 4.0+0.6=1.2$ m. The speed $v$ is the measured speed from /odom, not the profiled target, because the two differ most while braking into a corner. If /odom is older than 0.5 s, the profiled speed at the nearest point is used instead.

def adaptive_lookahead(speed, gain, min_lookahead, max_lookahead):
    ld = gain * abs(speed) + min_lookahead
    return float(np.clip(ld, min_lookahead, max_lookahead))

Why the lookahead matters so much. Suppose the car is driving parallel to a straight line, $e$ metres to its left. The target $L_d$ ahead on the line is then at $y\approx -e$ in the car’s frame, and the curvature formula below gives $\kappa \approx -2e/L_d^2$. The steering is $\delta \approx L\kappa$, so

Pure pursuit behaves like a proportional controller on the cross-track error, with a gain of $2L/L_d^2$ radians of steering per metre of error. At $L_d=0.6$ m that gain is 2.0 rad/m. At 1.5 m it is 0.32 rad/m, six times weaker. A short lookahead corrects hard, which tracks tight corners well at low speed but overshoots and weaves at high speed. A long lookahead corrects gently, which is smooth at speed but lets the car drift and start turning early. Scaling $L_d$ with speed takes each end where it works.

Find the target

From the nearest point, the node walks forward along the line, adding up the distance between points, until it has covered $L_d$. The point it reaches is the target. This snaps the target to a recorded point instead of intersecting the line with a circle exactly, which is simpler and off by at most the 0.15 m point spacing.

def find_lookahead_index(seg_len, nearest_index, lookahead_dist, closed=True):
    n = len(seg_len)
    acc = 0.0
    idx = nearest_index
    for _ in range(n):
        acc += seg_len[idx]
        nxt = idx + 1
        if nxt >= n:
            if not closed:
                return n - 1
            nxt = 0
        if acc >= lookahead_dist:
            return nxt
        idx = nxt
    return idx

seg_len[i] is the distance from point i to point i + 1. On a closed lap the walk wraps from the last point back to the first.

Rotate the target into the car’s frame

The target is known in map coordinates. Steering needs it relative to the car: $x$ forward and $y$ to the left. With $(\Delta x,\Delta y)$ the map offset from the rear axle to the target, rotating by $-\psi$ gives

A quick check: with the car facing along the map’s $x$ axis ($\psi =0$), a target to its left at $\Delta y>0$ gives $y=\Delta y>0$. Left stays left.

Curvature of the arc

The car can only drive arcs that leave the rear axle in the direction it is facing, so the arc’s centre lies on the car’s $y$ axis, at $(0,R)$. A positive $R$ puts the centre to the left, a left turn. The arc must also pass through the target $(x,y)$, which is at distance $R$ from the centre:

Expanding $(y-R{)}^2=y^2-2yR+R^2$, the $R^2$ terms cancel:

The curvature is $\kappa =1/R$. Since $x^2+y^2=L_d^2$ and $y=L_d\sin \alpha$, where $\alpha$ is the target’s angle from the car’s heading:

A target to the left ($y>0$) gives positive curvature, which matches the drive message’s convention that a positive steering angle turns left. The code uses the real squared distance $x^2+y^2$, not the requested $L_d$, because the snapped target is never at exactly $L_d$.

def steering_arc_curvature(x_body: float, y_body: float) -> float:
    dist_sq = x_body * x_body + y_body * y_body
    if dist_sq < 1e-9:
        return 0.0
    return 2.0 * y_body / dist_sq

Steering angle

The kinematic bicycle model from Lecture 6 (notes) turns a curvature into a front-wheel angle. Collapse each pair of wheels into one wheel on the centre line. The rear wheel cannot slide sideways, so the car turns about a point on the rear axle’s line, at distance $R$. The front wheel, $L$ further forward, must point at right angles to the line from that point. That makes a right-angled triangle with sides $R$ and $L$, and the angle at the turning point is the steering angle:

with $L$ = 0.36 m. A worked example: at 4.0 m/s the lookahead is 1.2 m. A target 10° to the left gives $\kappa =2\sin (10°)/1.2=0.289{\text{m}}^{-1}$, a 3.5 m radius, and $\delta =\arctan (0.36\times 0.289)=0.104$ rad, 5.9°.

def steering_from_curvature(kappa: float, wheelbase: float) -> float:
    return math.atan(wheelbase * kappa)

The whole steering law strings these together:

def steer(self, inp: TrackingInput, path: PathContext) -> SteeringResult:
    lookahead = racing_math.adaptive_lookahead(
        inp.speed, self.params.lookahead_speed_gain,
        self.params.min_lookahead, self.params.max_lookahead)
    target_idx = racing_math.find_lookahead_index(
        path.seg_len, inp.nearest_idx, lookahead, closed=path.closed)
    target_x, target_y = path.xy[target_idx]
    x_body, y_body = racing_math.world_to_body(
        float(target_x - inp.x), float(target_y - inp.y), inp.yaw)
    kappa = racing_math.steering_arc_curvature(x_body, y_body)
    unclipped = racing_math.steering_from_curvature(kappa, self.limits.wheelbase)
    steering = float(np.clip(
        unclipped, -self.limits.max_steering_angle, self.limits.max_steering_angle))
    ...

The angle is clipped to max_steering_angle here, and its change is limited to max_steering_rate (1.0 rad/s) immediately before publishing.

Speed

The base speed is the profiled speed at the car’s nearest point, clipped to [min_speed, max_speed]. The profile already contains the braking zones, so the nearest point’s speed is the right one for where the car is now. The target’s speed would brake too early.

The profile knows only the recorded line. When the car steers off it, for a localization correction, a swerve or an overtake, a second limit applies, from the steering commanded on this tick. Its curvature is ${\kappa }_{\text{cmd}}=\tan \delta /L$, and holding the sideways acceleration $v^2\kappa$ under the grip limit gives

with $a_{\text{lat}}$ = max_lateral_accel (2.5 m/s²). On the recorded line this limit is already met, so it only acts when the car does something the profile did not plan.

The commanded speed may rise by at most max_acceleration (6.0 m/s²) per second, starting from the larger of the last command and the measured speed. Every stop goes out at once, without any rate limit. max_acceleration has a narrow working range: at 3.0 the car cannot speed up again after a safety stop behind a slower car, and at 7.0 or more it arrives behind that car too fast to commit to a pass.

Parameters

Set in pure_pursuit.yamlGitHub. The lookahead and speed values can also be changed while racing, from the dashboard.

Each car’s steering limit is loaded on top of the shared file from its profile’s calibration/pure_pursuit.yaml:

Run it

race_launch.py starts the particle filter and pure_pursuit_node together on a saved run. See Map a course and race.

ros2 launch racerbot_launch race_launch.py run_dir:=$HOME/racerbot-ws/data/<id>/racerbot_auto/<run>

To run the controller alone, with localization already running and a profiled line:

ros2 launch pure_pursuit pure_pursuit_launch.py waypoints_file:=$HOME/racerbot-ws/src/pure_pursuit/waypoints/my_track_profiled.csv

Check it

ros2 topic hz /pf/viz/inferred_pose      # the pose the controller follows
ros2 topic hz /drive                     # 40 Hz while the node runs
ros2 topic echo /drive --field drive
ros2 param get /pure_pursuit_node steering_law

The terminal prints a decision line every second, in the same DRIVE [state] ... form as gap_follow. The line for normal driving includes the law’s own detail, for example pure_pursuit lookahead=1.20m target=412 kappa=+0.289/m: the lookahead, the index of the target point and the curvature asked for. With LB released the state is deadman_released and the speed is 0.00.

Tests

test_racing_math.pyGitHub checks each geometry function on this page against a closed form or an invariant, for example that a target to the left gives a left turn and that the curvature of a circle comes out as $1/R$. test_tracking_laws.pyGitHub checks every steering law with no ROS, and test_localization_watchdogs.py starts the node and checks the pose watchdogs: a frozen pose stops the car, a stale timestamp is caught even on a message that has just arrived, and a parked car does not trip either. See Run the tests.

Troubleshooting

Limitations

• The target is already round the bend before the car reaches it, so pure pursuit starts turning early and cuts the inside at corner entry. Stanley, LQR and MPC correct the error at the car instead, and trade this against sensitivity to delay and noise.
• It follows the recorded line exactly. It does not plan around the car’s limits beyond the speed profile and the off-line curvature limit.