Keep timeout only

Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp>

Author: anhnv3991

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -60,13 +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<std::string> & maps_to_add,
-    std::unordered_set<std::string> & maps_to_remove);
-  // Compute distance between an origin and a pcd segment
-  double dist(double px, double py, int idx, int idy);
-
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr loaded_pcd_pub_;
 
   rclcpp::Client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedPtr

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<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedFuture) {})};
 
-  // While waiting for the requested pcds, look for indices of the candidate pcds
-  std::unordered_set<std::string> 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,20 +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()) {
@@ -233,53 +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<std::string> & maps_to_add,
-  std::unordered_set<std::string> & maps_to_remove)
-{
-  int lower_x = static_cast<int>(floor((px - radius) / pcd_res_x_));
-  int lower_y = static_cast<int>(floor((py - radius) / pcd_res_y_));
-  int upper_x = static_cast<int>(floor((px + radius) / pcd_res_x_));
-  int upper_y = static_cast<int>(floor((py + radius) / pcd_res_y_));
-  double rel_px = px - pcd_lower_x_, rel_py = py - pcd_lower_y_;
-  std::unordered_set<std::string> 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);
-}

map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp

@@ -29,12 +29,6 @@ void DifferentialMapLoaderModule::differentialAreaLoad(
   const autoware_map_msgs::msg::AreaInfo & area, const std::vector<std::string> & 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<bool> should_remove(static_cast<int>(cached_ids.size()), true);
   for (const auto & ele : all_pcd_file_metadata_dict_) {
@@ -73,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<std::string> cached_ids = req->cached_ids;
   differentialAreaLoad(area, cached_ids, res);
@@ -90,12 +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);