Maps Manager works with Point Cloud Representation

README.md

@@ -2,11 +2,11 @@
 
 This stack is part of the Easy Navigation (EasyNav) project developed by the Intelligent Robotics Lab. It is composed of the **pointcloud_maps_builder** package and the **pointcloud_maps_manager** package (currently under construction).
 
-The `PointcloudMapsBuilderNode` subscribes to point cloud sensor data, downsamples and fuses it, and republishes the processed data for mapping or further processing.
-
+## Pointcloud Maps Builder
 
+The `PointcloudMapsBuilderNode` subscribes to point cloud sensor data, downsamples and fuses it, and republishes the processed data for mapping or further processing.
 
-## Parameters
+### Parameters
 
 | Parameter Name            | Type    | Default   | Description                                      |
 |--------------------------|---------|-----------|--------------------------------------------------|
@@ -16,26 +16,32 @@ The `PointcloudMapsBuilderNode` subscribes to point cloud sensor data, downsampl
 
 ---
 
-## Installation
+### Installation
 
 Clone the repository into your ROS 2 workspace:
+
 ```bash
 cd ~/ros2_ws/src
 git clone https://github.com/EasyNavigation/easynav_pointcloud_stack.git
+git clone https://github.com/EasyNavigation/easynav_outdoor_testcase.git
 cd ..
 colcon build --packages-select easynav_pointcloud_maps_builder
 ```
 
-## Usage
+### Usage
 
 Source your workspace:
+
 ```bash
 source ~/ros2_ws/install/setup.bash
 ```
+
 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:
@@ -54,7 +60,72 @@ pointcloud_maps_builder_node:
 ```
 
 2. Run the node using the parameter file with this command:
-```
+
+``` bash
 ros2 run easynav_pointcloud_maps_builder pointcloud_maps_builder_main \
---ros-args --params-file src/easynav_pointcloud_stack/params.yaml
-```
+--ros-args --params-file src/easynav_outdoor_testcase/params.yaml
+```
+
+## Pointcloud Maps Manager
+
+The `PointcloudMapsManager` subscribes to point cloud topic managed by Maps Builder. There is another option when the point cloud is stored in a Point Cloud Data file (`.pcd`). The manager can handle both cases, allowing for flexible integration with existing point cloud data.
+
+### Parameters
+
+| Parameter Name            | Type    | Default   | Description                                      |
+|--------------------------|---------|-----------|--------------------------------------------------|
+| `map_types`           | string | ` `                            | Map representation defined by map manager |
+
+By each `map_types` define in the params file is required to define the following parameters:
+
+| Parameter Name            | Type    | Default   | Description                                      |
+|--------------------------|---------|-----------|--------------------------------------------------|
+| `map_topic_in`        | string | `/<node name>/<plugin name>/topic_in`  | Topic where the point cloud is received |
+| `package_name`        | string | ` `                            | Package name where is located the `pcd` file.|
+| `map_path_file`       | string | `pointcloud_map.pcd`      | Filename for `pcd` map file.     |
+
+>> **Note:** The `map_path_file` is only required if the `package_name` is defined. If not, the point cloud data will be read from topic defined in `map_topic_in`.
+
+### Installation
+
+Clone the repository into your ROS 2 workspace:
+
+``` bash
+cd ~/ros2_ws/src
+git clone https://github.com/EasyNavigation/easynav_pointcloud_stack.git
+git clone https://github.com/EasyNavigation/easynav_outdoor_testcase.git
+cd ..
+colcon build --packages-select easynav_pointcloud_maps_manager easynav_pointcloud_common
+```
+
+### Usage
+
+Source your workspace:
+
+```bash
+source ~/ros2_ws/install/setup.bash
+```
+
+Run the main node:
+
+```bash
+ros2 run easynav_system system_main --ros-args --params-file <yaml file with parameters>
+```
+
+The file `maps_manager.params.yaml` could be modified to include the desired point cloud map types and their respective parameters.
+
+## How to create a parameter file for Pointcloud Maps Manager
+
+1. Create a parameter YAML file (e.g., `params.yaml`) with the following content:
+
+```yaml
+maps_manager_node:
+  ros__parameters:
+    use_sim_time: true # When data source is from file this parameter should be set to false
+    map_types: [PointCloudData] # Map types defined by the manager
+    PointCloudData:
+      plugin: easynav_pointcloud_maps_manager/PointCloudMapsManager # Plugin name
+      map_topic_in: /map_builder/cloud_filtered # Topic where the point cloud is received
+      package_name: easynav_outdoor_testcase # Package name where is located the `pcd` file.
+      map_path_file: maps/pointcloud_map.pcd  # Filename for `pcd` map file.
+```

easynav_pointcloud_common/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.20)
+project(easynav_pointcloud_common)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_CXX_STANDARD 23)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+find_package(ament_cmake REQUIRED)
+find_package(easynav_common REQUIRED)
+# find_package(pluginlib REQUIRED)
+
+
+add_library(${PROJECT_NAME} SHARED
+  src/easynav_pointcloud_common/PointCloudData.cpp
+)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  easynav_common::easynav_common
+  # pluginlib::pluginlib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  #add_subdirectory(tests)
+endif()
+
+ament_export_include_directories("include/${PROJECT_NAME}")
+ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(export_${PROJECT_NAME})
+ament_export_dependencies(
+  easynav_common
+  # pluginlib
+)
+ament_package()

easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp

@@ -0,0 +1,146 @@
+// 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 <http://www.gnu.org/licenses/> for details.
+//
+// Easy Navigation 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.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// 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/>.
+
+/// \file
+/// \brief Declaration of the PointCloudData type.
+
+#ifndef EASYNAV_POINTCLOUD_MAPS_MANAGER_POINTCLOUDDATA_HPP_
+#define EASYNAV_POINTCLOUD_MAPS_MANAGER_POINTCLOUDDATA_HPP_
+
+#include <vector>
+#include <stdexcept>
+#include <algorithm>
+#include <utility>
+#include <fstream>
+#include <sstream>
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/msg/point_field.hpp"
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include "pcl_conversions/pcl_conversions.h"
+#include "pcl/point_types_conversion.h"
+
+
+namespace easynav
+{
+
+/**
+ * @class PointCloudData
+ * @brief Point Cloud representation using basic C++ types.
+ */
+class PointCloudData
+{
+public:
+  /**
+   * @brief Default constructor.
+   */
+  PointCloudData();
+
+  /**
+   * @brief Initialize the map with characteristics of Point Cloud 2.
+   *
+   * @param width_ Number of columns.
+   * @param height_ Number of rows.
+   * @param point_step_ Number of elements per point.
+   * @param row_step_ Number of elements per row.
+   */
+  void initialize(
+    std::size_t width_, std::size_t height_, std::size_t point_step_,
+    std::size_t row_step_);
+
+  /**
+   * @brief Returns the width (number of columns) of the map.
+   */
+  size_t width() const {return width_;}
+
+  /**
+   * @brief Returns the height (number of rows) of the map.
+   */
+  size_t height() const {return height_;}
+
+  /**
+   * @brief Load map data from a PointCloudData object.
+   *
+   * This function get elements from another PointCloudData object to copy data.
+   *
+   * @param other PointCloudData object source of data.
+   */
+  void deep_copy(const PointCloudData & other);
+
+  /**
+   * @brief Load map data from sensor_msgs::msg::PointCloud2 message.
+   *
+   * This function read data from a Point Cloud 2 msg to set values
+   * in PointCloudData objects.
+   *
+   * @param cloud_msg The Point Cloud 2 message to load from.
+   */
+  void from_point_cloud(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+  /**
+   * @brief Updates a sensor_msgs::msg::PointCloud2 message from the sensors
+   * values into Easynav.
+   *
+   * @param cloud_msg The Point Cloud 2 message to be configured.
+   * @param cloud Point Cloud with updated values from sensors.
+   */
+  void refresh(
+    sensor_msgs::msg::PointCloud2 & cloud_msg,
+    pcl::PointCloud<pcl::PointXYZ> & cloud) const;
+
+  /**
+   * @brief Updates a sensor_msgs::msg::PointCloud2 message from the PointCloudData
+   * contents.
+   *
+   * @param cloud_msg The Point Cloud 2 message to fill or update.
+   */
+  void to_point_cloud(sensor_msgs::msg::PointCloud2 & cloud_msg) const;
+
+  /**
+  * @brief Saves the map to a file.
+  * @param path Path to the output file.
+  * @return true if the file was written successfully, false otherwise.
+  */
+  bool save_to_file(const std::string & path) const;
+
+  /**
+   * @brief Loads the map from a file.
+   * @param path Path to the input file.
+   * @return true if the file was read successfully and is valid, false otherwise.
+   */
+  bool load_from_file(const std::string & path);
+
+  void show(const std::string & comment) const;
+
+private:
+  std::size_t width_;
+  std::size_t height_;
+  std::size_t point_step_;
+  std::size_t row_step_;
+  std::vector<sensor_msgs::msg::PointField> fields_;
+  std::vector<uint8_t> data_;
+
+  std::vector<sensor_msgs::msg::PointField> get_fields(void) const;
+  void get_cloud(pcl::PointCloud<pcl::PointXYZ> & cloud) const;
+};
+
+}  // namespace easynav
+
+#endif  // EASYNAV_POINTCLOUD_MAPS_MANAGER_POINTCLOUDDATA_HPP_

easynav_pointcloud_common/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>easynav_pointcloud_common</name>
+  <version>0.0.1</version>
+  <description>Easy Navigation: Point Cloud Common Package.</description>
+  <maintainer email="juanscelyg@gmail.com">juanscelyg</maintainer>
+  <maintainer email="e.aguado.glez@gmail.com">esther</maintainer>
+  <license>GPL-3.0-only</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>easynav_common</depend>
+  <!-- <depend>pluginlib</depend> -->
+  <depend>ament_index_cpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>