Local Path Planning

The Local Path Planning module generates optimal short-horizon trajectories during the exploration phase (first lap). It processes locally observed cones from SLAM and computes time-optimal paths between track boundaries using nonlinear trajectory optimization.

Overview

During the first lap, the vehicle doesn’t have a complete map of the track. The local path planner enables autonomous navigation by processing partial cone observations and computing safe, time-optimal trajectories in real-time.

Key Features:

• Short-horizon optimization: Plans 10 waypoints (~10-20m lookahead)

• Time-optimal objective: Minimizes lap time while respecting vehicle dynamics

• Distance-based cone ordering: Sorts unordered cone observations into track boundaries

• Real-time capable: Solves in 60-180ms per update

• Bicycle dynamics: Uses same model as MPC for consistency

Operational Phase:

• Active: During first lap (exploration)

• Inactive: After global map is available (hands off to global planner)

System Flow

SLAM Local Map
     ↓
Cone Filtering (remove cones behind vehicle)
     ↓
Cone Ordering (distance-based greedy algorithm)
     ↓
Trajectory Optimization (time-optimal with dynamics constraints)
     ↓
Path Interpolation (convert to constant 100Hz time grid)
     ↓
FebPath Message → MPC Controller

Local Path Documentation

• Overview
  • Purpose
  • System Architecture
  • Key Characteristics
  • Integration with SLAM
  • Robustness Features
  • Output Format
  • Typical Performance
  • Limitations
  • Transition to Global Planner
• Cone Ordering
  • Problem Statement
  • Distance-Based Ordering
  • Implementation Details
  • Algorithm Characteristics
  • Edge Case Failures
  • Alternative Algorithm
  • Visualization
  • Configuration
• Trajectory Optimization
  • Optimization Problem
  • Decision Variables
  • Vehicle Dynamics Constraints
  • Velocity Constraints
  • Acceleration Constraints
  • Steering Constraints
  • Friction Circle Constraint
  • Initial Condition Constraints
  • Terminal Constraints
  • Solver Configuration
  • Solver Performance
  • Path Interpolation
  • Debugging
• Configuration
  • Settings Structure
  • Optimization Parameters
  • Vehicle Parameters
  • Constraint Limits
  • Cone Ordering Configuration
  • Debugging and Visualization
  • ROS Topics
  • Example Configurations

Published Topics

Topic	Type	Description
/path/local	feb_msgs/FebPath	Optimized local trajectory
/path/local/viz	sensor_msgs/PointCloud	Path waypoints for visualization
/path/local/vizpath	nav_msgs/Path	Path with full pose information
/path/local/vizconeorder	visualization_msgs/Marker	Cone pairing visualization (red lines)

Subscribed Topics

Topic	Type	Description
/slam/map/local	feb_msgs/Map	Local cone observations from SLAM
/slam/map/global	feb_msgs/Map	Global map (triggers planner shutdown)
/slam/state	feb_msgs/State	Current vehicle state (x, y, heading, velocity)
/path/finished	std_msgs/Bool	Signal that path planning is complete

Performance Characteristics

Computation Time:

• Cone ordering: 5-20ms

• Trajectory optimization: 50-150ms

• Path interpolation: 5-10ms

• Total: 60-180ms per update

Path Quality:

• Smooth, dynamically feasible trajectories

• Lap times typically within 10-15% of optimal (global planner)

• Conservative near track boundaries for safety

Update Rate:

• Triggered by SLAM local map updates (~1-5 Hz)

• Not time-critical (MPC extrapolates if needed)

API Reference

class local_path.local_opt_compiled.CompiledLocalOpt(N, nlp_solver='ipopt', solver_opts=None, car_params={'l_f': 1.5213, 'l_r': 1.4987, 'm': 1.0}, bbox={'l': 2, 'w': 1}, **constraints)

Bases: object

DEFAULT_SOPTS = {'ipopt': {'acceptable_obj_change_tol': 0.001, 'acceptable_tol': 0.01, 'expand': True, 'honor_original_bounds': True, 'ipopt.linear_solver': 'ma57'}, 'snopt': {'expand': False}, 'worhp': {'expand': True, 'worhp.NLPprint': -1}}

construct_solver(generate_c=False, compile_c=False, use_c=False, gcc_opt_flag='-Ofast')

creates a solver object

Parameters

• generate_c (bool, optional) – whether or not to generate new C code. Defaults to False.

• compile_c (bool, optional) – whether or not to look for and compile the C code. Defaults to False.

• use_c (bool, optional) – whether or not to load the compiled C code. Defaults to False.

• gcc_opt_flag (str, optional) – optimization flags to pass to GCC. can be -O1, -O2, -O3, or -Ofast depending on how long you’re willing to wait. Defaults to ‘-Ofast’.

load_solver()

alternative to construct_solver if you’re just loading a saved solver.

angle(a, b)

to_constant_tgrid(dt, z, u, t)

converts a solver result to a constant-dt representation. z, u, and t can be passed in with **res (from solve())

Parameters

• dt (float) – desired resultant time discretization interval

• z (np.ndarray) – states from solver

• u (np.ndarray) – controls from solver

• t (np.ndarray) – timestamps from solver

Returns

arrays of states, controls

Return type

(np.ndarray, np.ndarray)

solve(left, right, curr_state, err_ok=True)

crunch the numbers.

Parameters

• left (np.ndarray) – location of left cones. shape (N, 2).

• right (np.ndarray) – location of right cones. shape (N, 2).

• curr_state (np.ndarray) – current state of the car. shape (x, y, phi, v).

Returns

result of solve. keys ‘z’ (states), ‘u’ (controls), ‘t’ (timestamps)

Return type

dict

local_path.distance_cone_order.distance_cone_order(msg, state)

Parameters

• msg (feb_msgs.msg.Map) –

• state (list[float]) –

local_path.distance_cone_order.interpolate_between_cones(left_distances, right_distances)

local_path.distance_cone_order.test_interpolation()

local_path.distance_cone_order.plot_interpolation(left, right)