# Path Planner

This document describes the mission path-planning workflow implemented in `path_planner.py`.

The `PathPlanner` class connects the trained DQN policy to a practical mission-generation workflow. It loads a trained policy network, uses it to plan actions in the simulation environment, converts those actions into geodetic movement, and optionally exports the resulting mission path to ArduPilot/QGroundControl-compatible mission files.

## Overview

The path planner acts as the bridge between:

- the trained DQN policy
- the custom 3D simulation environment
- geodetic mission coordinates
- ArduPilot/QGroundControl waypoint mission output

The DQN policy operates in the custom Cartesian simulation environment. The path planner wraps this policy so that the selected actions can be converted into latitude, longitude, and altitude waypoints.

The resulting mission path can be exported as:

- a `.txt` mission file in the `QGC WPL 110` waypoint format
- a `.csv` file containing simplified latitude/longitude points for inspection or plotting

## Project Context

This path planner was developed as part of a broader search-and-rescue drone project.

The reinforcement learning component uses a Deep Q-Network (DQN) to select discrete movement actions. The model receives environment state information based on the drone's position and goal position in Cartesian coordinates. The selected actions are then interpreted by the path planner and mapped into geodetic coordinates.

The mission output is written in a format that can be used by ArduPilot-compatible ground-control workflows, such as QGroundControl or Mission Planner.

## Main Responsibilities

`PathPlanner` handles:

- loading a trained DQN policy checkpoint
- reconstructing the correct DQN architecture
- converting geodetic start/goal coordinates into the simulation frame
- running the policy in the environment
- mapping DQN actions to geodetic movement
- generating ArduPilot/QGroundControl-style mission waypoints
- optionally writing mission files to TXT and CSV
- optionally rendering the simulated path

## Model Registry

`path_planner.py` contains a `NETWORKS` dictionary that maps known model IDs to their architecture settings.

Each entry stores:

```text
model_id: (hidden_nodes, hidden_layers)
```

For example:

```python
NETWORKS = {
    "final_DQN": (512, 2),
    "_27_16_10": (512, 3),
    "_26_23_6": (512, 2),
}
```

This allows the path planner to automatically reconstruct the correct DQN architecture before loading the saved policy checkpoint.

If a model ID is not listed in `NETWORKS`, the number of hidden layers and hidden nodes must be provided manually using command-line arguments.

## Geodetic and Cartesian Coordinates

The DQN model operates inside the custom Cartesian simulation environment.

However, the mission planner accepts geodetic coordinates:

```text
latitude, longitude, altitude
```

The planner uses `pyproj.Geod` with the WGS84 ellipsoid to apply geodetic forward movements from the current latitude and longitude.

The planner also estimates an equivalent Cartesian goal position for the simulation environment. This allows the DQN to operate in the coordinate system it was trained on while still producing mission output in geodetic coordinates.

## Action Mapping

The trained DQN selects actions from the environment action space. The path planner interprets these actions as horizontal geodetic movement or altitude changes.

| Action | Mission movement |
|---|---|
| `0` | Horizontal movement using azimuth `0` |
| `1` | Horizontal movement using azimuth `90` |
| `2` | Increase altitude |
| `3` | Horizontal movement using azimuth `180` |
| `4` | Horizontal movement using azimuth `270` |
| `5` | Decrease altitude |
| `6` | No mapped movement |

The horizontal movement size is controlled by:

```text
delta_dist
```

The altitude movement size also uses `delta_dist`.

The simulation movement size is controlled separately by:

```text
step_length
```

## `PathPlanner`

`PathPlanner` is the main class used to generate mission paths.

### Constructor

```python
planner = PathPlanner(
    mission_dir="missions",
    model_dir="models",
    id="final_DQN",
    geo_start=(42.355118, -71.071305, 6),
    geo_goal=(42.355280, -71.070911, 26.5),
    verbosity=0,
    step_length=10,
    threshold=40,
    layers=2,
    neurons=512,
    delta_dist=0.5,
    cartesian_start=(5000, 5000, 5000),
    render=False,
    to_txt=False,
    to_csv=False,
)
```

### Key Arguments

| Argument | Description |
|---|---|
| `mission_dir` | Directory where generated mission files are written |
| `model_dir` | Directory containing saved model checkpoints |
| `id` | Model ID/prefix used to load the policy checkpoint |
| `geo_start` | Geodetic start position as `(latitude, longitude, altitude)` |
| `geo_goal` | Geodetic goal position as `(latitude, longitude, altitude)` |
| `verbosity` | Environment logging level |
| `step_length` | Simulation step size for environment actions |
| `threshold` | Distance threshold for mission completion in the simulation |
| `layers` | Number of hidden layers in the DQN model |
| `neurons` | Number of hidden nodes in the DQN model |
| `delta_dist` | Real-world movement distance for each geodetic action |
| `cartesian_start` | Starting position in the simulation environment |
| `render` | Whether to render the simulated path after planning |
| `to_txt` | Whether to export the generated mission path to a TXT file |
| `to_csv` | Whether to export latitude/longitude waypoints to CSV |

