f110_planning.utils package¶

Submodules¶

f110_planning.utils.constants module¶

Global constants for the F1TENTH vehicle and its on-board LiDAR sensor.

This file is the single auditable source of truth for the physical and sensor constants used throughout the planning stack.

All values correspond to the standard F1TENTH 1:10 scale platform with a Hokuyo UST-10LX LiDAR (as configured in the f110_gym simulator).

f110_planning.utils.constants.F110_LENGTH: float = 0.58¶: Overall vehicle length [m].

f110_planning.utils.constants.F110_MAX_STEER: float = 0.4189¶: Maximum steering angle magnitude [rad] (≈ 24 °).

f110_planning.utils.constants.F110_WHEELBASE: float = 0.33¶: Distance between front and rear axle centres [m].

f110_planning.utils.constants.F110_WIDTH: float = 0.31¶: Vehicle width [m].

f110_planning.utils.constants.LIDAR_FOV: float = 4.7¶: Total LiDAR field of view [rad] (≈ 270 °).

f110_planning.utils.constants.LIDAR_MAX_ANGLE: float = 2.35¶: Angle of the last (rightmost) LiDAR beam [rad].

f110_planning.utils.constants.LIDAR_MIN_ANGLE: float = -2.35¶: Angle of the first (leftmost) LiDAR beam [rad].

f110_planning.utils.constants.LIDAR_NUM_BEAMS: int = 1080¶: Number of beams in one LiDAR scan.

f110_planning.utils.geometry_utils module¶

Geometry utilities

f110_planning.utils.geometry_utils.pi_2_pi(angle: float) → float¶

Normalizes an angle to the range [-pi, pi].

Parameters:: angle – The input angle in radians.
Returns:: The normalized angle in radians.

f110_planning.utils.lidar_utils module¶

LiDAR and path tracking utility functions.

f110_planning.utils.lidar_utils.get_heading_error(waypoints: ndarray, car_position: ndarray, car_theta: float) → float¶

Computes the orientation error between the car’s heading and the local path segment.

Parameters:

waypoints – Array of waypoints [N, 2+] containing at least [x, y].
car_position – Current vehicle [x, y] position.
car_theta – Current orientation of the vehicle in radians.

Returns:

Heading error in radians. Positive values indicate the path is to the car’s left; negative to the right.

f110_planning.utils.lidar_utils.get_side_distances(scan: ndarray) → tuple[float, float]¶

Extracts minimum distances to the left and right walls from raw LiDAR data.

This uses a small window around the 90-degree and -90-degree points to robustly estimate the wall distance even if the car is slightly rotated.

Parameters:: scan – The 1D array of LiDAR distances.
Returns:: A tuple (left_min, right_min) in meters.

f110_planning.utils.lidar_utils.index_to_angle(i: int, num_beams: int = 1080) → float¶

Convert a LiDAR scan index to its corresponding angle.

Parameters:

i – Scan index.
num_beams – Total number of beams in the scan.

Returns:

Angle in radians.

Return type:

float

f110_planning.utils.lqr_utils module¶

LQR utilities

f110_planning.utils.lqr_utils.solve_lqr(A: ndarray, B: ndarray, Q: ndarray, R: ndarray, tolerance: float, max_num_iteration: int) → ndarray¶

Solves the Discrete-time Algebraic Riccati Equation (DARE) iteratively.

This finds the steady-state feedback matrix K that minimizes the quadratic cost function of the system.

Parameters:

A – Discrete-time state transition matrix.
B – Discrete-time input matrix.
Q – State cost matrix.
R – Input cost matrix.
tolerance – Convergence threshold for the P matrix update.
max_num_iteration – Maximum number of iterations to perform.

Returns:

The optimal feedback gain matrix K.

f110_planning.utils.lqr_utils.update_matrix(vehicle_state: ndarray, state_size: int, timestep: float, wheelbase: float) → tuple[ndarray, ndarray]¶

