Metadata-Version: 2.5
Name: guadalplanner
Version: 0.0.1
Summary: GuadalPlanner is a modular and extensible framework for the development, simulation, and real-world deployment of informative path planning algorithms for autonomous vehicles
Project-URL: Homepage, https://gitlab.ratatosk.cc/syanes/guadalplanner
Project-URL: Issues, https://gitlab.ratatosk.cc/syanes/guadalplanner/issues
Author-email: Samuel Yanes Luis <syanes@us.es>, Alejandro Mendoza Barrionuevo <amendoza1@us.es>, Alejandro Casado Perez <acasado4@us.es>
License-Expression: MIT
License-File: LICENSE
Classifier: Development Status :: 3 - Alpha
Classifier: Intended Audience :: Education
Classifier: Intended Audience :: Science/Research
Classifier: Operating System :: OS Independent
Classifier: Programming Language :: Python :: 3
Requires-Python: >=3.9
Requires-Dist: matplotlib
Requires-Dist: networkx
Requires-Dist: numpy
Requires-Dist: paho-mqtt<2
Requires-Dist: scipy
Requires-Dist: sylegendarium
Requires-Dist: tqdm
Provides-Extra: all
Requires-Dist: anytree; extra == 'all'
Requires-Dist: colorcet; extra == 'all'
Requires-Dist: deap; extra == 'all'
Requires-Dist: gpytorch; extra == 'all'
Requires-Dist: oilspillsim; extra == 'all'
Requires-Dist: opencv-python; extra == 'all'
Requires-Dist: pillow; extra == 'all'
Requires-Dist: scikit-learn; extra == 'all'
Requires-Dist: torch; extra == 'all'
Provides-Extra: genetic
Requires-Dist: deap; extra == 'genetic'
Provides-Extra: gp
Requires-Dist: gpytorch; extra == 'gp'
Requires-Dist: scikit-learn; extra == 'gp'
Requires-Dist: torch; extra == 'gp'
Provides-Extra: maps
Requires-Dist: opencv-python; extra == 'maps'
Requires-Dist: pillow; extra == 'maps'
Provides-Extra: mcts
Requires-Dist: anytree; extra == 'mcts'
Provides-Extra: oilspill
Requires-Dist: colorcet; extra == 'oilspill'
Requires-Dist: oilspillsim; extra == 'oilspill'
Provides-Extra: trash
Requires-Dist: colorcet; extra == 'trash'
Description-Content-Type: text/markdown

<div align="center">
  <p>
    <a href="https://gitlab.ratatosk.cc/syanes/guadalplanner" target="_blank">
      <img width="100%" src="https://gitlab.ratatosk.cc/syanes/guadalplanner/-/wikis/uploads/1f78c28d0abb49c343e63ef82c5a9d00/GuadalPlannerScheme.jpg" alt="GuadalPlanner Banner"></a>
  </p>
</div>

## :clipboard: **_GuadalPlanner_**: Environmental Monitoring System with Autonomous Vehicles

**_GuadalPlanner_** is a modular and extensible framework for the development, simulation, and real-world deployment of **informative path planning algorithms** for autonomous vehicles, built on the integration of **MAVLink**, **ROS2**, and **MQTT** for communications. The system addresses the gap between virtual algorithm testing and physical execution by maintaining a consistent software architecture across three levels of abstraction: abstract simulation, **ArduPilot SITL** simulation, and real deployment.

One of the key strengths of **_GuadalPlanner_** lies in its clear **separation between high-level algorithmic logic and low-level control interfaces**. This allows researchers to develop and evaluate decision algorithms isolated from hardware constraints, while ensuring smooth transfer to SITL or physical vehicles without code modifications. It is intended to be a contribution to the open robotics community, and to serve as a basis on which others can extend functionality and contribute new modules, thus encouraging **collaboration**.


