Initial commit

Signed-off-by: anhnv3991 <[email protected]>

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -60,6 +60,11 @@ 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
@@ -72,6 +77,13 @@ class MapUpdateModule
 
   std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
 
+  // Metadata of segmented PCDs
+  std::unordered_set<std::string> all_pcds_;
+  std::unordered_set<std::string> cur_pcds_;
+  double pcd_res_x_, pcd_res_y_;
+  double pcd_lower_x_, pcd_lower_y_;
+  std::string pcd_tag_;
+
   HyperParameters::DynamicMapLoading param_;
 
   // Indicate if there is a prefetch thread waiting for being collected

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -146,18 +146,54 @@ 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);
+  auto start = std::chrono::system_clock::now();
+
   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 false;  // No update
     }
+
+    auto cur = std::chrono::system_clock::now();
+
+    // Report an error if wait for too long
+    if (cur - start >= timeout)
+    {
+      RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Waited for incoming PCDs for too long. Abandon NDT update.");
+      return false;
+    }
+
     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;
 
+  // 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());
+    }
+  }
+
   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()) {
@@ -200,3 +236,59 @@ 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,6 +29,13 @@ 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_) {
@@ -67,6 +74,12 @@ 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);
@@ -78,6 +91,13 @@ 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);