Linearizes a kinematic bicycle model into discrete state-space form.

Parameters:

vehicle_state – Current state [x, y, heading, velocity].
state_size – Dimensions of the state vector.
timestep – Sampling time in seconds.
wheelbase – Physical distance between axles.

Returns:

A tuple of (Ad, Bd) matrices.

f110_planning.utils.nn_models module¶

Shared DNN architectures and serialisation helpers for F1TENTH.

Architecture overview (all produce a single scalar output):: 1 – tiny single-channel CNN (~134 params) 2 – 3-stage single-channel CNN (~small) 3 – [Conv(1→1), Conv(1→8)] → Linear(1072, 32) (~small) 4 – [Conv(1→1), Conv(1→16)] → Linear(2144, 32) (~small-medium) 5 – [Conv(1→8), Conv(8→16)] → Linear(2144, 64) (~medium) 6 – [Conv(1→8), Conv(8→32)] → Linear(4288, 64) (~medium-large) 7 – [Conv(1→16), Conv(16→32)] → Linear(4288, 128) (~large)

f110_planning.utils.nn_models.get_architecture(arch_id: int) → Module¶

Factory function for F1TENTH single-output LiDAR neural network architectures.

All architectures accept a (batch, 1, 1080) tensor and return a (batch, 1) scalar — suitable for heading error, left wall distance, or track width prediction.

Parameters:: arch_id – An integer in {1, 2, 3, 4, 5, 6, 7} identifying the specific layer configuration (ordered roughly by parameter count).
Returns:: An uninitialised nn.Sequential module.
Raises:: ValueError – If arch_id is not in the supported set.

f110_planning.utils.nn_models.load_torchscript(path: str | Path, map_location: str | device = 'cpu') → tuple[ScriptModule, dict[str, Any]]¶

Load a self-sufficient TorchScript model saved by save_as_torchscript().

For non-quantized models the returned torch.ScriptModule is ready for inference with no additional imports required — no nn_models.py, no arch_id, no config file.

For quantized models (metadata["quantized"] is True), the returned module is the FP32 TorchScript graph. Use the "arch_id" and the "_state_dict_bytes" private key in metadata to reconstruct the quantized module:

scripted, meta = load_torchscript(path)
if meta["quantized"]:
    model = get_architecture(meta["arch_id"])
    buf = io.BytesIO(meta["_state_dict_bytes"])
    model.load_state_dict(torch.load(buf, map_location=device))
    model.eval()
    quantize_(model, Int8DynamicActivationInt8WeightConfig())

Parameters:

path – Path to the .pt file produced by save_as_torchscript().
map_location – Device to map tensors to on load.

Returns:

A (scripted_model, metadata) tuple where metadata mirrors the dict passed to save_as_torchscript(). For quantized files an extra "_state_dict_bytes" key (raw bytes) is injected by this function for downstream reconstruction.

f110_planning.utils.nn_models.save_as_torchscript(model: Module, path: str | Path, metadata: dict[str, Any]) → None¶

Save an nn.Module as a self-sufficient TorchScript .pt file.

The saved file embeds both the full computation graph (via torch.jit.script) and all metadata (arch_id, target_col, training config, quantization flag, …) as a JSON sidecar inside the same archive. No separate architecture class or external config file is required to load a non-quantized model.

For quantized models (metadata["quantized"] is True) the FP32 computation graph is scripted and the FP32 state_dict is additionally embedded as a binary extra file. This is necessary because torchao.AffineQuantizedTensor weights are not TorchScript-serialisable. At load time the caller rebuilds the module, loads the FP32 weights, and re-applies quantize_()—which is semantically equivalent since torchao INT8 weight quantisation is deterministic given the FP32 weights.

Parameters:

model – The FP32 nn.Module to script and save. Must be in eval mode and not yet quantized.
path – Destination path for the .pt file.
metadata – Dict of metadata to embed (must be JSON-serialisable). Should contain at minimum "quantized" (bool). For quantized models "arch_id" is also required so the model can be faithfully reconstructed at load time.

