AStarPlanner Class Reference

A planner implementing the A* algorithm on a ::navmap::NavMap grid. More...

#include <AStarPlanner.hpp>

Inheritance diagram for AStarPlanner:

Collaboration diagram for AStarPlanner:

Public Member Functions
	AStarPlanner ()
	Default constructor.
virtual void	on_initialize () override
	Initializes the planner.
void	update (NavState &nav_state) override
	Executes a planning cycle using the current navigation state.

Protected Member Functions
std::vector< geometry_msgs::msg::Pose >	a_star_path (const ::navmap::NavMap &map, const geometry_msgs::msg::Pose &start, const geometry_msgs::msg::Pose &goal)
	Internal A* path planning routine.
void	ensure_graph_cache (const ::navmap::NavMap &map)
	Ensure internal caches (centroids and A* buffers) are sized for the given map.
nav_msgs::msg::Path	path_smoother (const nav_msgs::msg::Path &in_path, const ::navmap::NavMap &navmap, int iterations=5, float alpha=0.4f, float corner_keep_deg=0.0f)
	Smooth a Path in XY while keeping every waypoint inside its original NavCel.

Protected Attributes
std::vector< Eigen::Vector3f >	centroids_
	Cached centroids for each NavCel (same indexing as ::navmap::NavMap::navcels).
bool	continuous_replan_ {true}
	Whether to replan the path at control frequency.
double	cost_axial_
	Cost multiplier for axial (horizontal/vertical) moves.
double	cost_diagonal_
	Cost multiplier for diagonal moves.
double	cost_factor_
	Scaling factor applied to cell cost values.
geometry_msgs::msg::Pose	current_goal_
	Current goal.
nav_msgs::msg::Path	current_path_
	Most recently computed path.
std::vector< double >	g_
	Reusable buffers for A* search cost and parent links.
double	inflation_penalty_
	Extra cost penalty for paths near inflated obstacles.
std::string	layer_name_
std::vector< std::uint8_t >	occ_
	Cached per-NavCel occupancy / cost values (0..255).
std::vector<::navmap::NavCelId >	parent_
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr	path_pub_
	Publisher for the computed navigation path (for visualization or monitoring).

Detailed Description

A planner implementing the A* algorithm on a ::navmap::NavMap grid.

This class generates a collision-free path using A* search over a surface-based NavMap. It supports cost-based penalties and anisotropic movement costs.

Constructor & Destructor Documentation

AStarPlanner()

AStarPlanner ( )

explicit

Default constructor.

Initializes internal parameters and configuration values.

Member Function Documentation

a_star_path()

std::vector< geometry_msgs::msg::Pose > a_star_path ( const ::navmap::NavMap & map, const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal )

protected

Internal A* path planning routine.

Computes a path on the given NavMap from the start pose to the goal pose.

Movement cost is influenced by:

• The cost of each NavCel (retrieved from a layer).
• Additional inflation penalties near obstacles.

Parameters

map	The NavMap to plan over.
start	The robot's starting pose in world coordinates.
goal	The goal pose in world coordinates.

Returns

A vector of poses representing the planned path.

ensure_graph_cache()

void ensure_graph_cache ( const ::navmap::NavMap & map )

protected

Ensure internal caches (centroids and A* buffers) are sized for the given map.

This avoids reallocations on every planning call. Values in g_ and parent_ are reset for the current run.

Parameters

map	The NavMap for which caches must be valid.

on_initialize()

void on_initialize ( )

overridevirtual

Initializes the planner.

Loads planner parameters, sets up ROS publishers, and prepares the NavMap-based planning environment.

Exceptions

std::runtime_error

if initialization fails.

path_smoother()

nav_msgs::msg::Path path_smoother ( const nav_msgs::msg::Path & in_path, const ::navmap::NavMap & navmap, int iterations = 5, float alpha = 0.4f, float corner_keep_deg = 0.0f )

protected

Smooth a Path in XY while keeping every waypoint inside its original NavCel.

The algorithm performs several iterations of Laplacian smoothing on XY: p_i' = (1 - alpha) * p_i + alpha * 0.5 * (p_{i-1} + p_{i+1}) For each i, the candidate point is clamped to the original triangle (NavCel) using closest-point-on-triangle, so it never leaves that NavCel. The final z' is the triangle height at the resulting (x', y').

Endpoints are kept fixed. Optionally, points forming a sharp angle are also kept (see corner_keep_deg).

Parameters

in_path	Input nav_msgs::msg::Path (world coordinates).
navmap	The NavMap where the path lies on.
iterations	Number of smoothing iterations (>= 1). Default: 5.
alpha	Smoothing factor in (0, 0.5]. Default: 0.4.
corner_keep_deg	Angle threshold (degrees): if the interior angle at a point is below this value, the point is kept as an anchor. Set <= 0 to disable. Default: 0 (disabled).

Returns

nav_msgs::msg::Path Smoothed path, same frame_id and header stamp as input.

update()

void update ( NavState & nav_state )

override

Executes a planning cycle using the current navigation state.

Computes a path from the robot's current pose to the goal using A*.

Parameters

nav_state

Current shared navigation state (input/output).

Member Data Documentation

centroids_

std::vector<Eigen::Vector3f> centroids_

protected

Cached centroids for each NavCel (same indexing as ::navmap::NavMap::navcels).

continuous_replan_

bool continuous_replan_ {true}

protected

Whether to replan the path at control frequency.

cost_axial_

double cost_axial_

protected

Cost multiplier for axial (horizontal/vertical) moves.

cost_diagonal_

double cost_diagonal_

protected

Cost multiplier for diagonal moves.

cost_factor_

double cost_factor_

protected

Scaling factor applied to cell cost values.

current_goal_

geometry_msgs::msg::Pose current_goal_

protected

Current goal.

current_path_

nav_msgs::msg::Path current_path_

protected

Most recently computed path.

g_

std::vector<double> g_

protected

Reusable buffers for A* search cost and parent links.

inflation_penalty_

double inflation_penalty_

protected

Extra cost penalty for paths near inflated obstacles.

layer_name_

std::string layer_name_

protected

occ_

std::vector<std::uint8_t> occ_

protected

Cached per-NavCel occupancy / cost values (0..255).

parent_

std::vector<::navmap::NavCelId> parent_

protected

path_pub_

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_

protected

Publisher for the computed navigation path (for visualization or monitoring).

---

The documentation for this class was generated from the following files:

• planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp
• planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp