From 29cfc2e6ca1f8822338a0969a7daaba4b94b215c Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 11 Jul 2025 18:49:39 +0200 Subject: [PATCH 1/6] Blackboard was integrated --- easynav_pointcloud_common/CMakeLists.txt | 59 ++++++ .../PointCloudData.hpp | 146 +++++++++++++ easynav_pointcloud_common/package.xml | 24 +++ .../PointCloudData.cpp | 199 ++++++++++++++++++ .../CMakeLists.txt | 77 +++++++ ...asynav_pointcloud_maps_manager_plugins.xml | 9 + .../PointCloudMapsManager.hpp | 170 +++++++++++++++ easynav_pointcloud_maps_manager/package.xml | 30 +++ .../PointCloudMapsManager.cpp | 195 +++++++++++++++++ robot_params/maps_builder.params.yaml | 10 + robot_params/maps_manager.params.yaml | 10 + 11 files changed, 929 insertions(+) create mode 100644 easynav_pointcloud_common/CMakeLists.txt create mode 100644 easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp create mode 100644 easynav_pointcloud_common/package.xml create mode 100644 easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp create mode 100644 easynav_pointcloud_maps_manager/CMakeLists.txt create mode 100644 easynav_pointcloud_maps_manager/easynav_pointcloud_maps_manager_plugins.xml create mode 100644 easynav_pointcloud_maps_manager/include/easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp create mode 100644 easynav_pointcloud_maps_manager/package.xml create mode 100644 easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp create mode 100644 robot_params/maps_builder.params.yaml create mode 100644 robot_params/maps_manager.params.yaml 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..33f52da --- /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(void) 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..a3b36ea --- /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 +{ + std::cerr << "Getting Data from msg\n"; + 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(void) const +{ + std::cerr << ":: PointCloudData values :: \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..9d9d975 --- /dev/null +++ b/easynav_pointcloud_maps_manager/CMakeLists.txt @@ -0,0 +1,77 @@ +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(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 + 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..576c5d9 --- /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_; + +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..6845654 --- /dev/null +++ b/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp @@ -0,0 +1,195 @@ +// 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", 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", package_name); + node->get_parameter(plugin_name + ".map_path_file", map_path_file); + node->get_parameter(plugin_name + ".map_topic_in", map_topic_in); + + std::cerr << ":: PARAMETERS :: \n"; + std::cerr << "Package name: " << package_name << "\n"; + std::cerr << "Map path file: " << map_path_file << "\n"; + std::cerr << "map topic in name: " << map_topic_in << "\n"; + + if (package_name != "" && map_path_file != "") { + std::string pkgpath; + try { + pkgpath = ament_index_cpp::get_package_share_directory(package_name); + map_path_ = pkgpath + "/" + map_path_file; + } 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(), + "PointCloudMapsManager::File read : Map loaded"); + dynamic_map_.deep_copy(static_map_); + } + } + + std::string topic_name; + if (map_topic_in != "") { + topic_name = map_topic_in; + } else { + topic_name = node->get_name() + std::string("/") + plugin_name + "/topic_in"; + } + + if (map_path_file == "") { + map_path_ = "map.PointCloudData"; + } + + RCLCPP_INFO(get_node()->get_logger(), + "PointCloudMapsManager::Map path file: %s", map_path_.c_str()); + + 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(), + "PointCloudMapsManager::topic_callback: reading map"); + static_map_.from_point_cloud(*msg); + dynamic_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 = "Failed to save map to: " + map_path_; + } else { + response->success = true; + response->message = "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_); + + 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) +{ + RCLCPP_INFO(get_node()->get_logger(),"Updating"); + dynamic_map_.deep_copy(static_map_); + + if (!nav_state.has("points")) { + nav_state.set("map.static", static_map_); + nav_state.set("map.dynamic", dynamic_map_); + return; + } + + const auto & perceptions = nav_state.get("points"); + + auto fused = PointPerceptionsOpsView(perceptions) + .fuse("map")->as_points(); + + dynamic_map_.refresh(dynamic_map_msg_, fused); + + 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) diff --git a/robot_params/maps_builder.params.yaml b/robot_params/maps_builder.params.yaml new file mode 100644 index 0000000..55469d1 --- /dev/null +++ b/robot_params/maps_builder.params.yaml @@ -0,0 +1,10 @@ +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 \ No newline at end of file diff --git a/robot_params/maps_manager.params.yaml b/robot_params/maps_manager.params.yaml new file mode 100644 index 0000000..ee5f252 --- /dev/null +++ b/robot_params/maps_manager.params.yaml @@ -0,0 +1,10 @@ +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: + - PointCloudData + PointCloudData: + plugin: easynav_pointcloud_maps_manager/PointCloudMapsManager + package: easynav_pointcloud_maps_manager + map_topic_in: /map_builder/cloud_filtered + \ No newline at end of file From e65c22e882faac70ce3d71444c6118c5131f2889 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 14 Jul 2025 19:57:16 +0200 Subject: [PATCH 2/6] Dynamic map published by Maps manager --- .../PointCloudData.hpp | 2 +- .../PointCloudData.cpp | 4 +- .../CMakeLists.txt | 4 +- .../PointCloudMapsManager.cpp | 59 ++++++++----------- robot_params/maps_manager.params.yaml | 5 +- 5 files changed, 33 insertions(+), 41 deletions(-) diff --git a/easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp b/easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp index 33f52da..2a3fca7 100644 --- a/easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp +++ b/easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp @@ -127,7 +127,7 @@ class PointCloudData */ bool load_from_file(const std::string & path); - void show(void) const; + void show(const std::string & comment) const; private: std::size_t width_; diff --git a/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp b/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp index a3b36ea..5e3815f 100644 --- a/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp +++ b/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp @@ -116,7 +116,6 @@ PointCloudData::refresh( void PointCloudData::to_point_cloud(sensor_msgs::msg::PointCloud2 & cloud_msg) const { - std::cerr << "Getting Data from msg\n"; cloud_msg.width = static_cast(width_); cloud_msg.height = static_cast(height_); cloud_msg.point_step = static_cast(point_step_); @@ -185,9 +184,10 @@ PointCloudData::load_from_file(const std::string & path) } void -PointCloudData::show(void) const +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"; diff --git a/easynav_pointcloud_maps_manager/CMakeLists.txt b/easynav_pointcloud_maps_manager/CMakeLists.txt index 9d9d975..d4df115 100644 --- a/easynav_pointcloud_maps_manager/CMakeLists.txt +++ b/easynav_pointcloud_maps_manager/CMakeLists.txt @@ -12,6 +12,7 @@ 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) @@ -23,11 +24,12 @@ add_library(${PROJECT_NAME} SHARED ) 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 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 index 6845654..0837702 100644 --- a/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp +++ b/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp @@ -55,36 +55,31 @@ 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", package_name); + std::string map_path_file, map_topic_in; 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", package_name); node->get_parameter(plugin_name + ".map_path_file", map_path_file); node->get_parameter(plugin_name + ".map_topic_in", map_topic_in); std::cerr << ":: PARAMETERS :: \n"; - std::cerr << "Package name: " << package_name << "\n"; std::cerr << "Map path file: " << map_path_file << "\n"; std::cerr << "map topic in name: " << map_topic_in << "\n"; - if (package_name != "" && map_path_file != "") { - std::string pkgpath; - try { - pkgpath = ament_index_cpp::get_package_share_directory(package_name); - map_path_ = pkgpath + "/" + map_path_file; - } catch(ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); - } + if (map_path_file != "") { + map_path_ = map_path_file; if (!static_map_.load_from_file(map_path_)) { return std::unexpected("File [" + map_path_ + "] not found"); } else { RCLCPP_INFO(get_node()->get_logger(), - "PointCloudMapsManager::File read : Map loaded"); + "PointCloudMapsManager : Map loaded from file"); dynamic_map_.deep_copy(static_map_); } + } else { + map_path_ = "map.pcd"; + RCLCPP_INFO(get_node()->get_logger(), + "PointCloudMapsManager: Map path file to save: %s", map_path_.c_str()); } std::string topic_name; @@ -94,13 +89,6 @@ PointCloudMapsManager::on_initialize() topic_name = node->get_name() + std::string("/") + plugin_name + "/topic_in"; } - if (map_path_file == "") { - map_path_ = "map.PointCloudData"; - } - - RCLCPP_INFO(get_node()->get_logger(), - "PointCloudMapsManager::Map path file: %s", map_path_.c_str()); - static_map_pub_ = node->create_publisher( node->get_name() + std::string("/") + plugin_name + "/static_map", rclcpp::QoS(1).transient_local().reliable()); @@ -113,9 +101,8 @@ PointCloudMapsManager::on_initialize() [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { RCLCPP_INFO(get_node()->get_logger(), - "PointCloudMapsManager::topic_callback: reading map"); + "PointCloudMapsManager: topic_callback: reading map"); static_map_.from_point_cloud(*msg); - dynamic_map_.from_point_cloud(*msg); static_map_.to_point_cloud(static_map_msg_); static_map_msg_.header.frame_id = "map"; @@ -147,9 +134,13 @@ PointCloudMapsManager::on_initialize() 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 {}; } @@ -168,22 +159,22 @@ PointCloudMapsManager::set_dynamic_map(const PointCloudData & new_map) void PointCloudMapsManager::update(NavState & nav_state) { - RCLCPP_INFO(get_node()->get_logger(),"Updating"); - dynamic_map_.deep_copy(static_map_); - if (!nav_state.has("points")) { - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); - return; - } + dynamic_map_.deep_copy(static_map_); - const auto & perceptions = nav_state.get("points"); + if (nav_state.has("points")) { + const auto & perceptions = nav_state.get("points"); - auto fused = PointPerceptionsOpsView(perceptions) - .fuse("map")->as_points(); + auto fused = PointPerceptionsOpsView(perceptions) + .fuse("map")->as_points(); - dynamic_map_.refresh(dynamic_map_msg_, fused); + 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_); diff --git a/robot_params/maps_manager.params.yaml b/robot_params/maps_manager.params.yaml index ee5f252..559b37b 100644 --- a/robot_params/maps_manager.params.yaml +++ b/robot_params/maps_manager.params.yaml @@ -1,10 +1,9 @@ maps_manager_node: ros__parameters: use_sim_time: true - map_types: - - PointCloudData + map_types: [PointCloudData] PointCloudData: plugin: easynav_pointcloud_maps_manager/PointCloudMapsManager - package: easynav_pointcloud_maps_manager map_topic_in: /map_builder/cloud_filtered + map_path_file: /home/juanscelyg/ws/easynav_ws/src/easynav_outdoor_testcase/maps/map.pcd \ No newline at end of file From ec9c9e4c0c1a7e8dcc0c23945628c5f185d28a9b Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Tue, 15 Jul 2025 15:27:14 +0200 Subject: [PATCH 3/6] Maps Manager read and write PCD files, also by topic works --- .../PointCloudData.cpp | 2 +- .../PointCloudMapsManager.cpp | 43 +++++++++++++------ robot_params/maps_manager.params.yaml | 5 ++- 3 files changed, 34 insertions(+), 16 deletions(-) diff --git a/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp b/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp index 5e3815f..509664e 100644 --- a/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp +++ b/easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp @@ -186,7 +186,7 @@ PointCloudData::load_from_file(const std::string & path) void PointCloudData::show(const std::string & comment) const { - std::cerr << ":: PointCloudData values :: \n"; + std::cerr << ":: PointCloudData Values :: \n"; std::cerr << "Comment: " << comment << "\n"; std::cerr << "width: " << width_ << "\n"; std::cerr << "height: " << height_ << "\n"; 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 index 0837702..4fff36d 100644 --- a/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp +++ b/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp @@ -55,39 +55,55 @@ PointCloudMapsManager::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - std::string map_path_file, map_topic_in; + 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); - std::cerr << ":: PARAMETERS :: \n"; - std::cerr << "Map path file: " << map_path_file << "\n"; - std::cerr << "map topic in name: " << map_topic_in << "\n"; + RCLCPP_INFO(get_node()->get_logger(), + "PointCloud : ..:: PARAMETERS ::.."); - if (map_path_file != "") { - map_path_ = map_path_file; + 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(), - "PointCloudMapsManager : Map loaded from file"); + "PointCloud : Map loaded from file"); dynamic_map_.deep_copy(static_map_); } } else { - map_path_ = "map.pcd"; + map_path_ = "pointcloud_map.pcd"; RCLCPP_INFO(get_node()->get_logger(), - "PointCloudMapsManager: Map path file to save: %s", map_path_.c_str()); + "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", @@ -101,7 +117,7 @@ PointCloudMapsManager::on_initialize() [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { RCLCPP_INFO(get_node()->get_logger(), - "PointCloudMapsManager: topic_callback: reading map"); + "PointCloud : topic_callback : Reading Map"); static_map_.from_point_cloud(*msg); static_map_.to_point_cloud(static_map_msg_); @@ -118,12 +134,14 @@ PointCloudMapsManager::on_initialize() std::shared_ptr response) { (void)request; + if (!static_map_.save_to_file(map_path_)) { response->success = false; - response->message = "Failed to save map to: " + map_path_; + response->message = "PointCloud : Failed to save map to: " + map_path_; } else { + static_map_.show("Data to be saved"); response->success = true; - response->message = "Map successfully saved to: " + map_path_; + response->message = "PointCloud : Map successfully saved to: " + map_path_; } }); @@ -159,7 +177,6 @@ PointCloudMapsManager::set_dynamic_map(const PointCloudData & new_map) void PointCloudMapsManager::update(NavState & nav_state) { - dynamic_map_.deep_copy(static_map_); if (nav_state.has("points")) { diff --git a/robot_params/maps_manager.params.yaml b/robot_params/maps_manager.params.yaml index 559b37b..130dbd8 100644 --- a/robot_params/maps_manager.params.yaml +++ b/robot_params/maps_manager.params.yaml @@ -1,9 +1,10 @@ maps_manager_node: ros__parameters: - use_sim_time: true + use_sim_time: false map_types: [PointCloudData] PointCloudData: plugin: easynav_pointcloud_maps_manager/PointCloudMapsManager map_topic_in: /map_builder/cloud_filtered - map_path_file: /home/juanscelyg/ws/easynav_ws/src/easynav_outdoor_testcase/maps/map.pcd + package_name: easynav_outdoor_testcase + map_path_file: maps/pointcloud_map.pcd \ No newline at end of file From 1d92ed83811413e87dfdb8d1637209b418ea5765 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 16 Jul 2025 11:17:07 +0200 Subject: [PATCH 4/6] Readme update with Maps Manager instructions --- README.md | 69 +++++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 64 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 230f249..e7fecc5 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,7 +16,7 @@ The `PointcloudMapsBuilderNode` subscribes to point cloud sensor data, downsampl --- -## Installation +### Installation Clone the repository into your ROS 2 workspace: ```bash @@ -26,7 +26,7 @@ cd .. colcon build --packages-select easynav_pointcloud_maps_builder ``` -## Usage +### Usage Source your workspace: ```bash @@ -57,4 +57,63 @@ pointcloud_maps_builder_node: ``` ros2 run easynav_pointcloud_maps_builder pointcloud_maps_builder_main \ --ros-args --params-file src/easynav_pointcloud_stack/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 +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 lifecycle node: +```bash +ros2 run easynav_system system_main --ros-args --params-file /src/easynav_pointcloud_stack/robot_params/maps_manager.params.yaml +``` + +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. ``` \ No newline at end of file From 24baaaad0d18700e50f615d515ddfaaf97a41d0e Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 16 Jul 2025 16:06:31 +0200 Subject: [PATCH 5/6] robot params folder was deleted. Also readme was modified --- README.md | 26 +++++++++++++++++++------- robot_params/maps_builder.params.yaml | 10 ---------- robot_params/maps_manager.params.yaml | 10 ---------- 3 files changed, 19 insertions(+), 27 deletions(-) delete mode 100644 robot_params/maps_builder.params.yaml delete mode 100644 robot_params/maps_manager.params.yaml diff --git a/README.md b/README.md index e7fecc5..3a8d7fa 100644 --- a/README.md +++ b/README.md @@ -19,9 +19,11 @@ The `PointcloudMapsBuilderNode` subscribes to point cloud sensor data, downsampl ### 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 ``` @@ -29,13 +31,17 @@ colcon build --packages-select easynav_pointcloud_maps_builder ### 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,9 +60,10 @@ 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 @@ -77,14 +84,16 @@ By each `map_types` define in the params file is required to define the followin | `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`. +>> **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 + +``` 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 ``` @@ -92,12 +101,15 @@ colcon build --packages-select easynav_pointcloud_maps_manager easynav_pointclou ### Usage Source your workspace: + ```bash source ~/ros2_ws/install/setup.bash ``` -Run the lifecycle node: + +Run the main node: + ```bash -ros2 run easynav_system system_main --ros-args --params-file /src/easynav_pointcloud_stack/robot_params/maps_manager.params.yaml +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. @@ -116,4 +128,4 @@ maps_manager_node: 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. -``` \ No newline at end of file +``` diff --git a/robot_params/maps_builder.params.yaml b/robot_params/maps_builder.params.yaml deleted file mode 100644 index 55469d1..0000000 --- a/robot_params/maps_builder.params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -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 \ No newline at end of file diff --git a/robot_params/maps_manager.params.yaml b/robot_params/maps_manager.params.yaml deleted file mode 100644 index 130dbd8..0000000 --- a/robot_params/maps_manager.params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -maps_manager_node: - ros__parameters: - use_sim_time: false - map_types: [PointCloudData] - PointCloudData: - plugin: easynav_pointcloud_maps_manager/PointCloudMapsManager - map_topic_in: /map_builder/cloud_filtered - package_name: easynav_outdoor_testcase - map_path_file: maps/pointcloud_map.pcd - \ No newline at end of file From 7ec4dbbbe3850a347f636d794f9ec830f6eea51f Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 18 Jul 2025 11:31:41 +0200 Subject: [PATCH 6/6] Map file path by default have been changed --- .../easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp | 2 +- .../easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) 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 index 576c5d9..64903aa 100644 --- a/easynav_pointcloud_maps_manager/include/easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp +++ b/easynav_pointcloud_maps_manager/include/easynav_pointcloud_maps_manager/PointCloudMapsManager.hpp @@ -107,7 +107,7 @@ class PointCloudMapsManager : public easynav::MapsManagerBase /** * @brief Full path to the map file. */ - std::string map_path_; + std::string map_path_ {"/tmp/map.pcd"}; private: /** 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 index 4fff36d..5f9d1f3 100644 --- a/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp +++ b/easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp @@ -88,7 +88,6 @@ PointCloudMapsManager::on_initialize() dynamic_map_.deep_copy(static_map_); } } else { - map_path_ = "pointcloud_map.pcd"; RCLCPP_INFO(get_node()->get_logger(), "PointCloud : Path Map file to save = ./%s", map_path_.c_str()); }