Skip to content

Commit ba36f7f

Browse files
fmricofmrico
committed
Merge with rolling
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
2 parents c06bebc + d6e1a42 commit ba36f7f

12 files changed

Lines changed: 435 additions & 6 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: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,6 @@ 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)

‎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_gridmap_maps_manager">
3+
<class name="easynav_gridmap_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: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
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 GridmapMapsBuilderNode class.
22+
23+
#ifndef EASYNAV_GRIDMAP_MAPS_MANAGER__GRIDMAPMAPSBUILDERNODE_HPP_
24+
#define EASYNAV_GRIDMAP_MAPS_MANAGER__GRIDMAPMAPSBUILDERNODE_HPP_
25+
26+
#include "rclcpp/rclcpp.hpp"
27+
#include "rclcpp/macros.hpp"
28+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
29+
30+
#include "sensor_msgs/msg/point_cloud2.hpp"
31+
#include "grid_map_msgs/msg/grid_map.hpp"
32+
#include "std_srvs/srv/trigger.hpp"
33+
34+
#include "grid_map_ros/grid_map_ros.hpp"
35+
36+
#include "easynav_common/types/Perceptions.hpp"
37+
38+
namespace easynav
39+
{
40+
41+
/**
42+
* @class GridmapMapsBuilderNode
43+
* @brief Lifecycle node that subscribes to point cloud sensor data and builds a grid map.
44+
*
45+
* This node processes perception data (point clouds) to generate a single outdoor grid map.
46+
* It uses ROS 2 lifecycle management for clean startup, activation, deactivation, and cleanup.
47+
* The node publishes the resulting grid map for downstream use.
48+
*/
49+
class GridmapMapsBuilderNode : public rclcpp_lifecycle::LifecycleNode
50+
{
51+
public:
52+
RCLCPP_SMART_PTR_DEFINITIONS(GridmapMapsBuilderNode)
53+
54+
using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
55+
56+
/**
57+
* @brief Constructor.
58+
* @param options Options for node initialization.
59+
*/
60+
explicit GridmapMapsBuilderNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
61+
62+
/**
63+
* @brief Destructor.
64+
*/
65+
~GridmapMapsBuilderNode();
66+
67+
/**
68+
* @brief Lifecycle configure callback.
69+
* @param state Current lifecycle state.
70+
* @return CallbackReturnT indicating success or failure.
71+
*/
72+
CallbackReturnT on_configure(const rclcpp_lifecycle::State & state) override;
73+
74+
/**
75+
* @brief Lifecycle activate callback.
76+
* @param state Current lifecycle state.
77+
* @return CallbackReturnT indicating success or failure.
78+
*/
79+
CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) override;
80+
81+
/**
82+
* @brief Lifecycle deactivate callback.
83+
* @param state Current lifecycle state.
84+
* @return CallbackReturnT indicating success or failure.
85+
*/
86+
CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state) override;
87+
88+
/**
89+
* @brief Lifecycle cleanup callback.
90+
* @param state Current lifecycle state.
91+
* @return CallbackReturnT indicating success or failure.
92+
*/
93+
CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state) override;
94+
95+
/**
96+
* @brief Perform a processing cycle on the perception data and update the grid map.
97+
*
98+
* This method should be called periodically (e.g., in a timer or main loop) to process
99+
* incoming sensor data, update the internal grid map representation, and publish outputs.
100+
*/
101+
void cycle();
102+
103+
/**
104+
* @brief Registers a perception handler.
105+
* @param handler Shared pointer to a PerceptionHandler instance.
106+
*/
107+
void register_handler(std::shared_ptr<PerceptionHandler> handler);
108+
109+
const grid_map::GridMap & get_map() const {return map_;}
110+
void set_map(const grid_map::GridMap & map) {map_ = map;}
111+
112+
private:
113+
/// Name of the sensor topic to subscribe to (e.g., point clouds).
114+
std::string sensor_topic_;
115+
116+
/// Map of perception data grouped by sensor name.
117+
std::map<std::string, std::vector<PerceptionPtr>> perceptions_;
118+
119+
/// Callback group for concurrency management of subscriptions and timers.
120+
rclcpp::CallbackGroup::SharedPtr cbg_;
121+
122+
/// Downsampling resolution applied to point cloud data.
123+
double downsample_resolution_;
124+
125+
/// Default frame ID used for perception data and published messages.
126+
std::string perception_default_frame_;
127+
128+
/// Publisher for the processed grid map.
129+
rclcpp_lifecycle::LifecyclePublisher<grid_map_msgs::msg::GridMap>::SharedPtr pub_;
130+
131+
/// Registered perception handlers by sensor name.
132+
std::map<std::string, std::shared_ptr<PerceptionHandler>> handlers_;
133+
134+
grid_map::GridMap map_;
135+
};
136+
137+
} // namespace easynav
138+
139+
#endif // EASYNAV_GRIDMAP_MAPS_MANAGER__GRIDMAPMAPSBUILDERNODE_HPP_

‎easynav_gridmap_maps_manager/include/easynav_gridmap_maps_manager/utils.hpp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
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 GridmapMapsBuilderNode class.
22+
23+
#ifndef EASYNAV_GRIDMAP_MAPS_MANAGER__UTILS_HPP_
24+
#define EASYNAV_GRIDMAP_MAPS_MANAGER__UTILS_HPP_
25+
26+
27+
#include "grid_map_msgs/msg/grid_map.hpp"
28+
#include "grid_map_ros/grid_map_ros.hpp"
29+
30+
namespace easynav
31+
{
32+
33+
bool save_gridmap(const std::string & filename, const grid_map::GridMap & map);
34+
bool load_gridmap(const std::string & filename, grid_map::GridMap & map);
35+
36+
} // namespace easynav
37+
38+
#endif // EASYNAV_GRIDMAP_MAPS_MANAGER__GRIDMAPMAPSBUILDERNODE_HPP_

‎easynav_gridmap_maps_manager/package.xml‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>easynav_gridmap_maps_manager</name>
5+
<version>0.0.1</version>
6+
<description>Easy Navigation: Gridmap Map Builder Package.</description>
7+
<maintainer email="francisco.romero@urjc.es">Francisco Jose Romero Ramirez</maintainer>
8+
<license>GPL-3.0-only</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rclcpp</depend>
13+
<depend>sensor_msgs</depend>
14+
<depend>std_msgs</depend>
15+
<depend>std_srvs</depend>
16+
<depend>geometry_msgs</depend>
17+
<depend>easynav_common</depend>
18+
<depend>grid_map_ros</depend>
19+
<depend>grid_map_msgs</depend>
20+
<depend>ament_index_cpp</depend>
21+
<depend>yaml_cpp_vendor</depend>
22+
<depend>cv_bridge</depend>
23+
24+
<test_depend>ament_lint_auto</test_depend>
25+
<test_depend>ament_lint_common</test_depend>
26+
27+
<export>
28+
<build_type>ament_cmake</build_type>
29+
</export>
30+
</package>

‎easynav_gridmap_maps_manager/src/easynav_gridmap_maps_manager/utils.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -122,5 +122,4 @@ bool load_gridmap(const std::string & filename, grid_map::GridMap & map)
122122

123123
return true;
124124
}
125-
126125
} // namespace easynav

‎easynav_gridmap_maps_manager/src/gridmap_maps_builder_main.cpp‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
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+
#include "rclcpp/rclcpp.hpp"
21+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
22+
23+
#include "lifecycle_msgs/msg/transition.hpp"
24+
#include "lifecycle_msgs/msg/state.hpp"
25+
26+
#include "tf2_ros/buffer.h"
27+
#include "tf2_ros/transform_listener.h"
28+
29+
#include "easynav_common/RTTFBuffer.hpp"
30+
31+
#include "easynav_gridmap_maps_manager/GridmapMapsBuilderNode.hpp"
32+
33+
int main(int argc, char **argv)
34+
{
35+
rclcpp::init(argc, argv);
36+
37+
auto node = std::make_shared<easynav::GridmapMapsBuilderNode>(rclcpp::NodeOptions());
38+
39+
rclcpp::executors::SingleThreadedExecutor exec;
40+
exec.add_node(node->get_node_base_interface());
41+
42+
auto tf_node = rclcpp::Node::make_shared("tf_node");
43+
auto tf_buffer = easynav::RTTFBuffer::getInstance(tf_node->get_clock());
44+
45+
node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
46+
if (node->get_current_state().id() !=
47+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
48+
{
49+
RCLCPP_ERROR(node->get_logger(), "Failed to configure node");
50+
rclcpp::shutdown();
51+
return 1;
52+
}
53+
node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
54+
if (node->get_current_state().id() !=
55+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
56+
{
57+
RCLCPP_ERROR(node->get_logger(), "Failed to activate node");
58+
rclcpp::shutdown();
59+
return 1;
60+
}
61+
62+
tf2_ros::TransformListener tf_listener(*tf_buffer, tf_node, true);
63+
rclcpp::Rate rate(10);
64+
65+
while (rclcpp::ok()) {
66+
exec.spin_some();
67+
68+
if (node->get_current_state().id() ==
69+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
70+
{
71+
node->cycle();
72+
}
73+
74+
rate.sleep();
75+
}
76+
rclcpp::shutdown();
77+
return 0;
78+
}

‎easynav_gridmap_maps_manager/tests/CMakeLists.txt‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
ament_add_gtest(gridmap_maps_builder_tests gridmap_maps_builder_tests.cpp)
2+
target_include_directories(gridmap_maps_builder_tests PUBLIC
3+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
4+
$<INSTALL_INTERFACE:include>
5+
)
6+
target_link_libraries(gridmap_maps_builder_tests
7+
${PROJECT_NAME} ${dependencies}
8+
)
9+
10+
ament_add_gtest(gridmap_maps_manager_tests gridmap_maps_manager_tests.cpp)
11+
target_include_directories(gridmap_maps_manager_tests PUBLIC
12+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
13+
$<INSTALL_INTERFACE:include>
14+
)
15+
target_link_libraries(gridmap_maps_manager_tests
16+
${PROJECT_NAME} ${dependencies}
17+
)
18+
19+
ament_add_gtest(gridmap_utils_tests gridmap_utils_tests.cpp)
20+
target_include_directories(gridmap_utils_tests PUBLIC
21+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
22+
$<INSTALL_INTERFACE:include>
23+
)
24+
target_link_libraries(gridmap_utils_tests
25+
${PROJECT_NAME} ${dependencies}
26+
)

‎easynav_gridmap_maps_manager/tests/gridmap_maps_builder_tests.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -194,5 +194,4 @@ TEST_F(GridmapMapsBuilderTest, test_configure_success)
194194

195195
ASSERT_NEAR(map_z, incoming_z, 0.001);
196196
}
197-
198197
}

0 commit comments

Comments
 (0)