Skip to content

Commit 5fb342e

Browse files
fmricofmrico
committed
Add MapsManager for Gridmap
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
1 parent 63eff14 commit 5fb342e

10 files changed

Lines changed: 534 additions & 105 deletions

File tree

‎.github/workflows/rolling.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ jobs:
2828
- name: build and test
2929
uses: ros-tooling/action-ros-ci@0.4.3
3030
with:
31-
package-name: easynav_gridmap_maps_builder
31+
package-name: easynav_gridmap_maps_manager
3232
target-ros2-distro: rolling
3333
vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
3434
skip-test: true

‎easynav_gridmap_maps_manager/CMakeLists.txt‎

Lines changed: 55 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,13 @@ set(CMAKE_CXX_STANDARD 23)
99
set(CMAKE_CXX_STANDARD_REQUIRED ON)
1010
set(CMAKE_CXX_EXTENSIONS OFF)
1111

12-
set(CMAKE_BUILD_TYPE Debug)
13-
1412
# Dependencies
1513
find_package(ament_cmake REQUIRED)
1614
find_package(rclcpp REQUIRED)
1715
find_package(rclcpp_lifecycle REQUIRED)
1816
find_package(sensor_msgs REQUIRED)
1917
find_package(easynav_common REQUIRED)
18+
find_package(easynav_core REQUIRED)
2019
find_package(std_msgs REQUIRED)
2120
find_package(std_srvs REQUIRED)
2221
find_package(geometry_msgs REQUIRED)
@@ -27,48 +26,61 @@ find_package(yaml_cpp_vendor REQUIRED)
2726
find_package(cv_bridge REQUIRED)
2827

2928
set(dependencies
30-
rclcpp
31-
rclcpp_lifecycle
32-
sensor_msgs
33-
std_msgs
34-
std_srvs
35-
geometry_msgs
36-
easynav_common
37-
grid_map_ros
38-
grid_map_msgs
39-
ament_index_cpp
40-
yaml_cpp_vendor
41-
cv_bridge
29+
rclcpp::rclcpp
30+
rclcpp_lifecycle::rclcpp_lifecycle
31+
easynav_common::easynav_common
32+
easynav_core::easynav_core
33+
grid_map_ros::grid_map_ros
34+
ament_index_cpp::ament_index_cpp
35+
cv_bridge::cv_bridge
36+
${sensor_msgs_TARGETS}
37+
${std_msgs_TARGETS}
38+
${std_srvs_TARGETS}
39+
${geometry_msgs_TARGETS}
40+
${grid_map_msgs_TARGETS}
4241
)
4342

44-
include_directories(include)
45-
4643
# Library
4744
add_library(${PROJECT_NAME} SHARED
45+
src/easynav_gridmap_maps_manager/GridmapMapsManager.cpp
4846
src/easynav_gridmap_maps_manager/GridmapMapsBuilderNode.cpp
4947
src/easynav_gridmap_maps_manager/utils.cpp
5048
)
51-
ament_target_dependencies(${PROJECT_NAME} ${dependencies})
49+
target_include_directories(${PROJECT_NAME} PUBLIC
50+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
51+
$<BUILD_INTERFACE:${yaml-cpp_INCLUDE_DIRS}>
52+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
53+
)
54+
target_link_libraries(${PROJECT_NAME} PUBLIC ${dependencies})
5255

5356
# Executable
5457
add_executable(gridmap_maps_builder_main src/gridmap_maps_builder_main.cpp)
55-
ament_target_dependencies(gridmap_maps_builder_main ${dependencies})
56-
target_link_libraries(gridmap_maps_builder_main ${PROJECT_NAME})
58+
target_include_directories(${PROJECT_NAME} PUBLIC
59+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
60+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
61+
)
62+
target_link_libraries(gridmap_maps_builder_main ${PROJECT_NAME} ${dependencies})
5763

58-
# Install headers
59-
install(DIRECTORY include/
60-
DESTINATION include/
64+
install(
65+
DIRECTORY include/
66+
DESTINATION include/${PROJECT_NAME}
6167
)
6268

63-
# Install targets
6469
install(TARGETS
65-
${PROJECT_NAME}
6670
gridmap_maps_builder_main
6771
ARCHIVE DESTINATION lib
6872
LIBRARY DESTINATION lib
6973
RUNTIME DESTINATION lib/${PROJECT_NAME}
7074
)
7175

76+
install(TARGETS
77+
${PROJECT_NAME}
78+
EXPORT export_${PROJECT_NAME}
79+
ARCHIVE DESTINATION lib
80+
LIBRARY DESTINATION lib
81+
RUNTIME DESTINATION lib/${PROJECT_NAME}
82+
)
83+
7284
# Tests
7385
if(BUILD_TESTING)
7486
find_package(ament_lint_auto REQUIRED)
@@ -80,9 +92,25 @@ if(BUILD_TESTING)
8092
add_subdirectory(tests)
8193
endif()
8294

83-
# Export
84-
ament_export_include_directories(include)
95+
ament_export_include_directories("include/${PROJECT_NAME}")
8596
ament_export_libraries(${PROJECT_NAME})
86-
ament_export_dependencies(${dependencies})
97+
ament_export_targets(export_${PROJECT_NAME})
8798

99+
# Register the plugin
100+
pluginlib_export_plugin_description_file(easynav_core easynav_gridmap_maps_manager_plugins.xml)
101+
102+
ament_export_dependencies(
103+
rclcpp
104+
rclcpp_lifecycle
105+
sensor_msgs
106+
std_msgs
107+
std_srvs
108+
geometry_msgs
109+
easynav_common
110+
grid_map_ros
111+
grid_map_msgs
112+
ament_index_cpp
113+
yaml_cpp_vendor
114+
cv_bridge
115+
)
88116
ament_package()

‎easynav_gridmap_maps_manager/easynav_gridmap_maps_manager_plugins.xml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<class_libraries>
2+
<library path="easynav_simple_maps_manager">
3+
<class name="easynav_simple_maps_manager/GridmapMapsManager" type="easynav::GridmapMapsManager" base_class_type="easynav::MapsManagerBase">
4+
<description>
5+
A gridmap implementation for the Maps Manager.
6+
</description>
7+
</class>
8+
</library>
9+
</class_libraries>

‎easynav_gridmap_maps_manager/include/easynav_gridmap_maps_manager/GridmapMapsBuilderNode.hpp‎

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -128,11 +128,6 @@ class GridmapMapsBuilderNode : public rclcpp_lifecycle::LifecycleNode
128128
/// Publisher for the processed grid map.
129129
rclcpp_lifecycle::LifecyclePublisher<grid_map_msgs::msg::GridMap>::SharedPtr pub_;
130130

131-
/**
132-
* @brief Service for saving current map to disk.
133-
*/
134-
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr savemap_srv_;
135-
136131
/// Registered perception handlers by sensor name.
137132
std::map<std::string, std::shared_ptr<PerceptionHandler>> handlers_;
138133

‎easynav_gridmap_maps_manager/include/easynav_gridmap_maps_manager/GridmapMapsManager.hpp‎

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
// Copyright 2025 Intelligent Robotics Lab
2+
//
3+
// This file is part of the project Easy Navigation (EasyNav in sh0rt)
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 Definition of the GridmapManagerNode class.
22+
23+
#ifndef EASYNAV_GRIDMAP_MAPS_MANAGER__GRIDMAPMAPSMANAGER_HPP_
24+
#define EASYNAV_GRIDMAP_MAPS_MANAGER__GRIDMAPMAPSMANAGER_HPP_
25+
26+
#include "grid_map_msgs/msg/grid_map.hpp"
27+
#include "std_srvs/srv/trigger.hpp"
28+
29+
#include "grid_map_ros/grid_map_ros.hpp"
30+
31+
#include "easynav_core/MapsManagerBase.hpp"
32+
33+
#include "yaets/tracing.hpp"
34+
35+
namespace easynav
36+
{
37+
38+
39+
/**
40+
* @class GridmapMapsManager
41+
* @brief A plugin-based map manager using Gridmaps.
42+
*
43+
*/
44+
class GridmapMapsManager : public easynav::MapsManagerBase
45+
{
46+
public:
47+
/**
48+
* @brief Default constructor.
49+
*/
50+
GridmapMapsManager();
51+
52+
/**
53+
* @brief Destructor.
54+
*/
55+
~GridmapMapsManager();
56+
57+
/**
58+
* @brief Initializes the maps manager.
59+
*
60+
* Creates necessary publishers/subscribers and initializes the map instances.
61+
*
62+
* @return std::expected<void, std::string> Success or error string.
63+
*/
64+
virtual std::expected<void, std::string> on_initialize() override;
65+
66+
/**
67+
* @brief Updates the internal maps using the current navigation state.
68+
*
69+
* Intended to be called periodically. May perform dynamic map updates
70+
* based on new sensor data or internal state.
71+
*
72+
* @param nav_state Current state of the navigation system.
73+
*/
74+
virtual void update(NavState & nav_state) override;
75+
76+
protected:
77+
/**
78+
* @brief Full path to the map file.
79+
*/
80+
std::string map_path_;
81+
82+
private:
83+
/**
84+
* @brief The Gridmap.
85+
*/
86+
grid_map::GridMap map_;
87+
88+
/**
89+
* @brief Publisher for the gridmap.
90+
*/
91+
rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr gridmap_pub_;
92+
93+
/**
94+
* @brief Subscriber for external incoming gridmap updates.
95+
*/
96+
rclcpp::Subscription<grid_map_msgs::msg::GridMap>::SharedPtr incoming_map_sub_;
97+
98+
/**
99+
* @brief Service for saving current map to disk.
100+
*/
101+
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr savemap_srv_;
102+
103+
/**
104+
* @brief Cached gridmap message for the gridmap.
105+
*/
106+
grid_map_msgs::msg::GridMap gridmap_msg_;
107+
108+
bool map_need_update_ {true};
109+
};
110+
111+
} // namespace easynav
112+
113+
#endif // EASYNAV_GRIDMAP_MAPS_MANAGER__GRIDMAPMAPSBUILDERNODE_HPP_

‎easynav_gridmap_maps_manager/src/easynav_gridmap_maps_manager/GridmapMapsBuilderNode.cpp‎

Lines changed: 0 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -59,22 +59,6 @@ GridmapMapsBuilderNode::GridmapMapsBuilderNode(const rclcpp::NodeOptions & optio
5959
pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>(
6060
"map_builder_gridmap/gridmap", rclcpp::QoS(1).transient_local().reliable());
6161

62-
savemap_srv_ = create_service<std_srvs::srv::Trigger>(
63-
get_name() + std::string("/") + get_name() + "/savemap",
64-
[this](
65-
const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
66-
std::shared_ptr<std_srvs::srv::Trigger::Response> response)
67-
{
68-
(void)request;
69-
if (!save_gridmap("gridmap.yaml", this->map_)) {
70-
response->success = false;
71-
response->message = "Failed to save map_ to current directory";
72-
} else {
73-
response->success = true;
74-
response->message = "Map successfully saved to current directory";
75-
}
76-
});
77-
7862
register_handler(std::make_shared<PointPerceptionHandler>());
7963
}
8064

0 commit comments

Comments
 (0)