<div align="center">
    <a title="Communications Schema">
      <img src="https://gitlab.ratatosk.cc/syanes/guadalplanner/-/wikis/uploads/ff4ba89956da6c503fd4b9ce74f60b2b/Comms_schema.jpg" width="40%" />
    </a>
    <img src="https://gitlab.ratatosk.cc/syanes/guadalplanner/-/wikis/uploads/3d6bd97d601a98ab491ed8dd255b9671/logo-transparent.png" width="10%" alt="" />
    <a title="ROS2 Schema">
      <img src="https://gitlab.ratatosk.cc/syanes/guadalplanner/-/wikis/uploads/0c3b79213491fc17ef05cb27f5d36bdd/ROS2_schema.jpg" width="40%" />
    </a>
    <p><em>Figure 1: System architecture overview (left: communication schema, right: ROS2 integration).</em></p>
</div>


#### Framework Objectives

1. Ensure interchangeability between algorithms and environments, allowing different informative path planning algorithms to be tested on different scenarios (environments) without the need to rewrite the code base, thanks to a flexible and modular structure that decouples the decision logic from the model of the environment.
2. Unify simulated and real environments for real-time environmental data collection, allowing the same code to be executed in both contexts with minimal modifications.
3. Abstract decision-making from the control system.
4. Enable customisation and extensibility, offering an object-oriented structure with easily inheritable and adaptable classes.
5. Facilitate the modular integration of sensors, navigation algorithms and communication nodes through an architecture based on ROS2.


## :rocket: Quick Start Guide


<details open>
<summary>Installation</summary>

```bash
# Clone repository
git clone --recurse-submodules -j8 https://gitlab.ratatosk.cc/syanes/guadalplanner.git

# Or update as
git pull --recurse-submodules

# Install dependencies
cd guadalplanner
pip install numpy networkx matplotlib paho-mqtt scipy scikit-learn sylegendarium
```

</details>


<details close>
<summary>Abstract Simulation</summary>

```python
import numpy as np
from Environment.FleetUtils import BaseGraphEnvironment, Fleet
from Environment.GroundTruths import generate_gts

# Load map
base_matrix = np.load('Environment/maps/Alamillo30x49mask.npy')
coordinates = np.load('Environment/maps/Alamillo30x49latlon.npy')

# Create environment
env = BaseGraphEnvironment(
    base_matrix=base_matrix,
    resolution=1.0,
    diagonal=True,
    lat_lon_map=coordinates
)

# Generate environmental data
ground_truth = generate_gts(base_matrix, env.xy_positions, n_objectives=5)
env.fill_graph_attribute('ground_truth', ground_truth.T)

# Create fleet
fleet = Fleet(
    n_vehicles=2,
    initial_positions=[0, 50],
    graphEnv=env,
    max_distance=100.0
)

# Execute mission
fleet.reset()
for step in range(100):
    # Plan next objectives
    targets = [np.random.randint(0, len(env.xy_positions)) for _ in range(2)]

    # Move vehicles
    reached, dones = fleet.move(targets)

    # Take measurements
    if all(reached.values()):
        measurements = fleet.take_measurement()
        print(f"Measurements: {measurements}")

    # Visualize
    fleet.render()

    # Check stopping conditions
    if any(dones.values()):
        print("Mission completed")
        break
```
</details>

<details close>
<summary>Real Deployment/Ardupilot Software-in-the-loop</summary>
This example shows how to use the RemoteFleet class to simulate a fleet of vehicles with Ardupilot SITL simulation or for a deployment with real vehicles.
The RemoteFleet class allows to communicate with a fleet of vehicles that can communicate remotely.

* In simulation, Ardupilot SITL must be running to simulate the vehicles and the MQTT broker must be configured to communicate with them. MAVROS and ROS2 program properly configured must also be running. To avoid installation issues, it is recommended to build with necessary modifications the Docker image provided in the repository.
* In real deployment, vehicles need an autopilot compatible with MAVLink, such as Ardupilot or PX4.
The companion computer must run MAVROS and the ROS2 program properly configured to communicate with the vehicles, receive commands and send measurements. MQTT broker must be configured to communicate with the vehicles. To avoid installation issues, it is recommended to build with necessary modifications the Docker image provided in the repository and run it directly in the companion computer.

