Class CostmapPlanner

Inheritance Relationships

Base Type

  • public PlannerMethodBase

Class Documentation

class CostmapPlanner : public PlannerMethodBase

A planner implementing the A* algorithm on a Costmap2D grid.

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

Public Functions

explicit CostmapPlanner()

Default constructor.

Initializes internal parameters and configuration values.

virtual void on_initialize() override

Initializes the planner.

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

Throws:

std::runtime_error – if initialization fails.

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).

Protected Functions

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

Internal A* path planning routine.

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

Movement cost is influenced by:

  • The cost of each cell (retrieved from the costmap).

  • Additional inflation penalties near obstacles.

  • Anisotropic weights for axial vs diagonal movement.

Parameters:

  • map – The costmap 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.

Protected Attributes

double cost_factor_

Scaling factor applied to cell cost values.

double inflation_penalty_

Extra cost penalty for paths near inflated obstacles.

double heuristic_scale_ = {1.0}

Scaling factor applied to the heuristic term in A*.

bool continuous_replan_ = {true}

Wheter replan path at freq time.

nav_msgs::msg::Path current_path_

Most recently computed path.

geometry_msgs::msg::Pose current_goal_

Current goal.

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

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