Loco-nav — Victim Rescue Problem

A complete planning and control framework for an autonomous robot performing a victim rescue mission in cluttered environments. It integrates roadmap-based motion planning, combinatorial decision making, and curvature-constrained trajectory generation into a single pipeline.

Authors

eliaAvanzolini - AlexBarbi - Moska002

---

Description

This repository contains the complete codebase developed for the project of the course Robot Planning and its Applications at the University of Trento (UniTN), Academic Year 2025/2026.

The robot must navigate a partially cluttered environment under a strict travel budget, deciding which victims to rescue and in which order so as to maximize the collected reward while reaching the exit gate in time. The problem is decomposed into five well-defined layers:

1. Environment representation — map boundaries, obstacles, start/exit gates, and victim positions with associated rewards.
2. Roadmap construction — two interchangeable strategies (sampling-based PRM and combinatorial GVD).
3. Mission planning — victim selection and sequencing modeled as an Orienteering Problem and solved via MILP.
4. Trajectory generation — curvature-constrained multipoint Dubins paths through the chosen waypoints.
5. Control — trajectory tracking through the loco-nav unicycle controller.

A full description is available in the project report.

---

Approach

Roadmap generation

Two roadmap strategies are provided and can be selected at runtime.

Generalized Voronoi Diagram (GVD) — a maximum-clearance combinatorial roadmap built on the Boost Voronoi implementation. Paths stay as far as possible from obstacles, which makes the downstream Dubins curves far less likely to graze obstacle boundaries.

Roadmap	Discretized graph

Probabilistic Roadmap (PRM) — a sampling-based, multi-query roadmap. N collision-free configurations are sampled, key mission points are explicitly inserted, and each vertex is connected to its K nearest neighbors when the straight-line segment is collision-free.

Roadmap	Generated graph

Roadmap density is controllable — more sampled nodes yield better connectivity at a higher construction cost:

100 nodes	300 nodes	500 nodes	1000 nodes

Mission planning (Orienteering Problem)

Pairwise shortest-path costs between key nodes are precomputed on the roadmap with Dijkstra's algorithm, then the visitation problem is solved as an Orienteering Problem. The MILP formulation (solved with CasADi + the CBC solver) maximizes collected reward subject to a travel-budget constraint, using Miller–Tucker–Zemlin subtour elimination. A brute-force solver is also provided as a reference for small instances.

Trajectory generation (multipoint Dubins)

The ordered waypoints are converted into a dynamically feasible, curvature-constrained trajectory using a multipoint Dubins planner (fixed minimum turning radius, dynamic programming with refinement). Increasing the cost budget lets the robot rescue more victims along longer trajectories:

max_cost = 20	max_cost = 40	max_cost = 60

---

Repository Structure

RobotPlanning-Project/
├── CMakeLists.txt              # Build configuration (catkin/ROS package)
├── package.xml                 # ROS package manifest and dependencies
├── launch/
│   └── multiple_robots.launch  # Spawns Gazebo/RViz with the multi-robot scenario
├── include/robotplanning/      # Public headers
│   ├── problem.hpp             # Mission setup and Orienteering Problem (MILP/brute-force)
│   ├── roadmap.hpp             # Generic roadmap graph + Dijkstra shortest paths
│   ├── prm.hpp                 # Probabilistic Roadmap (sampling-based)
│   ├── voronoi.hpp             # Generalized Voronoi Diagram (combinatorial)
│   ├── dubins.hpp              # Two-point and multipoint Dubins planner
│   ├── collisions.hpp          # Collision checking (Clipper2-based)
│   ├── draw.hpp                # PNG rendering of environment/roadmap/graph/curves
│   └── utils.hpp               # Shared geometry types and helpers
├── src/                        # Implementations mirroring the headers
│   ├── main.cpp                # Entry point / node; parses CLI args and runs the pipeline
│   ├── problem.cpp             # Orienteering Problem solvers
│   ├── dubins.cpp              # Dubins trajectory generation
│   ├── draw.cpp                # Output rendering
│   └── roadmaps/
│       ├── roadmap.cpp         # Base roadmap graph logic
│       ├── prm.cpp             # PRM construction
│       └── voronoi.cpp         # GVD construction
├── external/
│   └── Clipper2/               # Git submodule — polygon clipping/offsetting for collisions
├── docs/
│   ├── media/                  # Figures used by this README
│   └── report/
│       └── VictimRescueProblem.pdf   # Compiled project report
└── report/                     # LaTeX sources (main.tex, sample.bib) and raw result images

The architecture is layered: roadmap.hpp defines a generic graph interface that both prm.hpp and voronoi.hpp build, so the higher-level problem.hpp (Orienteering Problem) and the dubins.hpp planner operate transparently on whichever roadmap was selected at runtime.

---

Installation

The project relies on Docker to ensure a reproducible and isolated environment.

1. Install Docker Follow the official installation guide available at:
   https://github.com/idra-lab/loco_nav/blob/master/install_docker_windows.md

2. Pull the Docker image

   docker pull eliaavanzolini/trento-lab-framework:v1.0-elia

3. Install LocoNav Follow the instructions provided in the official repository: https://github.com/mfocchi/loco_nav

4. Install CasADi Install CasADi along with the CBC solver by following the guide: https://github.com/casadi/casadi

If issues arise, it is recommended to use the precompiled tar package.

5. Clone this repository and initialize submodules

   git clone --recurse-submodules <repository_url>

Running the code

1. Start the Docker environment Access the Docker container using the alias:

   lab_planning

2. Launch the simulation Start Gazebo and RViz by running:

   roslaunch robotplanning multiple_robots.launch

3. Open a second terminal

   dock-other

4. Run the planning node

   rosrun robotplanning roadmap [prm|voronoi] [brute|milp] [max_cost] <limo_velocity>

Command Parameters

Parameter	Description
prm | voronoi	Roadmap generation method
brute | milp	Optimization strategy for victim selection
max_cost	Maximum allowable traversal cost (budget)
limo_velocity	Limo velocity during Dubins manouvers

5. Visualize generated images Open an addtional terminal and run

   eog <image_name>.png 

Generated Outputs

File name	Description
base.png	Environment with keypoints and obstacles
roadmap.png	Roadmap structure
graph.png	High-level planning graph
filtered_curve.png	Smoothed curvature-constrained trajectory

Environment (base.png)	Final trajectory (filtered_curve.png)

---

Results

Experiments compare the two roadmap strategies and the two OP solvers across varying numbers of obstacles, victims, and cost budgets (full tables in the report):

• Voronoi builds the roadmap much faster than PRM and yields robust, high-clearance paths; PRM is more flexible and adaptable given sufficient sampling density.
• The MILP solver scales far better than brute-force enumeration. Brute-force is fastest for very small instances (4 victims) but becomes infeasible at 20 victims, whereas MILP remains tractable.
• The Orienteering Problem solver is the primary bottleneck as the number of victims grows, while roadmap construction stays computationally affordable.

Victims	Voronoi BF [ms]	Voronoi MILP [ms]	PRM BF [ms]	PRM MILP [ms]
4	0.06	77	0.05	123
10	6179	4800	6045	1626
20	∞	40523	∞	101424