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