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
-----------

.. code-block:: text

    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

.. toctree::
   :maxdepth: 2
   :caption: Local Path Documentation

   local_path/overview
   local_path/cone_ordering
   local_path/optimization
   local_path/configuration

Published Topics
----------------

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - 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
-----------------

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - 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
-------------

.. automodule:: local_path.main
   :members:
   :undoc-members:
   :show-inheritance:

.. automodule:: local_path.local_opt_compiled
   :members:
   :undoc-members:
   :show-inheritance:

.. automodule:: local_path.distance_cone_order
   :members:
   :undoc-members:
   :show-inheritance: