Overview
========

The Global Path Planner computes the optimal racing line for the entire track after the first lap is complete. It uses the complete track map from SLAM to generate a single, time-optimal trajectory that the vehicle follows for all subsequent laps.

Purpose
-------

After completing the exploration lap, the vehicle has a complete map of the track. The global path planner uses this full information to:

1. **Compute optimal racing line**: Find the fastest path around the entire track
2. **Consider full track topology**: Account for upcoming corners and straights
3. **Generate repeatable trajectory**: Same path used for all remaining laps
4. **Maximize performance**: Achieve minimum lap times with full track knowledge

The global planner runs once when the global map is available, then the vehicle tracks this path for the remainder of the race.

Comparison with Local Planner
------------------------------

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

   * - Aspect
     - Local Planner
     - Global Planner
   * - **When Active**
     - First lap (exploration)
     - After first lap (racing)
   * - **Horizon**
     - 10 waypoints (~10-20m)
     - 100 waypoints (full track)
   * - **Update Frequency**
     - 1-5 Hz (every SLAM update)
     - Once (when global map received)
   * - **Solve Time**
     - 60-180ms
     - 1-5 seconds
   * - **Input**
     - Local cone observations
     - Complete track map
   * - **Objective**
     - Minimize time over short horizon
     - Minimize full lap time
   * - **Path Quality**
     - ~10-15% slower than optimal
     - Optimal racing line

System Architecture
-------------------

.. code-block:: text

    SLAM Global Map (after lap 1)
           ↓
    Cone Ordering ────→ Full Track Boundaries
           ↓
    Time-Optimal ────→ Complete Lap Trajectory
    Optimization       (100 waypoints)
           ↓
    Path Message ────→ MPC Controller
           ↓
    Node Shutdown (job complete)

The global planner operates once:

1. **Input**: Receives complete track map from SLAM
2. **Cone Ordering**: Sorts all cones into closed-loop left/right boundaries
3. **Trajectory Optimization**: Solves for time-optimal lap
4. **Output**: Publishes full-lap trajectory and signals completion
5. **Shutdown**: Node destroys itself (no longer needed)

Key Characteristics
-------------------

**Long Horizon**
~~~~~~~~~~~~~~~~

- Optimizes over N = 100 waypoints (configurable)
- Covers entire track (~100-200 meters depending on track size)
- Computation time: 1-5 seconds (acceptable since it runs once)

**Time-Optimal Objective**
~~~~~~~~~~~~~~~~~~~~~~~~~~~

Same as local planner:

.. math::

    \min \sum_{i=1}^N \Delta t_i

But now considers the entire lap, allowing for better planning of corner entry/exit speeds.

**Closed-Loop Path**
~~~~~~~~~~~~~~~~~~~~

Unlike the local planner, the global path forms a closed loop:

- First and last waypoints connect seamlessly
- Path repeats indefinitely
- Vehicle can track the same path for multiple laps

**Cone Collision Constraints**
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The global planner includes explicit cone avoidance constraints (unlike the local planner):

- Keeps vehicle bounding box away from all cones
- Uses approximate rectangular safety region around vehicle
- Considers N_CONES_PER_POINT nearest cones at each waypoint

**Note:** These constraints may be disabled depending on computational performance.

**See:** ``global_path/global_path/global_opt_compiled.py``

Track Representation
--------------------

Similar to the local planner, the track is represented as N pairs of left/right boundary points:

.. math::

    \text{position}_i = (1 - t_i) \cdot \text{left}_i + t_i \cdot \text{right}_i

Where t ∈ [0, 1] determines the racing line. However, for the global planner:

- N = 100 (covers full track)
- Path forms a closed loop (waypoint 101 = waypoint 1)
- Optimization can choose aggressive racing lines with full track knowledge

Integration with SLAM
----------------------

The global planner subscribes to ``/slam/map/global``, which SLAM publishes after:

1. Vehicle completes first lap (lap counter ≥ 1)
2. Loop closure is detected
3. Full track map is optimized

The global map contains:

- All observed cones (typically 50-200 cones depending on track size)
- Cone colors (blue/yellow for left/right boundaries)
- Optimized positions after loop closure

When the global map is received:

1. Global planner activates and starts optimization
2. Local planner receives the same message and shuts down
3. After optimization completes, global planner publishes path and shuts down

Optimization Time
-----------------

The global planner solves a much larger optimization problem than the local planner:

