EasyNav Gridmap Stack
Loading...
Searching...
No Matches
GridMapAStarPlanner.hpp
Go to the documentation of this file.
1// Copyright 2025 Intelligent Robotics Lab
2//
3// This file is part of the project Easy Navigation (EasyNav in short)
4// licensed under the GNU General Public License v3.0.
5// See <http://www.gnu.org/licenses/> for details.
6//
7// Easy Navigation program is free software: you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation, either version 3 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License
18// along with this program. If not, see <http://www.gnu.org/licenses/>.
19
22
23#ifndef EASYNAV_PLANNER__GRIDMAPASTARPLANNER_HPP_
24#define EASYNAV_PLANNER__GRIDMAPASTARPLANNER_HPP_
25
26#include <memory>
27#include <vector>
28#include <string>
29#include <expected>
30
31#include "rclcpp/rclcpp.hpp"
32#include "nav_msgs/msg/path.hpp"
33#include "geometry_msgs/msg/pose.hpp"
34
35#include "easynav_core/PlannerMethodBase.hpp"
36#include "easynav_common/types/NavState.hpp"
37
38#include "grid_map_core/GridMap.hpp"
39
40namespace easynav
41{
42
45{
46public:
48 explicit GridMapAStarPlanner();
49
57 virtual std::expected<void, std::string> on_initialize() override;
58
61 void update(NavState & nav_state) override;
62
63protected:
67
68 nav_msgs::msg::Path current_path_;
69 rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
70
76 std::vector<geometry_msgs::msg::Pose> a_star_path(
77 const grid_map::GridMap & map,
78 const geometry_msgs::msg::Pose & start,
79 const geometry_msgs::msg::Pose & goal);
80
86 bool isTraversable(
87 const grid_map::GridMap & map,
88 const grid_map::Index & from,
89 const grid_map::Index & to) const;
90};
91
92} // namespace easynav
93
94#endif // EASYNAV_PLANNER__GRIDMAPASTARPLANNER_HPP_
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