diff --git a/README.md b/README.md index 33146b0..230f249 100644 --- a/README.md +++ b/README.md @@ -36,3 +36,25 @@ Run the lifecycle node: ```bash ros2 run easynav_pointcloud_maps_builder pointcloud_maps_builder_node ``` +## Test + +1. Create a parameter YAML file (e.g., `params.yaml`) with the following content: + +```yaml +pointcloud_maps_builder_node: + ros__parameters: + use_sim_time: true + sensors: [map] + downsample_resolution: 0.1 + perception_default_frame: map + map: + topic: map + type: sensor_msgs/msg/PointCloud2 + group: points +``` + +2. Run the node using the parameter file with this command: +``` +ros2 run easynav_pointcloud_maps_builder pointcloud_maps_builder_main \ +--ros-args --params-file src/easynav_pointcloud_stack/params.yaml +``` \ No newline at end of file diff --git a/easynav_pointcloud_maps_builder/CMakeLists.txt b/easynav_pointcloud_maps_builder/CMakeLists.txt index 213642b..face2ea 100644 --- a/easynav_pointcloud_maps_builder/CMakeLists.txt +++ b/easynav_pointcloud_maps_builder/CMakeLists.txt @@ -16,41 +16,43 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) find_package(easynav_common REQUIRED) -set(dependencies - rclcpp - rclcpp_lifecycle - sensor_msgs - easynav_common -) - -include_directories(include) # Library add_library(${PROJECT_NAME} SHARED src/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) -ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -# Executable -add_executable(pointcloud_maps_builder_main src/pointcloud_maps_builder_main.cpp) -ament_target_dependencies(pointcloud_maps_builder_main ${dependencies}) -target_link_libraries(pointcloud_maps_builder_main ${PROJECT_NAME}) +target_link_libraries(${PROJECT_NAME} PUBLIC + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + sensor_msgs::sensor_msgs_library + easynav_common::easynav_common +) # Install headers install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) # Install targets install(TARGETS ${PROJECT_NAME} - pointcloud_maps_builder_main + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) +# Executable +add_executable(pointcloud_maps_builder_main src/pointcloud_maps_builder_main.cpp) +target_link_libraries(pointcloud_maps_builder_main PUBLIC + ${PROJECT_NAME} +) # Tests if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -63,8 +65,14 @@ if(BUILD_TESTING) endif() # Export -ament_export_include_directories(include) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(${dependencies}) +ament_export_targets(export_${PROJECT_NAME}) +ament_export_dependencies( + rclcpp + rclcpp_lifecycle + sensor_msgs + easynav_common +) ament_package() diff --git a/easynav_pointcloud_maps_builder/include/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.hpp b/easynav_pointcloud_maps_builder/include/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.hpp index 5a6e2ab..8f102b0 100644 --- a/easynav_pointcloud_maps_builder/include/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.hpp +++ b/easynav_pointcloud_maps_builder/include/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.hpp @@ -1,10 +1,6 @@ // Copyright 2025 Intelligent Robotics Lab // -// This file is part of the project Easy Navigation (EasyNav in sh0rt) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify +// This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. @@ -15,13 +11,18 @@ // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// along with this program. If not, see . /// \file /// \brief Definition of the PointcloudMapsBuilderNode class. -#ifndef EASYNAV_OUTDOOR_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_ -#define EASYNAV_OUTDOOR_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_ +#ifndef EASYNAV_POINTCLOUD_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_ +#define EASYNAV_POINTCLOUD_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_ + +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp/macros.hpp" @@ -33,88 +34,93 @@ namespace easynav { -/** - * @class PointcloudMapsBuilderNode - * @brief Lifecycle node that subscribes to point cloud sensor data and manages point cloud map building. - * - * This node processes perception data (point clouds) to build and update point cloud maps. - * It leverages ROS 2 lifecycle for clean startup, activation, deactivation, and cleanup phases. - * The node publishes processed (filtered or downsampled) point cloud maps for downstream use. - */ + /** + * @class PointcloudMapsBuilderNode + * @brief Lifecycle node that subscribes to point cloud sensor data and manages point cloud map building. + * + * This node processes perception data (point clouds) to build and update point cloud maps. + * It leverages ROS 2 lifecycle for clean startup, activation, deactivation, and cleanup phases. + * The node publishes processed (filtered or downsampled) point cloud maps for downstream use. + */ class PointcloudMapsBuilderNode : public rclcpp_lifecycle::LifecycleNode { public: RCLCPP_SMART_PTR_DEFINITIONS(PointcloudMapsBuilderNode) - using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /** - * @brief Constructor. - * @param options Node initialization options. - */ + /** + * @brief Constructor. + * @param options Node initialization options. + */ explicit PointcloudMapsBuilderNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - /** - * @brief Destructor. - */ + /** + * @brief Destructor. + */ ~PointcloudMapsBuilderNode(); - /** - * @brief Lifecycle configure callback. - * @param state Current lifecycle state. - * @return CallbackReturnT indicating success or failure. - */ + /** + * @brief Lifecycle configure callback. + * @param state Current lifecycle state. + * @return CallbackReturnT indicating success or failure. + */ CallbackReturnT on_configure(const rclcpp_lifecycle::State & state) override; - /** - * @brief Lifecycle activate callback. - * @param state Current lifecycle state. - * @return CallbackReturnT indicating success or failure. - */ + /** + * @brief Lifecycle activate callback. + * @param state Current lifecycle state. + * @return CallbackReturnT indicating success or failure. + */ CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Lifecycle deactivate callback. - * @param state Current lifecycle state. - * @return CallbackReturnT indicating success or failure. - */ + /** + * @brief Lifecycle deactivate callback. + * @param state Current lifecycle state. + * @return CallbackReturnT indicating success or failure. + */ CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Lifecycle cleanup callback. - * @param state Current lifecycle state. - * @return CallbackReturnT indicating success or failure. - */ + /** + * @brief Lifecycle cleanup callback. + * @param state Current lifecycle state. + * @return CallbackReturnT indicating success or failure. + */ CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state) override; - /** - * @brief Perform a processing cycle on perception data and update the point cloud map. - * - * This should be called periodically (e.g., in a timer or main loop) to process incoming sensor data, - * update internal map representations, and publish outputs. - */ + /** + * @brief Perform a processing cycle on perception data and update the point cloud map. + * + * This should be called periodically (e.g., in a timer or main loop) to process incoming sensor data, + * update internal map representations, and publish outputs. + */ void cycle(); -private: - /// Sensor topic name to subscribe to point clouds. - std::string sensor_topic_; - - /// Perception data container. - Perceptions perceptions_; + /** + * @brief Registers a perception handler. + * @param handler Shared pointer to a PerceptionHandler instance. + */ + void register_handler(std::shared_ptr handler); - /// Callback group for subscription and timers. +private: + /// Callback group used for subscriptions and timers. rclcpp::CallbackGroup::SharedPtr cbg_; - /// Downsampling resolution for point clouds. + /// Downsampling resolution for point clouds. double downsample_resolution_; - /// Default frame ID for perception and publishing. + /// Default frame ID used for perception and publishing. std::string perception_default_frame_; - /// Publisher for processed point cloud map. + /// Lifecycle publisher for the processed point cloud map. rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_; + + /// Map of perception data grouped by sensor name. + std::map> perceptions_; + + /// Registered perception handlers by sensor name. + std::map> handlers_; }; } // namespace easynav -#endif // EASYNAV_OUTDOOR_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_ +#endif // EASYNAV_POINTCLOUD_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_ diff --git a/easynav_pointcloud_maps_builder/src/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.cpp b/easynav_pointcloud_maps_builder/src/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.cpp index 5147f9e..3deec6a 100644 --- a/easynav_pointcloud_maps_builder/src/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.cpp +++ b/easynav_pointcloud_maps_builder/src/easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.cpp @@ -1,10 +1,6 @@ // Copyright 2025 Intelligent Robotics Lab // -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify +// This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. @@ -15,7 +11,7 @@ // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// along with this program. If not, see . /// \file /// \brief Implementation of the PointcloudMapsBuilderNode class. @@ -27,7 +23,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "easynav_pointcloud_maps_builder/PointcloudMapsBuilderNode.hpp" -#include "easynav_common/types/Perceptions.hpp" +#include "easynav_common/types/PointPerception.hpp" namespace easynav { @@ -37,13 +33,12 @@ PointcloudMapsBuilderNode::PointcloudMapsBuilderNode(const rclcpp::NodeOptions & { cbg_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (!has_parameter("sensor_topic")) { - declare_parameter("sensor_topic", "points"); - } - if (!has_parameter("downsample_resolution")) { declare_parameter("downsample_resolution", 1.0); } + if (!has_parameter("sensors")) { + declare_parameter("sensors", std::vector()); + } if (!has_parameter("perception_default_frame")) { declare_parameter("perception_default_frame", "map"); @@ -51,6 +46,8 @@ PointcloudMapsBuilderNode::PointcloudMapsBuilderNode(const rclcpp::NodeOptions & pub_ = this->create_publisher( "map_builder_pointcloud/cloud_filtered", rclcpp::QoS(1).transient_local().reliable()); + + register_handler(std::make_shared()); } PointcloudMapsBuilderNode::~PointcloudMapsBuilderNode() @@ -58,12 +55,6 @@ PointcloudMapsBuilderNode::~PointcloudMapsBuilderNode() if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVE_SHUTDOWN); } - if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_INACTIVE_SHUTDOWN); - } - if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); - } } using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -73,22 +64,46 @@ PointcloudMapsBuilderNode::on_configure(const rclcpp_lifecycle::State & state) { (void)state; - get_parameter("sensor_topic", sensor_topic_); + std::vector sensors; + get_parameter("sensors", sensors); get_parameter("downsample_resolution", downsample_resolution_); get_parameter("perception_default_frame", perception_default_frame_); - auto perception_entry = std::make_shared(); - perception_entry->data.points.clear(); - perception_entry->data.clear(); - perception_entry->frame_id = ""; - perception_entry->stamp = now(); - perception_entry->valid = false; - perception_entry->new_data = true; + for (const auto & sensor_id : sensors) { + std::string topic, msg_type, group; + + if (!has_parameter(sensor_id + ".topic")) { + declare_parameter(sensor_id + ".topic", topic); + } + if (!has_parameter(sensor_id + ".type")) { + declare_parameter(sensor_id + ".type", msg_type); + } + if (!has_parameter(sensor_id + ".group")) { + declare_parameter(sensor_id + ".group", group); + } - perceptions_.push_back(perception_entry); + get_parameter(sensor_id + ".topic", topic); + get_parameter(sensor_id + ".type", msg_type); + get_parameter(sensor_id + ".group", group); - perception_entry->subscription = create_typed_subscription( - *this, sensor_topic_, perception_entry, cbg_); + RCLCPP_DEBUG(get_logger(), + "Loaded sensor parameters: id=%s topic=%s type=%s group=%s", + sensor_id.c_str(), topic.c_str(), msg_type.c_str(), group.c_str()); + + auto handler_it = handlers_.find(group); + if (handler_it == handlers_.end()) { + RCLCPP_WARN(get_logger(), "No handler for group [%s]", group.c_str()); + continue; + } + + auto ptr = handler_it->second->create(sensor_id); + auto sub = handler_it->second->create_subscription(*this, topic, msg_type, ptr, cbg_); + + perceptions_[group].emplace_back(PerceptionPtr{ptr, sub}); + + RCLCPP_DEBUG(get_logger(), "Creating perception for sensor %s", sensor_id.c_str()); + RCLCPP_DEBUG(get_logger(), "Handler group = %s", group.c_str()); + } return CallbackReturnT::SUCCESS; } @@ -122,19 +137,20 @@ PointcloudMapsBuilderNode::on_cleanup(const rclcpp_lifecycle::State & state) void PointcloudMapsBuilderNode::cycle() { - // Finish cycle if no new perceptions - if (std::none_of(perceptions_.begin(), perceptions_.end(), - [](const auto & perception) - {return perception && perception->new_data;})) + // Finish cycle if no new perceptions + if (std::none_of(perceptions_["points"].begin(), perceptions_["points"].end(), + [](const auto & p) + {return p.perception->new_data;})) { return; } if (pub_->get_subscription_count() > 0) { - auto processed_perceptions = PerceptionsOpsView(perceptions_); - // Fuse perceptions if the frame_id is different from default and downsample - if (!perceptions_.empty() && perceptions_[0] && - perceptions_[0]->frame_id != perception_default_frame_) + auto point_perceptions = get_point_perceptions(perceptions_["points"]); + auto processed_perceptions = PointPerceptionsOpsView(point_perceptions); + // Fuse perceptions if the frame_id is different from default and downsample + if (!point_perceptions.empty() && point_perceptions[0] && + point_perceptions[0]->frame_id != perception_default_frame_) { processed_perceptions.downsample(downsample_resolution_).fuse(perception_default_frame_); } else { @@ -147,22 +163,29 @@ void PointcloudMapsBuilderNode::cycle() } auto msg = points_to_rosmsg(downsampled_points); - msg.header.frame_id = perceptions_[0]->frame_id; + msg.header.frame_id = point_perceptions[0]->frame_id; - if (perceptions_[0]->stamp.nanoseconds() != 0) { - msg.header.stamp = perceptions_[0]->stamp; + if (point_perceptions[0]->stamp.nanoseconds() != 0) { + msg.header.stamp = point_perceptions[0]->stamp; } else { msg.header.stamp = now(); } pub_->publish(msg); - // Mark perceptions as not new after published - for (auto & perception : perceptions_) { - if (perception->new_data) { - perception->new_data = false; + // Mark perceptions as not new after published + for (auto & p : perceptions_["points"]) { + if (p.perception->new_data) { + p.perception->new_data = false; } } } } -} // namespace easynav + +void +PointcloudMapsBuilderNode::register_handler(std::shared_ptr handler) +{ + handlers_[handler->group()] = handler; +} + +} // namespace easynav diff --git a/easynav_pointcloud_maps_builder/src/pointcloud_maps_builder_main.cpp b/easynav_pointcloud_maps_builder/src/pointcloud_maps_builder_main.cpp index 8025389..fefab42 100644 --- a/easynav_pointcloud_maps_builder/src/pointcloud_maps_builder_main.cpp +++ b/easynav_pointcloud_maps_builder/src/pointcloud_maps_builder_main.cpp @@ -1,10 +1,6 @@ // Copyright 2025 Intelligent Robotics Lab // -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify +// This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. @@ -15,7 +11,7 @@ // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// along with this program. If not, see . #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -34,7 +30,7 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(rclcpp::NodeOptions()); + auto node = easynav::PointcloudMapsBuilderNode::make_shared(); rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node->get_node_base_interface()); diff --git a/easynav_pointcloud_maps_builder/tests/CMakeLists.txt b/easynav_pointcloud_maps_builder/tests/CMakeLists.txt index fb1450f..ecc3a58 100644 --- a/easynav_pointcloud_maps_builder/tests/CMakeLists.txt +++ b/easynav_pointcloud_maps_builder/tests/CMakeLists.txt @@ -1,11 +1,14 @@ ament_add_gtest(pointcloud_maps_builder_tests pointcloud_maps_builder_tests.cpp) + target_include_directories(pointcloud_maps_builder_tests PUBLIC $ $ ) + target_link_libraries(pointcloud_maps_builder_tests ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + sensor_msgs::sensor_msgs_library + easynav_common::easynav_common ) -ament_target_dependencies(pointcloud_maps_builder_tests ${dependencies}) - - diff --git a/easynav_pointcloud_maps_builder/tests/pointcloud_maps_builder_tests.cpp b/easynav_pointcloud_maps_builder/tests/pointcloud_maps_builder_tests.cpp index 321c69a..082bf12 100644 --- a/easynav_pointcloud_maps_builder/tests/pointcloud_maps_builder_tests.cpp +++ b/easynav_pointcloud_maps_builder/tests/pointcloud_maps_builder_tests.cpp @@ -1,10 +1,6 @@ // Copyright 2025 Intelligent Robotics Lab // -// This file is part of the project Easy Navigation (EasyNav in sh0rt) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify +// This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. @@ -15,7 +11,7 @@ // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// along with this program. If not, see . #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -43,8 +39,7 @@ TEST_F(PointcloudMapsBuilderTest, test_configure_success) { auto options = rclcpp::NodeOptions(); - options.append_parameter_override("map_types", std::vector{"pcl"}); - options.append_parameter_override("sensor_topic", "points"); + options.append_parameter_override("downsample_resolution", 0.2); auto node = std::make_shared(options);