EasyNav Costmap Stack
Loading...
Searching...
No Matches
CostmapMapsManager.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__SIMPLEMAPMANAGER_HPP_
24#define EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_
25
26#include <vector>
27#include <stdexcept>
28#include <algorithm>
29#include <utility>
30#include <fstream>
31#include <sstream>
32
33#include "nav_msgs/msg/occupancy_grid.hpp"
34#include "std_srvs/srv/trigger.hpp"
35
36#include "tf2_ros/buffer.h"
37#include "tf2_ros/transform_listener.h"
38
39#include "easynav_core/MapsManagerBase.hpp"
41
43#include "pluginlib/class_loader.hpp"
44
45#include "yaets/tracing.hpp"
46
47namespace easynav
48{
49
58class CostmapMapsManager : public easynav::MapsManagerBase
59{
60public:
65
70
78 virtual std::expected<void, std::string> on_initialize() override;
79
88 virtual void update(NavState & nav_state) override;
89
95 void set_static_map(const Costmap2D & new_map);
96
97protected:
101 std::string map_path_;
102
103private:
107 Costmap2D static_map_;
108
112 rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr static_occ_pub_;
113
117 rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr dynamic_occ_pub_;
118
122 rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr incoming_map_sub_;
123
127 rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr savemap_srv_;
128
132 nav_msgs::msg::OccupancyGrid static_grid_msg_;
133
137 nav_msgs::msg::OccupancyGrid dynamic_grid_msg_;
138
139 std::unique_ptr<pluginlib::ClassLoader<CostmapFilter>> costmap_filters_loader_;
140
141 std::vector<std::shared_ptr<CostmapFilter>> costmap_filters_;
142};
143
144} // namespace easynav
145
146#endif // EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition costmap_2d.hpp:69
virtual void update(NavState &nav_state) override
Updates the internal maps using the current navigation state.
Definition CostmapMapsManager.cpp:184
CostmapMapsManager()
Default constructor.
Definition CostmapMapsManager.cpp:41
void set_static_map(const Costmap2D &new_map)
Replaces the current static map.
Definition CostmapMapsManager.cpp:177
~CostmapMapsManager()
Destructor.
Definition CostmapMapsManager.cpp:55
virtual std::expected< void, std::string > on_initialize() override
Initializes the maps manager.
Definition CostmapMapsManager.cpp:58
std::string map_path_
Full path to the map file.
Definition CostmapMapsManager.hpp:101
Provides a mapping for often used cost values.
Definition cost_values.hpp:41