|
| 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 | + |
| 20 | +/// \file |
| 21 | +/// \brief Declaration of the GridMapAStarPlanner class implementing A* path planning using elevation maps. |
| 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 | + |
| 40 | +namespace easynav |
| 41 | +{ |
| 42 | + |
| 43 | +/// \brief Planner based on A* algorithm using GridMap elevation data |
| 44 | +class GridMapAStarPlanner : public PlannerMethodBase |
| 45 | +{ |
| 46 | +public: |
| 47 | + /// \brief Default constructor |
| 48 | + explicit GridMapAStarPlanner(); |
| 49 | + |
| 50 | + /** |
| 51 | + * @brief Initializes the planner. |
| 52 | + * |
| 53 | + * Creates necessary publishers/subscribers and initializes the map instances. |
| 54 | + * |
| 55 | + * @return std::expected<void, std::string> Success or error string. |
| 56 | + */ |
| 57 | + virtual std::expected<void, std::string> on_initialize() override; |
| 58 | + |
| 59 | + /// \brief Computes a path using A* algorithm |
| 60 | + /// \param nav_state Current navigation state (with odometry and goals) |
| 61 | + void update(NavState & nav_state) override; |
| 62 | + |
| 63 | +protected: |
| 64 | + double robot_radius_; |
| 65 | + double clearance_distance_; |
| 66 | + double max_allowed_slope_; |
| 67 | + |
| 68 | + nav_msgs::msg::Path current_path_; |
| 69 | + rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_; |
| 70 | + |
| 71 | + /// \brief Internal A* implementation |
| 72 | + /// \param map The grid map with elevation layer |
| 73 | + /// \param start Pose in world coordinates |
| 74 | + /// \param goal Pose in world coordinates |
| 75 | + /// \return List of poses representing the path |
| 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 | + |
| 81 | + /// \brief Checks if the slope between two cells is below threshold |
| 82 | + /// \param map The grid map |
| 83 | + /// \param from Index of the first cell |
| 84 | + /// \param to Index of the second cell |
| 85 | + /// \return true if slope is acceptable, false otherwise |
| 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_ |
0 commit comments