f110_planning.utils.pure_pursuit_utils module¶

Pure Pursuit utilities

f110_planning.utils.pure_pursuit_utils.get_actuation(pose_theta: float, lookahead_point: ndarray, position: ndarray, lookahead_distance: float, wheelbase: float) → tuple[float, float]¶

Computes steering and velocity based on a lookahead point using Pure Pursuit math.

Parameters:

pose_theta – Current orientation of the vehicle in radians.
lookahead_point – Target point in global [x, y, v] format.
position – Current [x, y] coordinates of the vehicle.
lookahead_distance – Euclidean distance to the lookahead point.
wheelbase – Physical distance between axles.

Returns:

(speed, steering_angle) in m/s and radians respectively.

Return type:

tuple

f110_planning.utils.pure_pursuit_utils.intersect_point(point: ndarray, radius: float, trajectory: ndarray, t: float = 0.0, wrap: bool = False) → tuple[ndarray | None, int | None, float | None]¶

Finds the first point on a trajectory a specific distance away from a given point.

This function searches forward from the provided progress parameter ‘t’. It is used to find the lookahead point for Pure Pursuit.

Parameters:

point – Current [x, y] position of the vehicle.
radius – Lookahead distance.
trajectory – Array of [x, y, v] waypoints.
t – Starting progress on the trajectory (index + fractional part).
wrap – Whether the trajectory should wrap around (closed loop).

Returns:

(intersection_point, segment_index, segment_fraction): Returns (None, None, None) if no intersection is found within the search range.

Return type:

tuple

f110_planning.utils.pure_pursuit_utils.nearest_point(point: ndarray, trajectory: ndarray) → tuple[ndarray, float, float, int]¶

Return the nearest point along the given piecewise linear trajectory.

Parameters:

point (numpy.ndarray, (2, )) – (x, y) of current pose
trajectory (numpy.ndarray, (N, 2)) – array of (x, y) trajectory waypoints NOTE: points in trajectory should ideally be unique. Degenerate segments (consecutive identical points) are now handled gracefully by treating their projection parameter as zero, effectively ignoring the zero-length segment. This prevents numba from raising a SystemError on division by zero.

Returns:

nearest point on the trajectory to the point nearest_dist (float): distance to the nearest point t (float): projection fraction in [0, 1] along the nearest trajectory segment i (int): index of nearest point in the array of trajectory waypoints

Return type:

nearest_point (numpy.ndarray, (2, ))

f110_planning.utils.reactive_utils module¶

Utilities for reactive navigation, including coordinate conversions and LiDAR mappings.

f110_planning.utils.reactive_utils.get_reactive_action(planner: Any, **kwargs: Any) → Action¶

Wraps reactive actuation into a standard Action object for use in planners.

Parameters:

planner – High-level planner instance (e.g., LidarDNNPlanner).
**kwargs – Additional arguments for get_reactive_actuation (dists, errors, position).

Returns:

An Action instance containing the calculated steering and speed.

f110_planning.utils.reactive_utils.get_reactive_actuation(left_dist: float, right_dist: float, heading_error: float, car_position: ndarray, car_theta: float, current_speed: float, lookahead_gain: float, max_speed: float, wheelbase: float, lateral_gain: float) → tuple[ndarray, float, float]¶

Computes a dynamic waypoint and actuation using reactive features.

This function centers the car between walls based on distance sensors and aligns it with predicted heading errors, effectively acting as a reactive controller that mimics human lane-keeping behavior.

Parameters:

left_dist – Predicted or sensed distance to the left wall.
right_dist – Predicted or sensed distance to the right wall.
heading_error – Predicted orientation error relative to the corridor.
car_position – Current [x, y] coordinates of the vehicle.
car_theta – Current orientation of the vehicle in radians.
current_speed – Current longitudinal velocity.
lookahead_gain – Scaling factor for the adaptive lookahead distance.
max_speed – Maximum allow speed for the plan.
wheelbase – Physical distance between front and rear axles.
lateral_gain – Scaling factor for the lateral correction.

