mrpt_path_planning

Path planning and navigation algorithms for robots/vehicles moving on planar environments. This library builds upon mrpt-nav and the theory behind PTGs to generate libraries of “motion primitives” for vehicles with arbitrary shape and realistic kinematics and dynamics.

The planner optimizes SE(2) path cost (position + heading), not R(2) path length. For vehicles that rotate, arriving at a goal with the correct heading is part of the optimal solution — paths that are longer in Euclidean distance but better-aligned may genuinely have lower cost. See TPS_Astar.h for details on the cost model.

Status on ROS build farm

Distro	Build dev	Build releases	Stable version
ROS 2 Humble (u22.04)
ROS 2 Jazzy @ u24.04
ROS 2 Kilted @ u24.04
ROS 2 Lyrical (u26.04)
ROS 2 Rolling (u26.04)

EOL Distro	Last version
ROS 1 Noetic (u20.04)
ROS 2 Iron (u22.04)

Build requisites

• MRPT (>=2.12.0)
• mvsim (optional to run the live control simulator).

In Ubuntu 22.04 or newer, installed it with:

sudo apt install libmrpt-dev

For older versions of Ubuntu:

# MRPT from this PPA (or build from sources if preferred, or from ROS package `mrpt2`):
sudo add-apt-repository ppa:joseluisblancoc/mrpt
sudo apt update
sudo apt install libmrpt-dev

Use in your code

From your CMake script:

find_package(mrpt_path_planning REQUIRED)
target_link_libraries(YOUR_TARGET mpp::mrpt_path_planning)

Demo runs

path-planner-cli

Dump default planner parameters to a YAML file for inspection or customization:

build/bin/path-planner-cli --write-planner-parameters my-planner-params.yaml

Plan a path for a holonomic robot with an SE(2) goal pose (x y heading_deg), using a pre-built obstacle point cloud and an obstacle-proximity cost map:

build/bin/path-planner-cli \
  -s "[0.5 0 0]" \
  -g "[4 2.5 45]" \
  -c share/ptgs_holonomic_robot.ini \
  --obstacles share/obstacles_01.txt \
  --planner-parameters share/mvsim-demo-astar-planner-params.yaml \
  --costmap-obstacles share/costmap-obstacles.yaml

Plan a path with an R(2) goal (position only, heading-agnostic), printing the edge details of the found path and saving the interpolated trajectory to a CSV:

build/bin/path-planner-cli \
  -s "[0.5 0 0]" \
  -g "[4 2.5]" \
  -c share/ptgs_holonomic_robot.ini \
  --obstacles share/obstacles_01.txt \
  --planner-parameters share/mvsim-demo-astar-planner-params.yaml \
  --print-path-edges \
  --save-interpolated-path path.csv

Plan a path for an Ackermann (car-like) vehicle, show the full explored search tree, and animate the result. Note the goal is given as a position [x y] (R², heading-agnostic): with arc-based PTGs, arriving at a precise heading AND position simultaneously is very constrained, so position-only goals are the natural choice

File truncated at 100 lines see the full file

Changelog for package mrpt_path_planning

1.0.0 (2026-06-16)

• feat(TPS_Astar): weighted A* via heuristic_epsilon param (default 1.0 = exact A*); eps in [1.5, 2] cuts median plan time ~4-5x at ~3-8% longer paths
• feat(TPS_Astar): analytic expansion / early termination to goal (#25): accept a direct-to-goal PTG edge as soon as it lands in the goal cell, skipping unnecessary expansions
• feat(TPS_Astar): optional 2D-Dijkstra obstacle-aware heuristic (#24): opt-in use_obstacle_heuristic pre-computes a cost-to-go grid; ~5x fewer expansions on hard BARN worlds
• feat(TPS_Astar): wire stateGoal.vel into goal-cell exit speed (#18)
• feat(viz): SVG exporter for plans (#29): mpp::save_plan_to_svg() — dependency-free 2D vector export of motion tree, solution path, footprint, and start/goal markers; new --save-svg CLI option
• feat(gui): viz now shows selected point coordinates
• perf(TPS_Astar): batch TP-obstacle collision check per node expansion (#26): O(candidates×N) → O(N); ~2-3x faster on hard BARN worlds
• perf(TPS_Astar): defer per-edge path interpolation to the final solution (#27): ~17-21% additional speedup
• perf(TPS_Astar): coarsen default grid_resolution_yaw from 5° to 7.5°; ~23-37% faster with equal or better success rate on BARN/HouseExpo benchmarks
• perf(P3): xy-cell obstacle cache in cached_local_obstacles() (#20): ~6x per-call speedup; cache keyed by (ix,iy) and cleared each plan()
• fix(TPS_Astar): obstacle cache ignored heading, causing collision false negatives (#28): cache now stores heading-independent clipped cloud; heading-dependent transform done per call
• fix: refine_trajectory() distance overshoot
• fix: SVG draw now honors any-heading goals
• fix: restore A* admissibility by converting heuristic cost to seconds (#16)
• fix: store ptgTrimmableSpeed in best-path replacement block (#15)
• fix: replace NodeCoordsHash with boost::hash_combine avalanche pattern to eliminate bucket clustering in large lattices
• fix: bind obs_xs/obs_ys by const ref in transform_pc_square_clipping to avoid O(N) copy (#14)
• fix: remove dead THROW_EXCEPTION branches in edge_interpolated_path (#17)
• test: end-to-end A* tests for C-PTG forward and reverse motion (#23)
• test: add UnreachableGoal, Timeout, MultiPTG, MotionTree, and obstacle-cache unit tests (#22)
• build: bump minimum CMake version to 3.22 (#19)
• ci: install libgtest-dev so GTest suite is actually built and run
• docs: add ROS 2 Lyrical badge, update Rolling to Ubuntu 26.04 (Resolute)
• Contributors: Jose Luis Blanco-Claraco, SRai22

0.3.0 (2026-04-02)

• copyright year bump
• Integrate vscode config to build with colcon
• Add basic unit tests
• add params for ackermann
• Fix potential wrong lattice coordinates due to bad rounding
• Fix: use smooth quadratic cost
• Fix potential phi angle comparison wraps
• Docs: explain optimality criterion
• tune params and add more examples in readme
• fix potential non-optimal path search
• fix build against modern mvsim
• Add ARQUITECTURE.md
• Contributors: Jose Luis Blanco-Claraco

0.2.5 (2026-01-21)

• fix build against newer mvsim API
• Contributors: Jose Luis Blanco-Claraco

0.2.4 (2025-12-19)

• Fix build for mrpt >=2.15.3
• Contributors: Jose Luis Blanco-Claraco

0.2.3 (2025-11-10)

• Fix usage of new mrpt 2.15.0 API
• Contributors: Jose Luis Blanco-Claraco

File truncated at 100 lines see the full file