## Main Methods

## `set_start`

```python
set_start(lat, lon, alt)
```

Updates the geodetic start position.

## `set_goal`

```python
set_goal(lat, lon, alt)
```

Updates the geodetic goal position, recalculates the Cartesian goal, and updates the simulation environment.

## `get_action`

```python
get_action(state)
```

Processes the current environment state and selects the action with the highest predicted Q-value from the trained policy network.

The state excludes `"prev_position"`, matching the training and evaluation setup.

## `interpret_action`

```python
interpret_action(action)
```

Maps a DQN action to a geodetic movement direction or altitude change.

The method returns:

```text
delta_azimuth, delta_altitude, geographic_move_flag
```

## `do_action`

```python
do_action(action)
```

Applies the selected action to the current geodetic mission position.

Horizontal movement is calculated using geodetic forward projection. Altitude movement is applied directly to the altitude component.

## `setup_mission`

```python
setup_mission()
```

Initializes the mission path format.

The generated mission path uses the QGroundControl waypoint text format beginning with:

```text
QGC WPL 110
```

## `step`

```python
step(action)
```

Applies one action to the mission path and records the resulting waypoint.

## `end_sequence`

```python
end_sequence()
```

Adds the final mission command used to end the generated mission sequence.

In the current mission output, this final command is a return-to-launch command.

## `write_to_txt`

```python
write_to_txt()
```

Writes the generated mission path to a text file.

## `write_to_csv`

```python
write_to_csv()
```

Exports selected mission waypoint fields to CSV.

The CSV output includes:

- index
- latitude
- longitude

This is useful for plotting or inspecting the generated path on a map.

## `run`

```python
path = planner.run()
```

Runs the full path-planning sequence.

The method:

1. sets up the mission format
2. initializes the environment state
3. repeatedly selects actions using the DQN policy
4. applies each action in the simulation environment
5. applies each action to the geodetic mission path
6. records the resulting waypoint
7. adds the final mission command
8. optionally writes TXT output
9. optionally writes CSV output
10. optionally renders the simulated path

The returned `path` is the generated mission path as a text string.

## Usage

### Generate a Mission Path in Python

```python
from dqn_path_planning.path_planner import PathPlanner

planner = PathPlanner()

path = planner.run()

print(path)
```

### Generate and Save a Mission TXT File

```python
from dqn_path_planning.path_planner import PathPlanner

planner = PathPlanner(
    to_txt=True,
)

path = planner.run()
```

### Generate TXT and CSV Outputs

```python
from dqn_path_planning.path_planner import PathPlanner

planner = PathPlanner(
    to_txt=True,
    to_csv=True,
)

path = planner.run()
```

### Render the Simulated Path

```python
from dqn_path_planning.path_planner import PathPlanner

planner = PathPlanner(
    render=True,
    verbosity=2,
    to_txt=True,
)

path = planner.run()
```

Rendering requires `verbosity=2` because the simulated path is written to a position log before being loaded for rendering.

## Command-Line Usage

Run the path planner from the project root:

```bash
python -m dqn_path_planning.path_planner
```

### Save a TXT Mission File

```bash
python -m dqn_path_planning.path_planner --to_txt true
```

### Save TXT and CSV Mission Files

```bash
python -m dqn_path_planning.path_planner --to_txt true --to_csv true
```

### Render the Planned Path

```bash
python -m dqn_path_planning.path_planner --render true --verbosity 2
```

### Use a Specific Model

```bash
python -m dqn_path_planning.path_planner --id final_DQN
```

### Use Custom Geodetic Start and Goal Coordinates

Coordinates are passed using underscore-separated values:

```bash
python -m dqn_path_planning.path_planner \
    --start 42.355118_-71.071305_6 \
    --goal 42.355280_-71.070911_26.5
```

### Use an Unknown Model Architecture

If the model ID is not listed in `NETWORKS`, provide the architecture manually:

```bash
python -m dqn_path_planning.path_planner \
    --id custom_model \
    --neurons 512 \
    --layers 3
```

The expected checkpoint name is:

```text
<model_id>_policy_net.pth
```

For example:

```text
final_DQN_policy_net.pth
```

## Command-Line Arguments

| Argument | Description | Default |
|---|---|---|
| `--id` | Model ID to load | `final_DQN` |
| `--model_dir` | Directory containing saved model checkpoints | `models` |
| `--neurons` | Number of hidden nodes for unknown models | `None` |
| `--layers` | Number of hidden layers for unknown models | `None` |
| `--verbosity` | Environment logging level | `0` |
| `--render` | Whether to render the generated path | `False` |
| `--to_txt` | Whether to write the generated mission path to TXT | `False` |
| `--step_length` | Simulation step size for each environment action | `10` |
| `--threshold` | Distance threshold for mission completion | `40` |
| `--delta_dist` | Geodetic/real-world movement distance per action | `0.5` |
| `--start` | Geodetic start position as `lat_lon_alt` | `42.355118_-71.071305_6` |
| `--goal` | Geodetic goal position as `lat_lon_alt` | `42.355280_-71.070911_26.5` |
| `--mission_dir` | Directory for generated mission files | `missions` |
| `--to_csv` | Whether to export waypoint latitude/longitude to CSV | `False` |
| `--cartesian_start` | Simulation start position as `x_y_z` | `5000_5000_5000` |

