From a181ab9b75efabd1bbcbd62c7bab32bb0e400dbf Mon Sep 17 00:00:00 2001 From: anhnv3991 Date: Fri, 5 Apr 2024 08:46:51 +0700 Subject: [PATCH] Keep timeout only Signed-off-by: anhnv3991 --- .../ndt_scan_matcher/map_update_module.hpp | 5 -- .../src/map_update_module.cpp | 83 +------------------ .../differential_map_loader_module.cpp | 20 ----- 3 files changed, 3 insertions(+), 105 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 d91a539f69021..2eb14b6004282 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 @@ -60,11 +60,6 @@ class MapUpdateModule [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position); void publish_partial_pcd_map(); - // Looking for PCDs in the vicinity of the specified location - void query_pcds(double px, double py, double radius, std::unordered_set& maps_to_add, std::unordered_set& maps_to_remove); - // Compute distance between an origin and a pcd segment - double dist(double px, double py, int idx, int idy); - 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 18a2aec2efa45..9bb32b69dcad5 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -146,13 +146,8 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt request, [](rclcpp::Client::SharedFuture) {})}; - // While waiting for the requested pcds, look for indices of the candidate pcds - std::unordered_set pcds_to_remove, pcds_to_add; - - query_pcds(position.x, position.y, param_.map_radius, pcds_to_add, pcds_to_remove); - - // Wait for maximum 20 milliseconds - std::chrono::milliseconds timeout(20); + // Wait for maximum 10 milliseconds + std::chrono::milliseconds timeout(10); auto start = std::chrono::system_clock::now(); std::future_status status = result.wait_for(std::chrono::seconds(0)); @@ -177,23 +172,7 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt auto & maps_to_add = result.get()->new_pointcloud_with_ids; auto & map_ids_to_remove = result.get()->ids_to_remove; - // Check if there are any maps in the pcds_to_add not coming - for (auto& name : pcds_to_add) - { - if (maps_to_add.find(name) == maps_to_add.end()) - { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "File %s not coming.", name.c_str()); - } - } - - for (auto& name : pcds_to_remove) - { - if (map_ids_to_remove.find(name) == map_ids_to_remove.end()) - { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "File %s should be removed but it was not.", name.c_str()); - } - } - + // Check 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()) { @@ -236,59 +215,3 @@ void MapUpdateModule::publish_partial_pcd_map() loaded_pcd_pub_->publish(map_msg); } - -void MapUpdateModuble::query_pcds(double px, double py, double radius, std::unordered_set& maps_to_add, std::unordered_set& maps_to_remove) -{ - int lower_x = static_cast(floor((px - radius) / pcd_res_x_)); - int lower_y = static_cast(floor((py - radius) / pcd_res_y_)); - int upper_x = static_cast(floor((px + radius) / pcd_res_x_)); - int upper_y = static_cast(floor((py + radius) / pcd_res_y_)); - double rel_px = px - pcd_lower_x_, rel_py = py - pcd_lower_y_; - std::unordered_set nn_pcds; - - for (int idx = lower_x; idx < upper_x; ++idx) - { - for (int idy = lower_y; idy < upper_y; ++idy) - { - // Skip if the pcd file is not within the query radius - if (dist(rel_px, rel_py, idx, idy) > radius) - { - continue; - } - - // Generate name of the PCD file - std::string name = pcd_tag_ + "_" + std::to_string(x) + "_" + std::to_string(y) + ".pcd"; - - if (cur_pcds_.find(name) == cur_pcds_.end()) - { - maps_to_add.insert(name); - } - else - { - nn_pcds.insert(name); - } - } - } - - // Compare with @cur_pcds_ to find out which pcds to remove - for (auto& name : cur_pcds_) - { - if (nn_pcds.find(name) == nn_pcds.end()) - { - map_ids_to_remove.insert(name); - } - } -} - -double MapUpdateModule::dist(double px, double py, int idx, int idy) -{ - double lx = idx * pcd_res_x_; - double ly = idy * pcd_res_y_; - double ux = lx + pcd_res_x_; - double uy = ly + pcd_res_y_; - - double dx = (px < lx) ? lx - px : ((px >= ux) ? px - ux : 0); - double dy = (py < ly) ? ly - py : ((py >= uy) ? py - uy : 0); - - return std::hypot(dx, dy); -} \ No newline at end of file diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 00d6bfbee5df1..a8d380fd81b86 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -29,13 +29,6 @@ void DifferentialMapLoaderModule::differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo & area, const std::vector & cached_ids, GetDifferentialPointCloudMap::Response::SharedPtr & response) const { - - // For debug - std::ofstream test("/home/anh/Work/autoware/test.txt", std::ios::app); - - test << __FILE__ << "::" << __LINE__ << "::" << __func__ << std::endl; - // End - // iterate over all the available pcd map grids std::vector should_remove(static_cast(cached_ids.size()), true); for (const auto & ele : all_pcd_file_metadata_dict_) { @@ -74,12 +67,6 @@ bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( GetDifferentialPointCloudMap::Request::SharedPtr req, GetDifferentialPointCloudMap::Response::SharedPtr res) { - // For debug - std::ofstream test("/home/anh/Work/autoware/test.txt", std::ios::app); - - test << __FILE__ << "::" << __LINE__ << "::" << __func__ << std::endl; - // End - auto area = req->area; std::vector cached_ids = req->cached_ids; differentialAreaLoad(area, cached_ids, res); @@ -91,13 +78,6 @@ autoware_map_msgs::msg::PointCloudMapCellWithID DifferentialMapLoaderModule::loadPointCloudMapCellWithID( const std::string & path, const std::string & map_id) const { - - // For debug - std::ofstream test("/home/anh/Work/autoware/test.txt", std::ios::app); - - test << __FILE__ << "::" << __LINE__ << "::" << __func__ << std::endl; - // End - sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);