easynav_navmap_planner

Description

A* path planner operating over a NavMap triangular mesh.
The planner computes the minimum-cost path between the robot and the goal, taking into account both the geometric distance between triangles and the cost values stored in a selected NavMap layer (typically "inflated_obstacles").

Instead of simply avoiding non-free NavCels, the planner integrates their cost values (0–255) into the path evaluation. Cells marked as LETHAL_OBSTACLE, NO_INFORMATION or inscribed (INSCRIBED_INFLATED_OBSTACLE) are non-traversable (inscribed ones only near the start, so a robot inside that band can leave it), while inflated cells are allowed but penalized proportionally to their cost.

The search moves between NavCels that share a vertex, not only an edge, so no direction is favored by how the mesh is split; a move through a shared vertex is only taken when every NavCel around that vertex is traversable, so the path never slips between two obstacles that touch at a corner. Then the path is shortened by line of sight: from each waypoint it goes straight to the farthest one whose segment crosses only traversable NavCels no costlier than the waypoints it replaces, and the result is resampled at the NavCel spacing on the surface. The path ends exactly at the goal.

This enables smoother and safer trajectories that still respect proximity constraints imposed by obstacle inflation.

Cost model

For two neighboring NavCels u and v, the edge cost is computed as:

[ \text{cost}(u,v) = d(u,v) \times \left(\text{cost_factor} + \text{inflation_penalty} \times \frac{c(v)}{253}\right) ]

where d(u,v) is the Euclidean distance between triangle centroids,
and c(v) is the cost value of cell v.
This formulation ensures that:

• cells near obstacles (high cost) are traversed only if geometrically necessary,
• lethal (254) and unknown (255) cells are not traversable.

Authors and Maintainers

• Authors: Intelligent Robotics Lab
• Maintainers: Francisco Martín Rico fmrico@gmail.com

Supported ROS 2 Distributions

Distribution	Status
humble
jazzy
kilted
lyrical
rolling

Plugin (pluginlib)

• Plugin Name: easynav_navmap_planner/AStarPlanner
• Type: easynav::navmap::AStarPlanner
• Base Class: easynav::PlannerMethodBase
• Library: easynav_navmap_planner
• Description: A* path planner over a NavMap triangular mesh using per-cell costs to compute the shortest safe path.

Parameters

All parameters are declared under the plugin namespace, i.e.
/<node_fqn>/easynav_navmap_planner/AStarPlanner/...

The terrain limits use the NavMap descriptors (navmap::compute_descriptors), computed once per map geometry: a few tens of ms on maps of tens of thousands of NavCels (more if the map is rebuilt continuously, as in the maps manager’s local mode). The line-of-sight shortcut never replaces part of the path by a steeper or costlier straight segment, and never cuts across a link.

Note: The planner internally uses hardcoded values for layer_name (prefers "inflated_obstacles", fallback to "obstacles"), cost_axial, and cost_diagonal. These are not runtime-configurable parameters.

Interfaces (Topics and Services)

Publications

| Direction | Topic | Type | Purpose | QoS | |—|—|—|—|—| | Publisher | <node_fqn>/<plugin>/path | nav_msgs/msg/Path | Publishes the computed A* path. | depth=10 |

This plugin does not create subscriptions or services directly; it retrieves all inputs from NavState.

NavState Keys

| Key | Type | Access | Description | |—|—|—|—| | goals | nav_msgs::msg::Goals | Read | Target pose(s) for path planning. | | robot_pose | nav_msgs::msg::Odometry | Read | Current robot pose (start position). | | map.navmap | ::navmap::NavMap | Read | NavMap containing geometry and cost layer. The planner reads costs from the layer specified in <plugin>.layer (default: "inflated_obstacles"). If that layer does not exist, it automatically falls back to "obstacles". | | path | nav_msgs::msg::Path | Write | Output path, computed as the lowest-cost route. |

TF Frames

The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally.

License

Apache-2.0

Changelog for package easynav_navmap_planner

0.5.1 (2026-10-21)

• A*: terrain costs and limits, and links between surfaces
• Enable cpplint and copyright linters
• READMEs: supported on Humble, Jazzy, Kilted, Lyrical and Rolling
• Contributors: Francisco Martín Rico

0.5.0 (2026-10-08)

• Runtime reconfiguration
• Planners never keep a stale path
• The path ends at the goal, inscribed cells are not crossed, and cost_weight balances cost against length
• A* over NavCels that share a vertex (no direction favored by the mesh) and line-of-sight shortcut of the path
• Builds on Humble, Jazzy, Kilted, Lyrical and Rolling
• Contributors: Francisco Martín Rico

0.4.2 (2026-07-26)

• Complete deps
• GPLv3 -> Apache 2.0
• Improve navstate print including the time
• Remove C++20/C++23 features and update to new MethodBase interface
• Merge branch 'set_robot_frame' into frames-fix-pr-40
• TFInfo in RTTFBuffer
• Refactor to use TFInfo
• Optimize execution
• Cleanup unused headers
• Update sheets
• Path planner aware of obstacles and inflation
• A Star uses obstacle layer
• Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely, Juan S. Cely G., Miguel, migueldm

0.0.2 (2025-10-12)

• Reorganization initial
• Contributors: Francisco Martín Rico