Merge pull request #2 from estherag/rolling

Blackboard for Pointcloud Perceptions

Author: estherag

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
+```

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
-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()

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 <http://www.gnu.org/licenses/> 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 <http://www.gnu.org/licenses/>.
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
 /// \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 <string>
+#include <memory>
+#include <map>
+#include <vector>
 
 #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<PerceptionHandler> 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<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
+
+    /// Map of perception data grouped by sensor name.
+  std::map<std::string, std::vector<PerceptionPtr>> perceptions_;
+
+    /// Registered perception handlers by sensor name.
+  std::map<std::string, std::shared_ptr<PerceptionHandler>> handlers_;
 };
 
 }  // namespace easynav
 
-#endif  // EASYNAV_OUTDOOR_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_
+#endif  // EASYNAV_POINTCLOUD_MAPS_BUILDER__POINTCLOUDMAPSBUILDERNODE_HPP_