23#ifndef EASYNAV_PLANNER__GRIDMAPASTARPLANNER_HPP_
24#define EASYNAV_PLANNER__GRIDMAPASTARPLANNER_HPP_
31#include "rclcpp/rclcpp.hpp"
32#include "nav_msgs/msg/path.hpp"
33#include "geometry_msgs/msg/pose.hpp"
35#include "easynav_core/PlannerMethodBase.hpp"
36#include "easynav_common/types/NavState.hpp"
38#include "grid_map_core/GridMap.hpp"
57 virtual std::expected<void, std::string>
on_initialize()
override;
61 void update(NavState & nav_state)
override;
69 rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr
path_pub_;
77 const grid_map::GridMap & map,
78 const geometry_msgs::msg::Pose & start,
79 const geometry_msgs::msg::Pose & goal);
87 const grid_map::GridMap & map,
88 const grid_map::Index & from,
89 const grid_map::Index & to)
const;
GridMapAStarPlanner()
Default constructor.
Definition GridMapAStarPlanner.cpp:107
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.
Definition GridMapAStarPlanner.cpp:212
void update(NavState &nav_state) override
Computes a path using A* algorithm.
Definition GridMapAStarPlanner.cpp:141
double max_allowed_slope_
Definition GridMapAStarPlanner.hpp:66
double robot_radius_
Definition GridMapAStarPlanner.hpp:64
nav_msgs::msg::Path current_path_
Definition GridMapAStarPlanner.hpp:68
virtual std::expected< void, std::string > on_initialize() override
Initializes the planner.
Definition GridMapAStarPlanner.cpp:117
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr path_pub_
Definition GridMapAStarPlanner.hpp:69
double clearance_distance_
Definition GridMapAStarPlanner.hpp:65
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.
Definition GridMapAStarPlanner.cpp:189
Definition GridMapAStarPlanner.hpp:41