Global Path Planning
====================

The Global Path Planning module computes the optimal racing line for the complete track after the first lap is finished. It processes the full track map from SLAM and generates a single time-optimal trajectory that is used for all subsequent laps.

Overview
--------

After the vehicle completes its first exploration lap, SLAM publishes a complete map of the track. The global path planner uses this full track knowledge to compute the fastest possible racing line, considering the entire lap topology and optimizing corner entry/exit speeds.

**Key Features:**

- **Full-track optimization**: Plans 100 waypoints covering the entire lap
- **Time-optimal racing line**: Minimizes lap time with complete track knowledge
- **One-time computation**: Runs once when global map is available, then shuts down
- **Closed-loop path**: Generates repeatable trajectory for all remaining laps
- **10-15% faster**: Significant improvement over local planner paths

**Operational Phase:**

- **Inactive**: During first lap (local planner active)
- **Active**: After global map received (computes optimal path)
- **Shutdown**: After publishing path (job complete)

System Flow
-----------

.. code-block:: text

    SLAM Global Map (after lap 1 complete)
         ↓
    Cone Ordering (sort all cones into closed loop)
         ↓
    Trajectory Optimization (time-optimal over full lap)
         ↓
    Path Interpolation (convert to constant 100Hz grid)
         ↓
    Publish Path + Completion Signal
         ↓
    Node Shutdown (no longer needed)

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

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

   * - Aspect
     - Local Planner
     - Global Planner
   * - **Horizon**
     - 10 waypoints (~10-20m)
     - 100 waypoints (full lap)
   * - **When Active**
     - First lap only
     - After first lap
   * - **Solve Time**
     - 60-180ms
     - 1-5 seconds
   * - **Update Frequency**
     - Every SLAM update (~1-5 Hz)
     - Once per race
   * - **Path Type**
     - Open-ended short segment
     - Closed-loop full lap
   * - **Performance**
     - ~10-15% slower than optimal
     - Optimal racing line

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

   global_path/overview
   global_path/configuration

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

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

   * - Topic
     - Type
     - Description
   * - ``/path/global``
     - feb_msgs/FebPath
     - Optimized global trajectory (full lap)
   * - ``/path/finished``
     - std_msgs/Bool
     - Signals global path is ready (True)
   * - ``/path/global/viz``
     - sensor_msgs/PointCloud
     - Path waypoints for visualization

Subscribed Topics
-----------------

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

   * - Topic
     - Type
     - Description
   * - ``/slam/map/global``
     - feb_msgs/Map
     - Complete track map (triggers optimization)
   * - ``/slam/state``
     - feb_msgs/State
     - Current vehicle state (for initialization)

Performance Characteristics
---------------------------

**Computation Time:**

- Cone ordering: 10-50ms
- Trajectory optimization: 1-5 seconds
- Path interpolation: 10-20ms
- **Total**: 1-5 seconds (one-time)

**Path Quality:**

- True optimal racing line (within solver tolerance)
- Friction-limited cornering speeds
- Optimal braking/acceleration points
- **Lap time**: 10-15% faster than local planner

**Problem Size:**

- Decision variables: 700 (7 per waypoint × 100 waypoints)
- Constraints: 1000-2000 (dynamics, bounds, optional cone avoidance)
- Solver iterations: 50-200 (depends on track complexity)

Optimization Approach
---------------------

The global planner uses the same time-optimal formulation as the local planner:

**Objective:**

.. math::

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

**Subject to:**

- Bicycle dynamics constraints
- Track boundary constraints (closed loop)
- Velocity, acceleration, steering limits
- Friction circle constraint

**Key difference:** The optimization considers the entire lap as one connected path, allowing it to plan optimal corner entry/exit speeds by "looking ahead" to future sections of the track.

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

The global planner has a unique lifecycle designed to save resources:

1. **Initialization**: Node starts and waits for global map
2. **Triggered**: Receives ``/slam/map/global`` message
3. **Optimization**: Solves full-lap trajectory (1-5 seconds)
4. **Publishing**: Outputs global path and completion signal
5. **Shutdown**: Calls ``self.destroy_node()`` and exits

This design ensures the global planner only consumes resources when needed.

Integration with System
-----------------------

**Transition from Local to Global:**

1. Vehicle completes first lap
2. SLAM publishes ``/slam/map/global``
3. Local planner receives message and shuts down
4. Global planner receives message and starts optimization
5. Vehicle continues tracking previous local path
6. Global planner publishes new path
7. MPC seamlessly switches to tracking global path
8. Global planner shuts down

**MPC Integration:**

The MPC controller subscribes to both ``/path/local`` and ``/path/global``. It tracks whichever path is most recently published, making the transition seamless.

API Reference
-------------

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

.. automodule:: global_path.global_opt_compiled
   :members:
   :undoc-members:
   :show-inheritance:

.. automodule:: global_path.ConeOrdering
   :members:
   :undoc-members:
   :show-inheritance: