diff --git a/README.md b/README.md index 230f249..3a8d7fa 100644 --- a/README.md +++ b/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 -``` \ No newline at end of file +--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 | `///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 +``` + +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. +``` diff --git a/easynav_pointcloud_common/CMakeLists.txt b/easynav_pointcloud_common/CMakeLists.txt new file mode 100644 index 0000000..ab86eb0 --- /dev/null +++ b/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 + $ + $ +) +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() \ No newline at end of file diff --git a/easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp b/easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp new file mode 100644 index 0000000..2a3fca7 --- /dev/null +++ b/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 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 . + +/// \file +/// \brief Declaration of the PointCloudData type. + +#ifndef EASYNAV_POINTCLOUD_MAPS_MANAGER_POINTCLOUDDATA_HPP_ +#define EASYNAV_POINTCLOUD_MAPS_MANAGER_POINTCLOUDDATA_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/point_field.hpp" +#include +#include +#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 & 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 fields_; + std::vector data_; + + std::vector get_fields(void) const; + void get_cloud(pcl::PointCloud & cloud) const; +}; + +} // namespace easynav + +#endif // EASYNAV_POINTCLOUD_MAPS_MANAGER_POINTCLOUDDATA_HPP_ \ No newline at end of file diff --git a/easynav_pointcloud_common/package.xml b/easynav_pointcloud_common/package.xml new file mode 100644 index 0000000..e16e3cf --- /dev/null +++ b/easynav_pointcloud_common/package.xml @@ -0,0 +1,24 @@ + + + + easynav_pointcloud_common + 0.0.1 + Easy Navigation: Point Cloud Common Package. + juanscelyg + esther + GPL-3.0-only + + ament_cmake + + easynav_common + + ament_index_cpp + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp b/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp new file mode 100644 index 0000000..509664e --- /dev/null +++ b/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp @@ -0,0 +1,199 @@ +// 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 +// 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 . + + +#include +#include +#include +#include // std::pair +#include +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include +#include +#include +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_types_conversion.h" + +#include "easynav_pointcloud_common/PointCloudData.hpp" + +namespace easynav +{ + +PointCloudData::PointCloudData() +: width_(0), height_(0), point_step_(1), row_step_(1), data_() +{} + +void +PointCloudData::initialize( + std::size_t width, + std::size_t height, + std::size_t point_step, + std::size_t row_step) +{ + width_ = width; + height_ = height; + point_step_ = point_step; + row_step_ = row_step; + fields_ = get_fields(); + data_.assign(row_step_ * height, 0); +} + +void +PointCloudData::deep_copy(const PointCloudData & other) +{ + width_ = other.width_; + height_ = other.height_; + point_step_ = other.point_step_; + row_step_ = other.row_step_; + fields_ = other.fields_; + data_ = other.data_; +} + +std::vector +PointCloudData::get_fields() const +{ + std::vector fields; + sensor_msgs::msg::PointField field; + field.name = "x"; + field.offset = 0; + field.datatype = sensor_msgs::msg::PointField::FLOAT32; + field.count = 1; + fields.push_back(field); + + field.name = "y"; + field.offset = 4; + fields.push_back(field); + + field.name = "z"; + field.offset = 8; + fields.push_back(field); + + return fields; +} + +void +PointCloudData::refresh( + sensor_msgs::msg::PointCloud2 & cloud_msg, + pcl::PointCloud & cloud) const +{ + pcl::PCLPointCloud2 cloud1_in, cloud2_in, cloud_out; + std::vector fields; + pcl_conversions::toPCL(fields_, fields); + cloud1_in.width = static_cast(width_); + cloud1_in.height = static_cast(height_); + cloud1_in.point_step = static_cast(point_step_); + cloud1_in.row_step = static_cast(row_step_); + cloud1_in.fields = fields; + cloud1_in.data = data_; + + pcl::toPCLPointCloud2(cloud, cloud2_in); + + pcl::concatenate(cloud1_in, cloud2_in, cloud_out); + + pcl::PointCloud out; + pcl::fromPCLPointCloud2(cloud_out, out); + pcl::toROSMsg(out, cloud_msg); +} + +void +PointCloudData::to_point_cloud(sensor_msgs::msg::PointCloud2 & cloud_msg) const +{ + cloud_msg.width = static_cast(width_); + cloud_msg.height = static_cast(height_); + cloud_msg.point_step = static_cast(point_step_); + cloud_msg.row_step = static_cast(row_step_); + cloud_msg.fields = fields_; + cloud_msg.data = data_; +} + +void +PointCloudData::from_point_cloud(const sensor_msgs::msg::PointCloud2 & cloud_msg) +{ + initialize( + cloud_msg.width, + cloud_msg.height, + cloud_msg.point_step, + cloud_msg.row_step); + + fields_ = cloud_msg.fields; + data_ = cloud_msg.data; +} + +void +PointCloudData::get_cloud(pcl::PointCloud & cloud) const +{ + sensor_msgs::msg::PointCloud2 cloud_msg; + cloud_msg.width = static_cast(width_); + cloud_msg.height = static_cast(height_); + cloud_msg.point_step = static_cast(point_step_); + cloud_msg.row_step = static_cast(row_step_); + cloud_msg.fields = fields_; + cloud_msg.data = data_; + + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::toPCL(cloud_msg, pcl_pc2); + pcl::fromPCLPointCloud2(pcl_pc2, cloud); + +} + +bool +PointCloudData::save_to_file(const std::string & path) const +{ + std::cerr << " Path to save map: " << path << "\n"; + pcl::PointCloud temp_cloud; + get_cloud(temp_cloud); + pcl::io::savePCDFileASCII (path, temp_cloud); + return true; +} + +bool +PointCloudData::load_from_file(const std::string & path) +{ + + pcl::PointCloud cloud; + + if (pcl::io::loadPCDFile(path, cloud) == -1) { //* load the file + std::cerr << " :: Load PointCloudData file failed :: \n"; + std::cerr << "Couldn't read file in " << path << " \n"; + return false; + } + + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(cloud, cloud_msg); + from_point_cloud(cloud_msg); + + return true; +} + +void +PointCloudData::show(const std::string & comment) const +{ + std::cerr << ":: PointCloudData Values :: \n"; + std::cerr << "Comment: " << comment << "\n"; + std::cerr << "width: " << width_ << "\n"; + std::cerr << "height: " << height_ << "\n"; + std::cerr << "point step: " << point_step_ << "\n"; + std::cerr << "row step: " << row_step_ << "\n"; + std::cerr << "fields lenght: " << fields_.size() << "\n"; + std::cerr << "data lenght: " << data_.size() << "\n"; +} + +} // namespace easynav diff --git a/easynav_pointcloud_maps_manager/CMakeLists.txt b/easynav_pointcloud_maps_manager/CMakeLists.txt new file mode 100644 index 0000000..d4df115 --- /dev/null +++ b/easynav_pointcloud_maps_manager/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_pointcloud_maps_manager) + +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(easynav_core REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(easynav_pointcloud_common REQUIRED) +find_package(std_srvs REQUIRED) +find_package(yaets REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + tf2_ros::tf2_ros + pluginlib::pluginlib + ament_index_cpp::ament_index_cpp + easynav_pointcloud_common::easynav_pointcloud_common + yaets::yaets + ${std_srvs_TARGETS} +) + +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}) + +# Register the planning plugins +pluginlib_export_plugin_description_file(easynav_core easynav_pointcloud_maps_manager_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + ament_index_cpp + easynav_pointcloud_common + std_srvs + yaets +) +ament_package() \ No newline at end of file diff --git a/easynav_pointcloud_maps_manager/easynav_pointcloud_maps_manager_plugins.xml b/easynav_pointcloud_maps_manager/easynav_pointcloud_maps_manager_plugins.xml new file mode 100644 index 0000000..d0c1821 --- /dev/null +++ b/easynav_pointcloud_maps_manager/easynav_pointcloud_maps_manager_plugins.xml @@ -0,0 +1,9 @@ + + + + + An implementation for the Maps Manager using Point Cloud representation. + + + + \ No newline at end of file diff --git a/easynav_pointcloud_maps_manager/include/easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp b/easynav_pointcloud_maps_manager/include/easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp new file mode 100644 index 0000000..64903aa --- /dev/null +++ b/easynav_pointcloud_maps_manager/include/easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp @@ -0,0 +1,170 @@ +// 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 +// 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 . + +/// \file +/// \brief Declaration of the PointCloudMapsManager method. + +#ifndef EASYNAV_POINTCLOUD_MAPS_MANAGER_HPP_ +#define EASYNAV_POINTCLOUD_MAPS_MANAGER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_types_conversion.h" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "easynav_core/MapsManagerBase.hpp" +#include "easynav_pointcloud_common/PointCloudData.hpp" + +#include "yaets/tracing.hpp" + +namespace easynav +{ + +/** + * @class PointCloudMapsManager + * @brief A plugin-based map manager using the PointCloudData structure. + * + * This manager implements a minimal mapping approach using Point Cloud Data + * (PointCloudData) for both static and dynamic maps. It supports publishing and + * receiving ROS Poitn Cloud 2 messages. + */ +class PointCloudMapsManager : public easynav::MapsManagerBase +{ +public: + /** + * @brief Default constructor. + */ + PointCloudMapsManager(); + + /** + * @brief Destructor. + */ + ~PointCloudMapsManager(); + + /** + * @brief Initializes the maps manager. + * + * Creates necessary publishers/subscribers and initializes the map instances. + * + * @return std::expected Success or error string. + */ + virtual std::expected on_initialize() override; + + /** + * @brief Updates the internal maps using the current navigation state. + * + * Intended to be called periodically. May perform dynamic map updates + * based on new sensor data or internal state. + * + * @param nav_state Current state of the navigation system. + */ + virtual void update(NavState & nav_state) override; + + /** + * @brief Replaces the current static map. + * + * @param new_map Shared pointer to a new map object. Must be of type PointCloudData. + */ + void set_static_map(const PointCloudData & new_map); + + /** + * @brief Replaces the current dynamic map. + * + * @param new_map Shared pointer to a new map object. Must be of type PointCloudData. + */ + void set_dynamic_map(const PointCloudData & new_map); + +protected: + /** + * @brief Full path to the map file. + */ + std::string map_path_ {"/tmp/map.pcd"}; + +private: + /** + * @brief Internal static map. + */ + PointCloudData static_map_; + + /** + * @brief Internal dynamic map. + */ + PointCloudData dynamic_map_; + + /** + * @brief Publisher for the static Point Cloud. + */ + rclcpp::Publisher::SharedPtr static_map_pub_; + + /** + * @brief Publisher for the dynamic Point Cloud. + */ + rclcpp::Publisher::SharedPtr dynamic_map_pub_; + + /** + * @brief Subscriber for external incoming static map updates. + */ + rclcpp::Subscription::SharedPtr incoming_map_sub_; + + /** + * @brief Service for saving current map to disk. + */ + rclcpp::Service::SharedPtr savemap_srv_; + + /** + * @brief Cached Point Cloud 2 message for the static map. + */ + sensor_msgs::msg::PointCloud2 static_map_msg_; + + /** + * @brief Cached Point Cloud 2 message for the dynamic map. + */ + sensor_msgs::msg::PointCloud2 dynamic_map_msg_; + + /** + * @brief Buffer storing transformations between frames. + */ + std::shared_ptr tf_buffer_; + + /** + * @brief Transform listener that populates the tf buffer. + */ + std::shared_ptr tf_listener_; + + pcl::PointCloud fused_perception_; + + std::shared_ptr session_; +}; + +} // namespace easynav + +#endif // EASYNAV_POINTCLOUD_MAPS_MANAGER_HPP_ diff --git a/easynav_pointcloud_maps_manager/package.xml b/easynav_pointcloud_maps_manager/package.xml new file mode 100644 index 0000000..c4fe605 --- /dev/null +++ b/easynav_pointcloud_maps_manager/package.xml @@ -0,0 +1,30 @@ + + + + easynav_pointcloud_maps_manager + 0.0.1 + Easy Navigation: Point Cloud Map Manager Package. + juanscelyg + esther + GPL-3.0-only + + ament_cmake + + easynav_common + easynav_core + pluginlib + nav_msgs + geometry_msgs + ament_index_cpp + easynav_pointcloud_common + std_srvs + yaets + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp b/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp new file mode 100644 index 0000000..5f9d1f3 --- /dev/null +++ b/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp @@ -0,0 +1,202 @@ +// 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 +// 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 . + +/// \file +/// \brief Implementation of the PointCloudMapsManager class. + +#include + +#include "easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp" +#include "easynav_common/types/Perceptions.hpp" +#include "easynav_common/types/PointPerception.hpp" + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_prefix.hpp" + +namespace easynav +{ + +using std::placeholders::_1; + +PointCloudMapsManager::PointCloudMapsManager() +{ + NavState::register_printer( + [](const PointCloudData & map) { + std::string ret = "PointCloud of (" + + std::to_string(map.width()) + " x " + + std::to_string(map.height()) + ") with resolution "; + return ret; + }); +} + +PointCloudMapsManager::~PointCloudMapsManager() +{ +} + +std::expected +PointCloudMapsManager::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + std::string package_name, map_path_file, map_topic_in; + node->declare_parameter(plugin_name + ".package_name", package_name); + node->declare_parameter(plugin_name + ".map_path_file", map_path_file); + node->declare_parameter(plugin_name + ".map_topic_in", map_topic_in); + + node->get_parameter(plugin_name + ".package_name", package_name); + node->get_parameter(plugin_name + ".map_path_file", map_path_file); + node->get_parameter(plugin_name + ".map_topic_in", map_topic_in); + + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : ..:: PARAMETERS ::.."); + + if (package_name != "" && map_path_file != "") { + std::string pkgpath; + try { + pkgpath = ament_index_cpp::get_package_share_directory(package_name); + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : Package name = %s", package_name.c_str()); + map_path_ = pkgpath + "/" + map_path_file; + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : Map path file = %s", map_path_.c_str()); + } catch(ament_index_cpp::PackageNotFoundError & ex) { + return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + } + + if (!static_map_.load_from_file(map_path_)) { + return std::unexpected("File [" + map_path_ + "] not found"); + } else { + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : Map loaded from file"); + dynamic_map_.deep_copy(static_map_); + } + } else { + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : Path Map file to save = ./%s", map_path_.c_str()); + } + + std::string topic_name; + if (map_topic_in != "") { + topic_name = map_topic_in; + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : Map topic in name = %s", topic_name.c_str()); + } else { + topic_name = node->get_name() + std::string("/") + plugin_name + "/topic_in"; + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : Input Topic by default"); + } + + + static_map_pub_ = node->create_publisher( + node->get_name() + std::string("/") + plugin_name + "/static_map", + rclcpp::QoS(1).transient_local().reliable()); + + dynamic_map_pub_ = node->create_publisher( + node->get_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + + incoming_map_sub_ = node->create_subscription( + topic_name, rclcpp::QoS(1).transient_local().reliable(), + [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { + + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : topic_callback : Reading Map"); + static_map_.from_point_cloud(*msg); + + static_map_.to_point_cloud(static_map_msg_); + static_map_msg_.header.frame_id = "map"; + static_map_msg_.header.stamp = this->get_node()->now(); + + static_map_pub_->publish(static_map_msg_); + }); + + savemap_srv_ = node->create_service( + node->get_name() + std::string("/") + plugin_name + "/savemap", + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + (void)request; + + if (!static_map_.save_to_file(map_path_)) { + response->success = false; + response->message = "PointCloud : Failed to save map to: " + map_path_; + } else { + static_map_.show("Data to be saved"); + response->success = true; + response->message = "PointCloud : Map successfully saved to: " + map_path_; + } + }); + + tf_buffer_ = std::make_shared(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, node, + true); + + static_map_.to_point_cloud(static_map_msg_); + static_map_msg_.header.frame_id = "map"; + static_map_msg_.header.stamp = this->get_node()->now(); + static_map_pub_->publish(static_map_msg_); + + dynamic_map_.to_point_cloud(dynamic_map_msg_); + dynamic_map_msg_.header.frame_id = "map"; + dynamic_map_msg_.header.stamp = this->get_node()->now(); + dynamic_map_pub_->publish(dynamic_map_msg_); + + return {}; +} + +void +PointCloudMapsManager::set_static_map(const PointCloudData & new_map) +{ + static_map_ = new_map; +} + +void +PointCloudMapsManager::set_dynamic_map(const PointCloudData & new_map) +{ + dynamic_map_ = new_map; +} + +void +PointCloudMapsManager::update(NavState & nav_state) +{ + dynamic_map_.deep_copy(static_map_); + + if (nav_state.has("points")) { + const auto & perceptions = nav_state.get("points"); + + auto fused = PointPerceptionsOpsView(perceptions) + .fuse("map")->as_points(); + + dynamic_map_.refresh(dynamic_map_msg_, fused); + dynamic_map_.show("dynamic fused"); + } + nav_state.set("map.static", static_map_); + nav_state.set("map.dynamic", dynamic_map_); + + dynamic_map_.to_point_cloud(dynamic_map_msg_); + dynamic_map_msg_.header.frame_id = "map"; + dynamic_map_msg_.header.stamp = get_node()->now(); + dynamic_map_pub_->publish(dynamic_map_msg_); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::PointCloudMapsManager, easynav::MapsManagerBase)