This example assumes that the Docker image or the necessary environment is already set up and running.

```python
from Environment.FleetUtils import RemoteFleet

# Configure communication
mqtt_params = {
    'broker_ip': 'your-mqtt-broker.com',
    'port': 1883,
    'username': 'your-username',
    'password': 'your-password'
}

# Define sensors
sensors = {
    0: ['temperature_ct', 'turbidity', 'conductivity', 'ph', 'depth'],
    1: ['temperature_ct', 'turbidity', 'conductivity', 'ph', 'depth']
}

# Create real fleet
fleet = RemoteFleet(
    n_vehicles=2,
    initial_positions=[0, 50],
    graphEnv=env,
    mqtt_comm_params=mqtt_params,
    objectives_names=sensors,
    use_sim_measurements=False
)

# Execute real mission
fleet.reset()  # Vehicles move to initial positions
# ... rest of code similar to simulation
```

## :dart: Specific Use Example

### 1. **Water Quality Monitoring**

```python
from Environment.GaussianProcessEnv import GaussianProcessEnv
from Agents.ExpectedImprovementAgent import ExpectedImprovementAgent

# Configure environment with Gaussian processes
env = GaussianProcessEnv(base_matrix, n_objectives=5)

# Using expected improvement agent
agent = ExpectedImprovementAgent(env)

# Running adaptive mission
for step in range(200):
    # The agent decides where to sample based on uncertainty
    next_targets = agent.plan_next_actions(fleet.get_positions())
    fleet.move(next_targets)

    # Updating model with new measurements
    measurements = fleet.take_measurement()
    agent.update_model(measurements)
```
</details>




## :construction_site: Project Architecture

The GitLab repository is structured in two parts:

- The main repository contains the GuadalPlanner code, structured in the following main folders:
  * Agents: contains examples of algorithms.
  * Environment: contains all the base code of the framework (FleetUtils, graphEnv, maps, and example environments, among others).
  * Examples: contains a series of basic examples organized from simplest to most advanced, helping users familiarize themselves with the framework's structure.
  * Experiments: contains the code for the experiments carried out in Alamillo Park, as well as example metrics saved with Legendarium. To visualize them, there is an example in Environment/utils/load_experiment.py.

```
guadalplanner/
├── README.md                           # This file
├── representa.py                       # Main representation script
│
├── Agents/                            # Path planning algorithms
│   ├── ExpectedImprovementAgent.py    # Expected improvement based agent
│   ├── GeneticAlgorithmSequential.py  # Sequential genetic algorithm
│   ├── MonteCarloTreeSearch.py        # Monte Carlo tree search
│   ├── MyopicGreedy.py                # Myopic greedy algorithm
│   └── WanderingAgent.py              # Random exploration agent
│
├── Docker/                            # Folder for docker files
│   ├── compose.yaml                   # Docker compose
│   └── .env                           # Environment variables
│
├── Environment/                       # Simulation environment
│   ├── FleetUtils.py                  # Fleet and vehicle system
│   ├── graphEnvs.py                   # Graph-based environments for monitorization
│   ├── GroundTruths.py                # Synthetic data generation or loading
│   ├── FakeVehicles.py                # MQTT response simulator
│   ├── GaussianProcessEnv.py          # Gaussian process environment
│   ├── OilSpillEnv.py                 # Oil spill simulation
│   ├── TrashCleaningEnv.py            # Trash cleaning
│   ├── OptimalSensorPlacementEnv.py   # Optimal sensor placement
│   └── generate_map.py                # Map generation
│
├── Examples/                          # Usage examples
│   ├── 1_ExampleAbstractSimulation.py # Abstract simulation
│   ├── 2_ExampleSITLSimulation.py     # SITL simulation
│   ├── 3_ExampleRealDeployment.py     # Real deployment
│   └── 4_ExampleGaussianProcessEnv    # Gaussian process example
│
├── Experiments/                       # Experimental results
│   └── *.meta.yaml, *.metrics.xz     # Metadata and metrics
│
└── Documentation/                     # Additional documentation
    └── Figures/                       # Figures and images
```

