From 75c13e2c3dbc83c9262787350b275c93cc36ed39 Mon Sep 17 00:00:00 2001 From: anhnv3991 Date: Tue, 13 Feb 2024 20:25:57 +0700 Subject: [PATCH 1/8] Fixed conflicts Signed-off-by: anhnv3991 --- .../ndt_scan_matcher/map_update_module.hpp | 21 +++- .../src/map_update_module.cpp | 95 +++++++++++++++---- 2 files changed, 93 insertions(+), 23 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 157421fc3ccb1..668a7b6a6e6c6 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -37,6 +37,7 @@ #include #include #include +#include class MapUpdateModule { @@ -44,36 +45,46 @@ class MapUpdateModule using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = pclomp::MultiGridNormalDistributionsTransform; + using NdtPtrType = std::shared_ptr; public: MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, + std::shared_ptr& ndt_ptr, HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; - void update_ndt( + // Update the specified NDT + void update_ndt(NdtPtrType& ndt, const std::vector & maps_to_add, const std::vector & map_ids_to_remove); void update_map(const geometry_msgs::msg::Point & position); - [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position) const; + [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position); void publish_partial_pcd_map(); + // Pre-fetch NDT map in a child thread + void prefetch_map(const geometry_msgs::msg::Point& position, NdtPtrType& ndt); + rclcpp::Publisher::SharedPtr loaded_pcd_pub_; rclcpp::Client::SharedPtr pcd_loader_client_; - std::shared_ptr ndt_ptr_; - std::mutex * ndt_ptr_mutex_; + // TODO: Replace pointer by a reference to pointer + NdtPtrType& ndt_ptr_; + std::mutex* ndt_ptr_mutex_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::optional last_update_position_ = std::nullopt; HyperParameters::DynamicMapLoading param_; + + // Indicate if there is a prefetch thread waiting for being collected + NdtPtrType secondary_ndt_ptr_; + bool rebuild_; }; #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 402ec9da32782..d95369c393723 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -16,8 +16,8 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, HyperParameters::DynamicMapLoading param) -: ndt_ptr_(std::move(ndt_ptr)), + std::shared_ptr& ndt_ptr, HyperParameters::DynamicMapLoading param) +: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), clock_(node->get_clock()), @@ -28,29 +28,50 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client("pcd_loader_service"); + + secondary_ndt_ptr_.reset(new NormalDistributionsTransform); + + if (ndt_ptr_) + { + *secondary_ndt_ptr_ = *ndt_ptr_; + } + + // Initially, a direct map update on ndt_ptr_ is needed. + // ndt_ptr_'s mutex is locked until it is fully rebuilt. + // From the second update, the update is done on secondary_ndt_ptr_, + // and ndt_ptr_ is only locked when swapping its pointer with + // secondary_ndt_ptr_. + rebuild_ = true; } -bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) { - if (last_update_position_ == std::nullopt) { + if (last_update_position_ == std::nullopt) + { return false; } + const double dx = position.x - last_update_position_.value().x; const double dy = position.y - last_update_position_.value().y; const double distance = std::hypot(dx, dy); if (distance + param_.lidar_radius > param_.map_radius) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); + // If the map does not keep up with the current position, + // lock ndt_ptr_ entirely until it is fully rebuilt. + rebuild_ = true; } + return distance > param_.update_distance; } -void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) +void MapUpdateModule::prefetch_map(const geometry_msgs::msg::Point& position, NdtPtrType& ndt) { auto request = std::make_shared(); + request->area.center_x = static_cast(position.x); request->area.center_y = static_cast(position.y); request->area.radius = static_cast(param_.map_radius); - request->cached_ids = ndt_ptr_->getCurrentMapIDs(); + request->cached_ids = ndt->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); @@ -69,11 +90,53 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) } status = result.wait_for(std::chrono::seconds(1)); } - update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); + update_ndt(ndt, result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); +} + +void MapUpdateModule::update_map(const geometry_msgs::msg::Point& position) +{ + // If the current position is super far from the previous loading position, + // lock and rebuild ndt_ptr_ + if (rebuild_) + { + ndt_ptr_mutex_->lock(); + auto param = ndt_ptr_->getParams(); + + ndt_ptr_.reset(new NormalDistributionsTransform); + + ndt_ptr_->setParams(param); + + prefetch_map(position, ndt_ptr_); + ndt_ptr_mutex_->unlock(); + rebuild_ = false; + } + else + { + // Load map to the secondary_ndt_ptr, which does not require a mutex lock + // Since the update of the secondary ndt ptr and the NDT align (done on + // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly. + // If the updating is done the main ndt_ptr_, either the update or the NDT + // align will be blocked by the other. + prefetch_map(position, secondary_ndt_ptr_); + + ndt_ptr_mutex_->lock(); + auto input_source = ndt_ptr_->getInputSource(); + ndt_ptr_ = secondary_ndt_ptr_; + ndt_ptr_->setInputSource(input_source); + ndt_ptr_mutex_->unlock(); + } + + secondary_ndt_ptr_.reset(new NormalDistributionsTransform); + *secondary_ndt_ptr_ = *ndt_ptr_; + + // Memorize the position of the last update last_update_position_ = position; + + // Publish the new ndt maps + publish_partial_pcd_map(); } -void MapUpdateModule::update_ndt( +void MapUpdateModule::update_ndt(NdtPtrType& ndt, const std::vector & maps_to_add, const std::vector & map_ids_to_remove) { @@ -94,38 +157,34 @@ void MapUpdateModule::update_ndt( pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); } - (*ndt_ptr_mutex_).lock(); - // Add pcd for (size_t i = 0; i < add_size; i++) { - ndt_ptr_->addTarget(points_pcl[i], maps_to_add[i].cell_id); + ndt->addTarget(points_pcl[i], maps_to_add[i].cell_id); } // Remove pcd for (const std::string & map_id_to_remove : map_ids_to_remove) { - ndt_ptr_->removeTarget(map_id_to_remove); + ndt->removeTarget(map_id_to_remove); } - ndt_ptr_->createVoxelKdtree(); - - (*ndt_ptr_mutex_).unlock(); + ndt->createVoxelKdtree(); const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); - - publish_partial_pcd_map(); } +int count = 0; + void MapUpdateModule::publish_partial_pcd_map() { pcl::PointCloud map_pcl = ndt_ptr_->getVoxelPCD(); - sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(map_pcl, map_msg); map_msg.header.frame_id = "map"; loaded_pcd_pub_->publish(map_msg); } + From 1308d3c741d8b932463556ff3dee179abaeaeaca Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 13 Feb 2024 13:45:35 +0000 Subject: [PATCH 2/8] style(pre-commit): autofix --- .../ndt_scan_matcher/map_update_module.hpp | 13 ++++--- .../src/map_update_module.cpp | 37 ++++++++----------- 2 files changed, 23 insertions(+), 27 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 668a7b6a6e6c6..32e6b9bfa0d4e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -36,8 +36,8 @@ #include #include #include -#include #include +#include class MapUpdateModule { @@ -50,14 +50,15 @@ class MapUpdateModule public: MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr& ndt_ptr, + std::shared_ptr & ndt_ptr, HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; // Update the specified NDT - void update_ndt(NdtPtrType& ndt, + void update_ndt( + NdtPtrType & ndt, const std::vector & maps_to_add, const std::vector & map_ids_to_remove); void update_map(const geometry_msgs::msg::Point & position); @@ -65,7 +66,7 @@ class MapUpdateModule void publish_partial_pcd_map(); // Pre-fetch NDT map in a child thread - void prefetch_map(const geometry_msgs::msg::Point& position, NdtPtrType& ndt); + void prefetch_map(const geometry_msgs::msg::Point & position, NdtPtrType & ndt); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; @@ -73,8 +74,8 @@ class MapUpdateModule pcd_loader_client_; // TODO: Replace pointer by a reference to pointer - NdtPtrType& ndt_ptr_; - std::mutex* ndt_ptr_mutex_; + NdtPtrType & ndt_ptr_; + std::mutex * ndt_ptr_mutex_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index d95369c393723..34a3bf4dca751 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -16,7 +16,7 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr& ndt_ptr, HyperParameters::DynamicMapLoading param) + std::shared_ptr & ndt_ptr, HyperParameters::DynamicMapLoading param) : ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), @@ -31,8 +31,7 @@ MapUpdateModule::MapUpdateModule( secondary_ndt_ptr_.reset(new NormalDistributionsTransform); - if (ndt_ptr_) - { + if (ndt_ptr_) { *secondary_ndt_ptr_ = *ndt_ptr_; } @@ -40,14 +39,13 @@ MapUpdateModule::MapUpdateModule( // ndt_ptr_'s mutex is locked until it is fully rebuilt. // From the second update, the update is done on secondary_ndt_ptr_, // and ndt_ptr_ is only locked when swapping its pointer with - // secondary_ndt_ptr_. + // secondary_ndt_ptr_. rebuild_ = true; } -bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) { - if (last_update_position_ == std::nullopt) - { + if (last_update_position_ == std::nullopt) { return false; } @@ -64,7 +62,7 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi return distance > param_.update_distance; } -void MapUpdateModule::prefetch_map(const geometry_msgs::msg::Point& position, NdtPtrType& ndt) +void MapUpdateModule::prefetch_map(const geometry_msgs::msg::Point & position, NdtPtrType & ndt) { auto request = std::make_shared(); @@ -93,32 +91,29 @@ void MapUpdateModule::prefetch_map(const geometry_msgs::msg::Point& position, Nd update_ndt(ndt, result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); } -void MapUpdateModule::update_map(const geometry_msgs::msg::Point& position) +void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { // If the current position is super far from the previous loading position, // lock and rebuild ndt_ptr_ - if (rebuild_) - { + if (rebuild_) { ndt_ptr_mutex_->lock(); auto param = ndt_ptr_->getParams(); ndt_ptr_.reset(new NormalDistributionsTransform); ndt_ptr_->setParams(param); - - prefetch_map(position, ndt_ptr_); + + prefetch_map(position, ndt_ptr_); ndt_ptr_mutex_->unlock(); rebuild_ = false; - } - else - { + } else { // Load map to the secondary_ndt_ptr, which does not require a mutex lock - // Since the update of the secondary ndt ptr and the NDT align (done on + // Since the update of the secondary ndt ptr and the NDT align (done on // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly. - // If the updating is done the main ndt_ptr_, either the update or the NDT + // If the updating is done the main ndt_ptr_, either the update or the NDT // align will be blocked by the other. prefetch_map(position, secondary_ndt_ptr_); - + ndt_ptr_mutex_->lock(); auto input_source = ndt_ptr_->getInputSource(); ndt_ptr_ = secondary_ndt_ptr_; @@ -136,7 +131,8 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point& position) publish_partial_pcd_map(); } -void MapUpdateModule::update_ndt(NdtPtrType& ndt, +void MapUpdateModule::update_ndt( + NdtPtrType & ndt, const std::vector & maps_to_add, const std::vector & map_ids_to_remove) { @@ -187,4 +183,3 @@ void MapUpdateModule::publish_partial_pcd_map() loaded_pcd_pub_->publish(map_msg); } - From b48e1086b125739214c051fcaa888663fc598890 Mon Sep 17 00:00:00 2001 From: anhnv3991 Date: Tue, 13 Feb 2024 20:50:53 +0700 Subject: [PATCH 3/8] Remove unnecessary TODO Signed-off-by: anhnv3991 --- .../include/ndt_scan_matcher/map_update_module.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 32e6b9bfa0d4e..b1fb406e8daa3 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -73,7 +73,6 @@ class MapUpdateModule rclcpp::Client::SharedPtr pcd_loader_client_; - // TODO: Replace pointer by a reference to pointer NdtPtrType & ndt_ptr_; std::mutex * ndt_ptr_mutex_; rclcpp::Logger logger_; From f06d431ae7fe4dc4228ff1868f5db159fcc12b56 Mon Sep 17 00:00:00 2001 From: anhnv3991 Date: Wed, 14 Feb 2024 21:46:40 +0700 Subject: [PATCH 4/8] Removed unused debug variable and rename rebuild_ to need_rebuild_ for better understanding Signed-off-by: anhnv3991 --- .../include/ndt_scan_matcher/map_update_module.hpp | 2 +- .../ndt_scan_matcher/src/map_update_module.cpp | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index b1fb406e8daa3..cd9b71467ae89 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -84,7 +84,7 @@ class MapUpdateModule // Indicate if there is a prefetch thread waiting for being collected NdtPtrType secondary_ndt_ptr_; - bool rebuild_; + bool need_rebuild_; }; #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 34a3bf4dca751..5d2012ef360a2 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -40,7 +40,7 @@ MapUpdateModule::MapUpdateModule( // From the second update, the update is done on secondary_ndt_ptr_, // and ndt_ptr_ is only locked when swapping its pointer with // secondary_ndt_ptr_. - rebuild_ = true; + need_rebuild_ = true; } bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) @@ -56,7 +56,7 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); // If the map does not keep up with the current position, // lock ndt_ptr_ entirely until it is fully rebuilt. - rebuild_ = true; + need_rebuild_ = true; } return distance > param_.update_distance; @@ -95,7 +95,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { // If the current position is super far from the previous loading position, // lock and rebuild ndt_ptr_ - if (rebuild_) { + if (need_rebuild_) { ndt_ptr_mutex_->lock(); auto param = ndt_ptr_->getParams(); @@ -105,7 +105,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) prefetch_map(position, ndt_ptr_); ndt_ptr_mutex_->unlock(); - rebuild_ = false; + need_rebuild_ = false; } else { // Load map to the secondary_ndt_ptr, which does not require a mutex lock // Since the update of the secondary ndt ptr and the NDT align (done on @@ -172,8 +172,6 @@ void MapUpdateModule::update_ndt( RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); } -int count = 0; - void MapUpdateModule::publish_partial_pcd_map() { pcl::PointCloud map_pcl = ndt_ptr_->getVoxelPCD(); From 27ae11af0e2e679cd1f5b23e0d2ddc28b0f70389 Mon Sep 17 00:00:00 2001 From: anhnv3991 Date: Mon, 19 Feb 2024 09:44:53 +0700 Subject: [PATCH 5/8] Replace pointer to ndt by reference, output error when the input ndt_ptr is null Signed-off-by: anhnv3991 --- .../ndt_scan_matcher/map_update_module.hpp | 16 +--- .../src/map_update_module.cpp | 85 +++++++++---------- 2 files changed, 45 insertions(+), 56 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index cd9b71467ae89..38b5a932ae91d 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -43,31 +43,23 @@ class MapUpdateModule { using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; - using NdtPtrType = std::shared_ptr; + using NdtType = pclomp::MultiGridNormalDistributionsTransform; + using NdtPtrType = std::shared_ptr; public: MapUpdateModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr & ndt_ptr, + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr, HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; // Update the specified NDT - void update_ndt( - NdtPtrType & ndt, - const std::vector & maps_to_add, - const std::vector & map_ids_to_remove); + void update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt); void update_map(const geometry_msgs::msg::Point & position); [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position); void publish_partial_pcd_map(); - // Pre-fetch NDT map in a child thread - void prefetch_map(const geometry_msgs::msg::Point & position, NdtPtrType & ndt); - rclcpp::Publisher::SharedPtr loaded_pcd_pub_; rclcpp::Client::SharedPtr diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 5d2012ef360a2..e300118bfc84f 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -16,7 +16,7 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr & ndt_ptr, HyperParameters::DynamicMapLoading param) + NdtPtrType & ndt_ptr, HyperParameters::DynamicMapLoading param) : ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), @@ -29,9 +29,10 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client("pcd_loader_service"); - secondary_ndt_ptr_.reset(new NormalDistributionsTransform); + secondary_ndt_ptr_.reset(new NdtType); if (ndt_ptr_) { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); *secondary_ndt_ptr_ = *ndt_ptr_; } @@ -62,35 +63,6 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi return distance > param_.update_distance; } -void MapUpdateModule::prefetch_map(const geometry_msgs::msg::Point & position, NdtPtrType & ndt) -{ - auto request = std::make_shared(); - - request->area.center_x = static_cast(position.x); - request->area.center_y = static_cast(position.y); - request->area.radius = static_cast(param_.map_radius); - request->cached_ids = ndt->getCurrentMapIDs(); - - while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); - } - - // send a request to map_loader - auto result{pcd_loader_client_->async_send_request( - request, - [](rclcpp::Client::SharedFuture) {})}; - - std::future_status status = result.wait_for(std::chrono::seconds(0)); - while (status != std::future_status::ready) { - RCLCPP_INFO(logger_, "waiting response"); - if (!rclcpp::ok()) { - return; - } - status = result.wait_for(std::chrono::seconds(1)); - } - update_ndt(ndt, result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); -} - void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { // If the current position is super far from the previous loading position, @@ -99,11 +71,11 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) ndt_ptr_mutex_->lock(); auto param = ndt_ptr_->getParams(); - ndt_ptr_.reset(new NormalDistributionsTransform); + ndt_ptr_.reset(new NdtType); ndt_ptr_->setParams(param); - prefetch_map(position, ndt_ptr_); + update_ndt(position, *ndt_ptr_); ndt_ptr_mutex_->unlock(); need_rebuild_ = false; } else { @@ -112,7 +84,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly. // If the updating is done the main ndt_ptr_, either the update or the NDT // align will be blocked by the other. - prefetch_map(position, secondary_ndt_ptr_); + update_ndt(position, *secondary_ndt_ptr_); ndt_ptr_mutex_->lock(); auto input_source = ndt_ptr_->getInputSource(); @@ -121,7 +93,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) ndt_ptr_mutex_->unlock(); } - secondary_ndt_ptr_.reset(new NormalDistributionsTransform); + secondary_ndt_ptr_.reset(new NdtType); *secondary_ndt_ptr_ = *ndt_ptr_; // Memorize the position of the last update @@ -131,23 +103,48 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) publish_partial_pcd_map(); } -void MapUpdateModule::update_ndt( - NdtPtrType & ndt, - const std::vector & maps_to_add, - const std::vector & map_ids_to_remove) +void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt) { + auto request = std::make_shared(); + + request->area.center_x = static_cast(position.x); + request->area.center_y = static_cast(position.y); + request->area.radius = static_cast(param_.map_radius); + request->cached_ids = ndt.getCurrentMapIDs(); + + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); + } + + // send a request to map_loader + auto result{pcd_loader_client_->async_send_request( + request, + [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(logger_, "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } + + auto& maps_to_add = result.get()->new_pointcloud_with_ids; + auto& map_ids_to_remove = result.get()->ids_to_remove; + RCLCPP_INFO( logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); if (maps_to_add.empty() && map_ids_to_remove.empty()) { RCLCPP_INFO(logger_, "Skip map update"); return; } - const auto exe_start_time = std::chrono::system_clock::now(); + const auto exe_start_time = std::chrono::system_clock::now(); const size_t add_size = maps_to_add.size(); - // Perform heavy processing outside of the lock scope std::vector>> points_pcl(add_size); + for (size_t i = 0; i < add_size; i++) { points_pcl[i] = pcl::make_shared>(); pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); @@ -155,15 +152,15 @@ void MapUpdateModule::update_ndt( // Add pcd for (size_t i = 0; i < add_size; i++) { - ndt->addTarget(points_pcl[i], maps_to_add[i].cell_id); + ndt.addTarget(points_pcl[i], maps_to_add[i].cell_id); } // Remove pcd for (const std::string & map_id_to_remove : map_ids_to_remove) { - ndt->removeTarget(map_id_to_remove); + ndt.removeTarget(map_id_to_remove); } - ndt->createVoxelKdtree(); + ndt.createVoxelKdtree(); const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = From 024b72e7502d51e3452aa052feec66b840a6a39e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 19 Feb 2024 02:46:47 +0000 Subject: [PATCH 6/8] style(pre-commit): autofix --- .../ndt_scan_matcher/src/map_update_module.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index e300118bfc84f..7e48c0be4c539 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -15,8 +15,8 @@ #include "ndt_scan_matcher/map_update_module.hpp" MapUpdateModule::MapUpdateModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - NdtPtrType & ndt_ptr, HyperParameters::DynamicMapLoading param) + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr, + HyperParameters::DynamicMapLoading param) : ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), @@ -32,7 +32,7 @@ MapUpdateModule::MapUpdateModule( secondary_ndt_ptr_.reset(new NdtType); if (ndt_ptr_) { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); *secondary_ndt_ptr_ = *ndt_ptr_; } @@ -130,8 +130,8 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt status = result.wait_for(std::chrono::seconds(1)); } - auto& maps_to_add = result.get()->new_pointcloud_with_ids; - auto& map_ids_to_remove = result.get()->ids_to_remove; + auto & maps_to_add = result.get()->new_pointcloud_with_ids; + auto & map_ids_to_remove = result.get()->ids_to_remove; RCLCPP_INFO( logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); From 0a077533c1c17c9fcd3d410132f902a199881fc4 Mon Sep 17 00:00:00 2001 From: anhnv3991 Date: Mon, 19 Feb 2024 12:31:52 +0700 Subject: [PATCH 7/8] Fix error message Signed-off-by: anhnv3991 --- localization/ndt_scan_matcher/src/map_update_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index e300118bfc84f..b67bc60b55876 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -32,9 +32,12 @@ MapUpdateModule::MapUpdateModule( secondary_ndt_ptr_.reset(new NdtType); if (ndt_ptr_) { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); *secondary_ndt_ptr_ = *ndt_ptr_; } + else + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); + } // Initially, a direct map update on ndt_ptr_ is needed. // ndt_ptr_'s mutex is locked until it is fully rebuilt. From 2a2ba52805bbf2908377a5e082d6d676cce8bb4e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 19 Feb 2024 05:34:48 +0000 Subject: [PATCH 8/8] style(pre-commit): autofix --- localization/ndt_scan_matcher/src/map_update_module.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index c00fb3b3ba67e..17bafcfedfe80 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -33,9 +33,7 @@ MapUpdateModule::MapUpdateModule( if (ndt_ptr_) { *secondary_ndt_ptr_ = *ndt_ptr_; - } - else - { + } else { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); }