EasyNav Gridmap Stack
Loading...
Searching...
No Matches
GridMapAStarPlanner Class Reference

Planner based on A* algorithm using GridMap elevation data. More...

#include <GridMapAStarPlanner.hpp>

Inheritance diagram for GridMapAStarPlanner:
Collaboration diagram for GridMapAStarPlanner:

Public Member Functions

 GridMapAStarPlanner ()
 Default constructor.
 
virtual std::expected< void, std::string > on_initialize () override
 Initializes the planner.
 
void update (NavState &nav_state) override
 Computes a path using A* algorithm.
 

Protected Member Functions

std::vector< geometry_msgs::msg::Pose > a_star_path (const grid_map::GridMap &map, const geometry_msgs::msg::Pose &start, const geometry_msgs::msg::Pose &goal)
 Internal A* implementation.
 
bool isTraversable (const grid_map::GridMap &map, const grid_map::Index &from, const grid_map::Index &to) const
 Checks if the slope between two cells is below threshold.
 

Protected Attributes

double clearance_distance_
 
nav_msgs::msg::Path current_path_
 
double max_allowed_slope_
 
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr path_pub_
 
double robot_radius_
 

Detailed Description

Planner based on A* algorithm using GridMap elevation data.

Constructor & Destructor Documentation

◆ GridMapAStarPlanner()

GridMapAStarPlanner ( )
explicit

Default constructor.

Member Function Documentation

◆ a_star_path()

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

Internal A* implementation.

Parameters
mapThe grid map with elevation layer
startPose in world coordinates
goalPose in world coordinates
Returns
List of poses representing the path

◆ isTraversable()

bool isTraversable ( const grid_map::GridMap & map,
const grid_map::Index & from,
const grid_map::Index & to ) const
protected

Checks if the slope between two cells is below threshold.

Parameters
mapThe grid map
fromIndex of the first cell
toIndex of the second cell
Returns
true if slope is acceptable, false otherwise

◆ on_initialize()

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

Initializes the planner.

Creates necessary publishers/subscribers and initializes the map instances.

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

◆ update()

void update ( NavState & nav_state)
override

Computes a path using A* algorithm.

Parameters
nav_stateCurrent navigation state (with odometry and goals)

Member Data Documentation

◆ clearance_distance_

double clearance_distance_
protected

◆ current_path_

nav_msgs::msg::Path current_path_
protected

◆ max_allowed_slope_

double max_allowed_slope_
protected

◆ path_pub_

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

◆ robot_radius_

double robot_radius_
protected

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