- In addition to the main repository, the [*asv*](https://gitlab.ratatosk.cc/syanes/asv) repository is included as a submodule, necessary for real-world implementation or simulation with Ardupilot SITL. That is, for using GuadalPlanner's Remote classes. This repository contains the ROS2 program, which acts as middleware between GuadalPlanner and the vehicle's autopilot running MAVLink. Although this submodule is designed for autonomous surface vehicles, it can be adapted to other types of vehicles. Through [nodes](https://gitlab.ratatosk.cc/syanes/asv/-/tree/main/asv_workspace/src/asv_loyola_us/asv_loyola_us?ref_type=heads), ROS2 handles low-level path planning, communication with the central server via the MQTT protocol, and reading/sending sensor measurements, among other tasks. For users to run this code, they must modify it according to their needs, especially the [configuration file](https://gitlab.ratatosk.cc/syanes/asv/-/blob/main/asv_workspace/src/asv_loyola_us/config/config.yaml?ref_type=heads). To facilitate usage, Docker images have been implemented and can be executed using a [bash script](https://gitlab.ratatosk.cc/syanes/asv/-/blob/main/startasv.sh?ref_type=heads), though variables must be adjusted for each user's specific setup. The Dockerfiles are available for editing [here](https://gitlab.ratatosk.cc/syanes/asv/-/tree/main/dockerfiles?ref_type=heads), as well as for running them on systems not compatible with the bash script.

## :tools: Main Modules

### 1. `Environment/` - :globe_with_meridians: Vehicles and simulation environments

#### **FleetUtils.py**

FleetUtils is a Python module that implements a complete system for **environmental monitoring using fleets of autonomous vehicles**. It is designed to coordinate multiple vehicles (simulated or real) that can navigate an environment, take environmental measurements and report data in real time.

**Main components:**

- `BaseGraphEnvironment`: Converts an array of pixels (representing a map) into a navigable graph where each valid pixel becomes a node. Each node has stored information, such as ground truth values or latitude and longitude coordinates.
- `Vehicle`: Simulated vehicle with autonomous navigation. Autonomous graph navigation, battery system/maximum distance, taking simulated measurements, statuses (position, current node, distance travelled), etc.
- `RemoteVehicle`: Real vehicle controlled via MQTT. In addition to the features inherited from the non-remote class: two-way MQTT communication, sending GPS waypoints, receiving arrival confirmations, taking actual measurements from sensors.
- `Fleet`: Coordination of multiple simulated vehicles. Collision avoidance, movement synchronisation, real-time visualisation, fleet status management, etc.
- `RemoteFleet`: Coordination of multiple real vehicles. Inherits the functions of the non-remote class.
- `MQTTRemoteCommNode`: Bidirectional MQTT communication

#### **graphEnvs.py**

This module provides graph-based environments specifically designed for environmental monitoring missions with autonomous vehicles. It extends the base graph functionality with specialized monitoring capabilities and fleet coordination.

**Main components:**

- `MonitorizationEnvironment`: is the core of the environmental monitoring system that coordinates multiple autonomous vehicles in environmental data collection missions. This class integrates previous components, such as the transformation of binary navigation matrices into navigable graphs, the management of vehicle fleets with position and movement control, and the simulated environmental data (ground truth) for variables, and provides real-time visualization of mission status.
- `RemoteMonitorizationEnvironment`: Extended environment for real vehicle deployments via MQTT. It offers real-time vehicle information exchange and simulated or real sensor measurements.

#### **Specialized Environments**

**GaussianProcessEnv.py**

- Environmental phenomena modeling with Gaussian processes
- Value prediction in unsampled locations
- Uncertainty-based route optimization

**OilSpillEnv.py**

- Oil spill simulation
- Temporal dispersion modeling
- Containment and cleanup strategies

**TrashCleaningEnv.py**

- Environment for aquatic trash cleanup
- Waste detection and collection
- Cleanup path optimization

**OptimalSensorPlacementEnv.py**

- Optimal environmental sensor placement
- Information coverage maximization
- Spatial optimization algorithms

### 2. `Agents/` - :robot: Informative path planning algorithms

**ExpectedImprovementAgent.py**

- Bayesian optimization for exploration
- Exploration-exploitation balance
- Expected improvement as acquisition function

**GeneticAlgorithmSequential.py**

- Genetic algorithm for route planning
- Solution population evolution
- Multi-objective optimization

**MonteCarloTreeSearch.py**

- Monte Carlo tree search in decision trees
- Stochastic simulations for evaluation
- Long-term planning

**MyopicGreedy.py**

- Short-horizon greedy algorithm
- Locally optimal decisions
- Fast real-time response

**WanderingAgent.py**

- Random environment exploration
- Baseline for algorithm comparison
- Uniform space coverage

### 3. `Examples/` - :books: Progressive Tutorials With Usage Examples

[**1_ExampleAbstractSimulation.py**](https://gitlab.ratatosk.cc/syanes/guadalplanner/-/blob/main/Examples/1_ExampleAbstractSimulation.py?ref_type=heads)

```python
# Basic simulation without hardware
from Environment.FleetUtils import Fleet, BaseGraphEnvironment

# Configure environment
env = BaseGraphEnvironment(base_matrix, resolution=1.0)
fleet = Fleet(n_vehicles=2, initial_positions=[0, 10], graphEnv=env)

# Execute mission
fleet.reset()
fleet.move([target1, target2])
measurements = fleet.take_measurement()
```

[**2_ExampleSITLSimulation.py**](https://gitlab.ratatosk.cc/syanes/guadalplanner/-/blob/main/Examples/2_ExampleSITLSimulation.py?ref_type=heads)

```python
# Simulation with MQTT protocol (Software-in-the-Loop)
from Environment.FleetUtils import RemoteFleet
from Environment.FakeVehicles import MQTTFakeVehiclesResponses

# Configure simulated communication
mqtt_params = {'broker_ip': 'localhost', 'port': 1883}
fake_vehicles = MQTTFakeVehiclesResponses(**mqtt_params, n_vehicles=2)

# Create remote fleet
fleet = RemoteFleet(n_vehicles=2, mqtt_comm_params=mqtt_params, ...)
```

[**3_ExampleRealDeployment.py**](https://gitlab.ratatosk.cc/syanes/guadalplanner/-/blob/main/Examples/3_ExampleRealDeployment.py?ref_type=heads)

```python
# Real deployment with hardware
mqtt_params = {'broker_ip': 'field-station.local', 'port': 1883}
fleet = RemoteFleet(
    n_vehicles=3,
    mqtt_comm_params=mqtt_params,
    use_sim_measurements=False  # Use real sensors
)
```

[**4_ExampleGaussianProcessEnv.py**](https://gitlab.ratatosk.cc/syanes/guadalplanner/-/blob/main/Examples/4_ExampleGaussianProcessEnv.py?ref_type=heads)

```python
# Gaussian Process-based environmental monitoring
from Environment.GaussianProcessEnv import GPbasedEnvironment
from Agents.MyopicGreedy import UncertaintyGreedyAgent

# Configure GP environment
env = GPbasedEnvironment(
    base_matrix=mask_grid,
    max_distance=925,
    n_objectives=1,
    initial_positions=initial_positions,
    lengthscale_bounds=(1.0, 8.0),
    ground_truth_fn=generate_gts
)

# Use uncertainty-based agent
agent = UncertaintyGreedyAgent()
target = agent.policy(env, current_node, radius=5)
reward, reached, dones, info = env.move(target, "sequential")
```

### 4. `Experiments/` - Experimental Results

Contains results from experiments conducted with different configurations:

**File structure:**

- `*.meta.yaml`: Experiment metadata (configuration, parameters)
- `*.metrics.xz`: Compressed metrics (performance, trajectories)

**Example experiments:**

- `Alamillo_20250424_*`: Experiments in Alamillo area
- `Alamillo30x49_*`: Experiments on 30x49 grid
- `AlamilloAccess11x15_*`: Experiments in access area

## :whale: Docker compose
This Docker Compose setup provides a ready-to-run execution environment for GuadalPlanner at the Software-in-the-Loop abstraction level. It launches all the required services to simulate an autonomous vehicle, handle middleware communication, and exchange planning commands using MQTT, without requiring manual configuration of each component.

The goal of this setup is to offer a single entry point that encapsulates all runtime dependencies, allowing users to focus on developing and testing IPP algorithms rather than on system integration.

### Services Description

All services are connected through a shared Docker network to ensure consistent internal communication.
The Docker Compose configuration instantiates the following components:

##### `sitl_internal/` - Vehicle SITL 
A simulated ArduPilot-based vehicle running in SITL mode, initialized at a configurable geographic location. The initial vehicle position is defined through environment variables and can be easily modified in [**.env**](https://gitlab.ratatosk.cc/syanes/guadalplanner/-/blob/main/Docker/.env?ref_type=heads) file.
Relevant parameters:
- `VEH_LAT`, `VEH_LON`: Initial latitude and longitude of the vehicle.
- `SITLPORT`: MAVLink port exposed to the host.

#####  `asv_runner_internal/` - Vehicle middleware container
Executes the onboard software stack, including navigation and communication nodes, and connects to the SITL instance using MAVLink, and to the MQTT broker.
Relevant parameters:

- `ASV_ID`: Identifier of the vehicle.
- `GUIDED_WAYPOINTS`: Enables waypoint-based navigation in GUIDED mode. It is recommended not to change.
- `MQTT_ADDR`: Address of the MQTT broker (local by default).

##### `fake_start/` - Automatic arming and mode initialization
This auxiliary container is responsible for initializing the vehicle state once all required services are running. It automatically arms the vehicle and switches it to GUIDED mode, making it ready to receive waypoints or planning commands.

##### `mqtt_broker_internal/` - MQTT broker
A local MQTT broker used for communication between GuadalPlanner components and external planning algorithms. By default, this broker is used for all communications, but it can be replaced by an external broker. To use an external broker, uncomment and set `MQTT_ADDR` in [**.env**](https://gitlab.ratatosk.cc/syanes/guadalplanner/-/blob/main/Docker/.env?ref_type=heads).

### Usage
To launch the complete SITL execution environment:

```bash
cd Docker
docker compose up
```

Once all containers are running, the system is ready to receive commands from any IPP algorithm implemented using GuadalPlanner. Communication between GuadalPlanner and the execution environment relies on MQTT. The only additional step required is for both sides to be connected to the same common MQTT broker. If no remote MQTT broker has been set, default is _localhost_, as seen in [**example tutorials**](https://gitlab.ratatosk.cc/syanes/guadalplanner#3-examples---books-progressive-tutorials-with-usage-examples).

To stop the system:

```bash
docker compose down
```

## :rotating_light: Troubleshooting

### **Common Problems**

1. **MQTT connection failed**

```python
# Verify connectivity
import paho.mqtt.client as mqtt

def test_mqtt_connection(broker_ip, port):
    client = mqtt.Client()
    try:
        client.connect(broker_ip, port, 60)
        print("✅ Successful MQTT connection")
        return True
    except Exception as e:
        print(f"❌ Error in connection: {e}")
        return False
```

## :handshake: Contributions

### **How to Contribute**

1. **Fork** the repository
2. **Create** branch for new functionality
3. **Implement** changes with tests
4. **Document** changes
5. **Send** pull request