EasyNav Simple Stack
Loading...
Searching...
No Matches
SimplePlanner Class Reference

A planner implementing the A* algorithm on a SimpleMap grid. More...

#include <SimplePlanner.hpp>

Inheritance diagram for SimplePlanner:
Collaboration diagram for SimplePlanner:

Public Member Functions

virtual std::expected< void, std::string > on_initialize () override
 Initializes the planner.
 
 SimplePlanner ()
 Default constructor.
 
void update (NavState &nav_state) override
 Updates the planner by computing a new path.
 

Protected Member Functions

std::vector< geometry_msgs::msg::Pose > a_star_path (const SimpleMap &map, const geometry_msgs::msg::Pose &start, const geometry_msgs::msg::Pose &goal, double resolution)
 Runs the A* algorithm to compute a path.
 
bool isFreeWithClearance (const SimpleMap &map, int cx, int cy, double clearance_cells)
 Checks whether a map cell is free, considering a clearance area.
 

Protected Attributes

double clearance_distance_
 Minimum clearance distance from obstacles in meters.
 
nav_msgs::msg::Path current_path_
 The last computed path.
 
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr path_pub_
 Publisher for the computed navigation path.
 
double robot_radius_
 Radius of the robot used for collision checking.
 

Detailed Description

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

Constructor & Destructor Documentation

◆ SimplePlanner()

SimplePlanner ( )
explicit

Default constructor.

Initializes the internal variables and parameters of the planner.

Member Function Documentation

◆ a_star_path()

std::vector< geometry_msgs::msg::Pose > a_star_path ( const SimpleMap & map,
const geometry_msgs::msg::Pose & start,
const geometry_msgs::msg::Pose & goal,
double resolution )
protected

Runs the A* algorithm to compute a path.

Parameters
mapThe occupancy map used for path planning.
startThe starting pose in world coordinates.
goalThe target pose in world coordinates.
resolutionThe cell resolution of the map (in meters).
Returns
A sequence of poses representing the planned path.

◆ isFreeWithClearance()

bool isFreeWithClearance ( const SimpleMap & map,
int cx,
int cy,
double clearance_cells )
protected

Checks whether a map cell is free, considering a clearance area.

This function verifies if a cell and its surrounding cells (within the specified clearance radius) are free of obstacles.

Parameters
mapThe occupancy map to query.
cxThe x-coordinate of the cell.
cyThe y-coordinate of the cell.
clearance_cellsThe clearance radius expressed in number of cells.
Returns
true if the cell and its clearance area are free, false otherwise.

◆ on_initialize()

std::expected< void, std::string > on_initialize ( )
overridevirtual

Initializes the planner.

Configures publishers, retrieves parameters, and prepares the planner for path generation using the available map data.

Returns
std::expected<void, std::string> Success or an error message.

◆ update()

void update ( NavState & nav_state)
override

Updates the planner by computing a new path.

Uses the current navigation state (including the robot's position and goal) to generate a path based on the A* algorithm.

Parameters
nav_stateThe current navigation state (contains odometry and goal information).

Member Data Documentation

◆ clearance_distance_

double clearance_distance_
protected

Minimum clearance distance from obstacles in meters.

◆ current_path_

nav_msgs::msg::Path current_path_
protected

The last computed path.

◆ path_pub_

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

Publisher for the computed navigation path.

◆ robot_radius_

double robot_radius_
protected

Radius of the robot used for collision checking.


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