Returns:

(target_point_global, steering_angle, speed)

target_point_global: [x, y, target_speed] in the map frame.
steering_angle: Calculated steering command in radians.
speed: Calculated velocity command in m/s.

Return type:

tuple

f110_planning.utils.sim_utils module¶

Shared utilities and constants for F1TENTH simulation and data generation scripts.

f110_planning.utils.sim_utils.add_common_sim_args(parser: ArgumentParser, multi_waypoint: bool = False) → None¶

Registers standard simulation CLI arguments to an ArgumentParser.

Parameters:

parser – The ArgumentParser instance to populate.
multi_waypoint – If True, allows multiple waypoint files to be passed, which setup_env will interpret as multiple agents.

f110_planning.utils.sim_utils.setup_env(args: Namespace, render_mode: str | None = None) → Any¶

Initializes a standard F1TENTH Gym environment based on CLI arguments.

Parameters:

args – Parsed arguments containing map, waypoints, and render settings.
render_mode – Overrides the render mode in args if provided.

Returns:

The initialized F1TENTH Gym environment.

f110_planning.utils.tracking_utils module¶

Utility functions for tracking planners.

f110_planning.utils.tracking_utils.calculate_tracking_errors(vehicle_state: ndarray, waypoints: ndarray, wheelbase: float) → tuple[float, float, int, float]¶

Calculates lateral and heading errors relative to the front axle hub.

This projection allows controllers like Stanley to compensate for the vehicle’s non-holonomic constraints more effectively by tracking from the steering pivot.

Parameters:

vehicle_state – Current state [x, y, heading, velocity].
waypoints – Reference path coordinates [x, y, …].
wheelbase – Physical distance between front and rear axles.

Returns:

(theta_e, ef, target_index, goal_velocity)

theta_e: Heading error in radians.
ef: Lateral crosstrack error at the front axle.
target_index: Index of the closest path segment.
goal_velocity: Target speed at that segment.

Return type:

tuple

f110_planning.utils.tracking_utils.get_vehicle_state(obs: dict[str, Any], ego_idx: int) → ndarray¶

Extracts the vehicle’s pose and velocity from a gym observation dictionary.

Parameters:

obs – The raw observation dictionary from the environment.
ego_idx – The index of the vehicle to extract state for.

Returns:

A numpy array containing [x, y, theta, linear_vel_x].

f110_planning.utils.waypoint_utils module¶

Utilities for loading and processing waypoints.

f110_planning.utils.waypoint_utils.load_waypoints(file_path: str, delimiter: str = '\t') → ndarray¶

Loads waypoints from a CSV or TSV file.

The function attempts to find ‘x_m’ and ‘y_m’ columns first, then falls back to taking the first two columns if those headers are missing.

Parameters:

file_path – Path to the waypoint file on disk.
delimiter – The character separating values in the file.

Returns:

A numpy array of shape (N, 2) containing [x, y] coordinates.

Module contents¶

Utility functions for motion planners

f110_planning.utils.add_common_sim_args(parser: ArgumentParser, multi_waypoint: bool = False) → None¶

Registers standard simulation CLI arguments to an ArgumentParser.

Parameters:

parser – The ArgumentParser instance to populate.
multi_waypoint – If True, allows multiple waypoint files to be passed, which setup_env will interpret as multiple agents.

f110_planning.utils.calculate_tracking_errors(vehicle_state: ndarray, waypoints: ndarray, wheelbase: float) → tuple[float, float, int, float]¶

Calculates lateral and heading errors relative to the front axle hub.

This projection allows controllers like Stanley to compensate for the vehicle’s non-holonomic constraints more effectively by tracking from the steering pivot.