- **Variables**: 7 × 100 = 700 decision variables
- **Constraints**: ~1000-2000 constraints (dynamics, bounds, cone avoidance)
- **Solve time**: 1-5 seconds depending on track complexity

This is acceptable because:

- It only runs once per race
- Vehicle continues tracking local path until global path is ready
- 1-5 seconds is negligible compared to lap times (~20-40 seconds)

Output Format
-------------

The planner publishes a ``FebPath`` message containing the complete lap trajectory:

**States** (100 waypoints):

- ``x, y``: Position (meters)
- ``psi``: Heading angle (radians)
- ``v``: Velocity (m/s)
- ``th``: Steering angle (radians)

**Controls** (between waypoints):

- ``a``: Acceleration (m/s²)
- ``thdot``: Steering rate (rad/s)

The path is interpolated to a constant time grid (10ms intervals) to match the MPC control frequency, similar to the local planner.

Additionally, the global planner publishes:

- ``/path/finished`` (Bool): Signals that global path is available
- ``/path/global/viz`` (PointCloud): Path visualization

Typical Performance
-------------------

**Computation Time**
~~~~~~~~~~~~~~~~~~~~

- Cone ordering: 10-50ms (more cones than local)
- Optimization solve: 1-5 seconds
- Path interpolation: 10-20ms
- **Total**: 1-5 seconds (acceptable for one-time computation)

**Path Quality**
~~~~~~~~~~~~~~~~

- True optimal racing line (within solver tolerance)
- Maximum corner speeds (friction-limited)
- Optimal corner entry/exit trajectories
- Lap times typically 10-15% faster than local planner paths

**Lap Time Improvement**
~~~~~~~~~~~~~~~~~~~~~~~~~

Example improvements over local planner:

- Straight-heavy tracks: 5-10% faster
- Technical tracks: 10-15% faster
- Very tight tracks: 15-20% faster

The improvement comes from better anticipation of upcoming corners and optimized speed profiles.

Robustness
----------

**Solver Convergence**
~~~~~~~~~~~~~~~~~~~~~~

The larger problem is more challenging to solve:

- May require more iterations (50-200 vs 10-50 for local)
- Occasionally fails to converge on very complex tracks
- Warm-starting not available (no previous solution)

If the solver fails:

- Vehicle continues using local path planner
- Manual intervention may be required to adjust constraints
- Alternative: Pre-compute path offline and load from file

**Cone Ordering**
~~~~~~~~~~~~~~~~~

Cone ordering for the full track is more challenging:

- Must form a closed loop (first and last points connect)
- Longer boundaries increase risk of ordering errors
- Same distance-based algorithm as local planner (prone to same failure modes)

If cone ordering fails:

- Global planner won't produce a valid path
- Vehicle continues with local planner
- SLAM map quality should be verified

Node Lifecycle
--------------

The global planner node has a unique lifecycle:

1. **Startup**: Node initializes and waits for global map
2. **Activation**: Receives ``/slam/map/global`` and begins optimization
3. **Publishing**: Outputs global path and completion signal
4. **Shutdown**: Destroys itself (``self.destroy_node()``)

This design saves computational resources since the global planner is only needed once.

**See:** ``global_path/global_path/main.py:88``

Transition from Local to Global
--------------------------------

The handoff between local and global planners is designed to be seamless:

1. SLAM publishes global map
2. Local planner receives global map, publishes ``/path/finished``, shuts down
3. Global planner receives global map, starts optimization
4. Vehicle continues tracking previous local path during optimization
5. Global planner publishes new path
6. MPC seamlessly switches to tracking global path

From the MPC perspective, it simply tracks whatever path is published on ``/path/global`` or ``/path/local``, whichever is most recent.

Limitations
-----------

**Single Computation**
~~~~~~~~~~~~~~~~~~~~~~

The global path is computed once and never updated:

- Cannot adapt to changing track conditions
- If cones are moved, path may become invalid
- No dynamic obstacle avoidance

**Cone Ordering Challenges**
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Same limitations as local planner, but amplified by full track:

- Complex tracks may fail to order correctly
- No error recovery once optimization starts
- Requires high-quality SLAM map

**Computational Cost**
~~~~~~~~~~~~~~~~~~~~~~

While 1-5 seconds is acceptable for one-time computation:

- Very large tracks (>200 cones) may take >10 seconds
- Embedded systems may struggle with memory requirements
- Alternative: Pre-compute offline and load at runtime