## Mission File Output

When `to_txt=True`, the planner writes a mission file to the mission directory.

The filename is generated using the model ID and a generation counter to avoid overwriting previous outputs.

Example:

```text
missions/final_DQN_0_mission_file.txt
```

When `to_csv=True`, a CSV file is also written with the same base name:

```text
missions/final_DQN_0_mission_file.csv
```

The TXT mission output follows the legacy QGroundControl waypoint text format, beginning with:

```text
QGC WPL 110
```

This format is commonly used in ArduPilot-compatible mission workflows and can be interpreted by ground-control tools such as QGroundControl and Mission Planner.

## ArduPilot / QGroundControl Mission Format

Each mission row after the header is a tab-separated mission item with the following structure:

```text
INDEX  CURRENT_WP  COORD_FRAME  COMMAND  PARAM1  PARAM2  PARAM3  PARAM4  PARAM5/X/LAT  PARAM6/Y/LON  PARAM7/Z/ALT  AUTOCONTINUE
```

A generated row may look like:

```text
1    0    3    82    0    0    0    0    42.355118    -71.071299    6.0    1
```

### Column Meanings

| Column | Name | Meaning |
|---:|---|---|
| 1 | `INDEX` | Mission item number |
| 2 | `CURRENT_WP` | Whether the item is marked as the current waypoint |
| 3 | `COORD_FRAME` | Coordinate frame used by the mission item |
| 4 | `COMMAND` | MAVLink command ID |
| 5 | `PARAM1` | Command-specific parameter 1 |
| 6 | `PARAM2` | Command-specific parameter 2 |
| 7 | `PARAM3` | Command-specific parameter 3 |
| 8 | `PARAM4` | Command-specific parameter 4 |
| 9 | `PARAM5 / X / LAT` | Latitude for global mission items |
| 10 | `PARAM6 / Y / LON` | Longitude for global mission items |
| 11 | `PARAM7 / Z / ALT` | Altitude for global mission items |
| 12 | `AUTOCONTINUE` | Whether to automatically continue to the next mission item |

The meaning of `PARAM1` through `PARAM7` depends on the selected MAVLink command.

For the generated waypoint commands in this project, the most important fields are:

- latitude
- longitude
- altitude
- command type
- autocontinue flag

## Commands Used

The generated mission file uses the following command IDs:

| Command ID | MAVLink command | Purpose |
|---:|---|---|
| `22` | `MAV_CMD_NAV_TAKEOFF` | Initial takeoff command |
| `82` | `MAV_CMD_NAV_SPLINE_WAYPOINT` | Main path waypoint command |
| `20` | `MAV_CMD_NAV_RETURN_TO_LAUNCH` | Final return-to-launch command |

The generated mission path generally follows this structure:

1. Start with a `QGC WPL 110` file header.
2. Add an initial takeoff command.
3. Add a sequence of spline waypoint commands generated from the DQN action sequence.
4. Add a final return-to-launch command.

## Example Mission Snippet

```text
QGC WPL 110
0    0    0    22    0    0    0    0    42.355118    -71.071305    6.0    1
1    0    3    82    0    0    0    0    42.355118    -71.071299    6.0    1
2    0    3    82    0    0    0    0    42.355118    -71.071293    6.0    1
```

In this example:

- the first line identifies the mission file format
- the first mission item is a takeoff command
- the following rows are spline waypoint commands
- latitude, longitude, and altitude are updated as the path progresses

## Limitations

The current path planner reflects an early DQN-based path-planning implementation.

Known limitations include:

- the DQN uses a discrete action space
- the policy may select similar path patterns across different start/goal pairs
- the environment is lightweight and custom-built rather than a full physics simulator
- the geodetic wrapper maps discrete simulation actions into real-world waypoint steps
- additional training would be needed for stronger generalization across arbitrary start and goal positions

These limitations are important context for interpreting the generated mission files. The current implementation demonstrates an initial reinforcement-learning-based path planner and mission-output workflow rather than a production-ready autonomous navigation stack.

## Notes

- The path planner uses the trained policy network only.
- The target network is not required for mission generation.
- The policy selects the action with the highest predicted Q-value.
- The DQN still operates in the Cartesian simulation environment.
- Geodetic mission coordinates are updated separately using `pyproj.Geod`.
- Mission TXT output is written in an ArduPilot/QGroundControl-compatible waypoint format.
- The generated path is intended as an output format for downstream drone simulation, ground-control, or robotics integration work.
- If rendering is enabled, use `verbosity=2` so the environment writes the path log needed for rendering.