Parameters:

vehicle_state – Current state [x, y, heading, velocity].
waypoints – Reference path coordinates [x, y, …].
wheelbase – Physical distance between front and rear axles.

Returns:

(theta_e, ef, target_index, goal_velocity)

theta_e: Heading error in radians.
ef: Lateral crosstrack error at the front axle.
target_index: Index of the closest path segment.
goal_velocity: Target speed at that segment.

Return type:

tuple

f110_planning.utils.get_actuation(pose_theta: float, lookahead_point: ndarray, position: ndarray, lookahead_distance: float, wheelbase: float) → tuple[float, float]¶

Computes steering and velocity based on a lookahead point using Pure Pursuit math.

Parameters:

pose_theta – Current orientation of the vehicle in radians.
lookahead_point – Target point in global [x, y, v] format.
position – Current [x, y] coordinates of the vehicle.
lookahead_distance – Euclidean distance to the lookahead point.
wheelbase – Physical distance between axles.

Returns:

(speed, steering_angle) in m/s and radians respectively.

Return type:

tuple

f110_planning.utils.get_heading_error(waypoints: ndarray, car_position: ndarray, car_theta: float) → float¶

Computes the orientation error between the car’s heading and the local path segment.

Parameters:

waypoints – Array of waypoints [N, 2+] containing at least [x, y].
car_position – Current vehicle [x, y] position.
car_theta – Current orientation of the vehicle in radians.

Returns:

Heading error in radians. Positive values indicate the path is to the car’s left; negative to the right.

f110_planning.utils.get_reactive_action(planner: Any, **kwargs: Any) → Action¶

Wraps reactive actuation into a standard Action object for use in planners.

Parameters:

planner – High-level planner instance (e.g., LidarDNNPlanner).
**kwargs – Additional arguments for get_reactive_actuation (dists, errors, position).

Returns:

An Action instance containing the calculated steering and speed.

f110_planning.utils.get_reactive_actuation(left_dist: float, right_dist: float, heading_error: float, car_position: ndarray, car_theta: float, current_speed: float, lookahead_gain: float, max_speed: float, wheelbase: float, lateral_gain: float) → tuple[ndarray, float, float]¶

Computes a dynamic waypoint and actuation using reactive features.

This function centers the car between walls based on distance sensors and aligns it with predicted heading errors, effectively acting as a reactive controller that mimics human lane-keeping behavior.

Parameters:

left_dist – Predicted or sensed distance to the left wall.
right_dist – Predicted or sensed distance to the right wall.
heading_error – Predicted orientation error relative to the corridor.
car_position – Current [x, y] coordinates of the vehicle.
car_theta – Current orientation of the vehicle in radians.
current_speed – Current longitudinal velocity.
lookahead_gain – Scaling factor for the adaptive lookahead distance.
max_speed – Maximum allow speed for the plan.
wheelbase – Physical distance between front and rear axles.
lateral_gain – Scaling factor for the lateral correction.

Returns:

(target_point_global, steering_angle, speed)

target_point_global: [x, y, target_speed] in the map frame.
steering_angle: Calculated steering command in radians.
speed: Calculated velocity command in m/s.

Return type:

tuple

f110_planning.utils.get_side_distances(scan: ndarray) → tuple[float, float]¶

Extracts minimum distances to the left and right walls from raw LiDAR data.

This uses a small window around the 90-degree and -90-degree points to robustly estimate the wall distance even if the car is slightly rotated.

Parameters:: scan – The 1D array of LiDAR distances.
Returns:: A tuple (left_min, right_min) in meters.

f110_planning.utils.get_vehicle_state(obs: dict[str, Any], ego_idx: int) → ndarray¶

Extracts the vehicle’s pose and velocity from a gym observation dictionary.

Parameters:

obs – The raw observation dictionary from the environment.
ego_idx – The index of the vehicle to extract state for.

Returns:

A numpy array containing [x, y, theta, linear_vel_x].

f110_planning.utils.index_to_angle(i: int, num_beams: int = 1080) → float¶

Convert a LiDAR scan index to its corresponding angle.

Parameters:

i – Scan index.
num_beams – Total number of beams in the scan.

Returns:

Angle in radians.

Return type:

float

f110_planning.utils.intersect_point(point: ndarray, radius: float, trajectory: ndarray, t: float = 0.0, wrap: bool = False) → tuple[ndarray | None, int | None, float | None]¶

Finds the first point on a trajectory a specific distance away from a given point.

This function searches forward from the provided progress parameter ‘t’. It is used to find the lookahead point for Pure Pursuit.

Parameters:

point – Current [x, y] position of the vehicle.
radius – Lookahead distance.
trajectory – Array of [x, y, v] waypoints.
t – Starting progress on the trajectory (index + fractional part).
wrap – Whether the trajectory should wrap around (closed loop).

Returns:

(intersection_point, segment_index, segment_fraction): Returns (None, None, None) if no intersection is found within the search range.

Return type:

tuple

f110_planning.utils.load_waypoints(file_path: str, delimiter: str = '\t') → ndarray¶

Loads waypoints from a CSV or TSV file.

The function attempts to find ‘x_m’ and ‘y_m’ columns first, then falls back to taking the first two columns if those headers are missing.

Parameters:

file_path – Path to the waypoint file on disk.
delimiter – The character separating values in the file.

Returns:

A numpy array of shape (N, 2) containing [x, y] coordinates.

f110_planning.utils.nearest_point(point: ndarray, trajectory: ndarray) → tuple[ndarray, float, float, int]¶

Return the nearest point along the given piecewise linear trajectory.

Parameters:

point (numpy.ndarray, (2, )) – (x, y) of current pose
trajectory (numpy.ndarray, (N, 2)) – array of (x, y) trajectory waypoints NOTE: points in trajectory should ideally be unique. Degenerate segments (consecutive identical points) are now handled gracefully by treating their projection parameter as zero, effectively ignoring the zero-length segment. This prevents numba from raising a SystemError on division by zero.

Returns:

Return type:

nearest_point (numpy.ndarray, (2, ))

f110_planning.utils.pi_2_pi(angle: float) → float¶

Normalizes an angle to the range [-pi, pi].

Parameters:: angle – The input angle in radians.
Returns:: The normalized angle in radians.

f110_planning.utils.setup_env(args: Namespace, render_mode: str | None = None) → Any¶

Initializes a standard F1TENTH Gym environment based on CLI arguments.

Parameters:

args – Parsed arguments containing map, waypoints, and render settings.
render_mode – Overrides the render mode in args if provided.

Returns:

The initialized F1TENTH Gym environment.

f110_planning.utils.solve_lqr(A: ndarray, B: ndarray, Q: ndarray, R: ndarray, tolerance: float, max_num_iteration: int) → ndarray¶

Solves the Discrete-time Algebraic Riccati Equation (DARE) iteratively.

This finds the steady-state feedback matrix K that minimizes the quadratic cost function of the system.

Parameters:

A – Discrete-time state transition matrix.
B – Discrete-time input matrix.
Q – State cost matrix.
R – Input cost matrix.
tolerance – Convergence threshold for the P matrix update.
max_num_iteration – Maximum number of iterations to perform.

Returns:

The optimal feedback gain matrix K.

f110_planning.utils.update_matrix(vehicle_state: ndarray, state_size: int, timestep: float, wheelbase: float) → tuple[ndarray, ndarray]¶

Linearizes a kinematic bicycle model into discrete state-space form.

Parameters:

vehicle_state – Current state [x, y, heading, velocity].
state_size – Dimensions of the state vector.
timestep – Sampling time in seconds.
wheelbase – Physical distance between axles.

Returns:

A tuple of (Ad, Bd) matrices.