refactor(ndt_scan_matcher, map_loader): remove map_module

localization/ndt_scan_matcher/CMakeLists.txt

@@ -30,7 +30,6 @@ ament_auto_add_executable(ndt_scan_matcher
   src/particle.cpp
   src/ndt_scan_matcher_node.cpp
   src/ndt_scan_matcher_core.cpp
-  src/map_module.cpp
   src/map_update_module.cpp
 )
 

localization/ndt_scan_matcher/README.md

@@ -213,20 +213,15 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma
 
 ### Parameters
 
-| Name                                  | Type   | Description                                                          |
-| ------------------------------------- | ------ | -------------------------------------------------------------------- |
-| `use_dynamic_map_loading`             | bool   | Flag to enable dynamic map loading feature for NDT (TRUE by default) |
-| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s)                                 |
-| `dynamic_map_loading_map_radius`      | double | Map loading radius for every update                                  |
-| `lidar_radius`                        | double | LiDAR radius used for localization (only used for diagnosis)         |
+| Name                                  | Type   | Description                                                  |
+| ------------------------------------- | ------ | ------------------------------------------------------------ |
+| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s)                         |
+| `dynamic_map_loading_map_radius`      | double | Map loading radius for every update                          |
+| `lidar_radius`                        | double | LiDAR radius used for localization (only used for diagnosis) |
 
 ### Enabling the dynamic map loading feature
 
-To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node.
-Follow the next two instructions.
-
-1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package)
-2. split the PCD files into grids (recommended size: 20[m] x 20[m])
+To use dynamic map loading feature for `ndt_scan_matcher`, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m])
 
 Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of
 
@@ -235,14 +230,10 @@ Note that the dynamic map loading may FAIL if the map is split into two or more
 
 Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip)
 
-|   PCD files    | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) |
-| :------------: | :-----------------------: | :------------------------: | :------------------: |
-|  single file   |           true            |            true            |  at once (standard)  |
-|  single file   |           true            |           false            |  **does NOT work**   |
-|  single file   |           false           |         true/false         |  at once (standard)  |
-| multiple files |           true            |            true            |     dynamically      |
-| multiple files |           true            |           false            |  **does NOT work**   |
-| multiple files |           false           |         true/false         |  at once (standard)  |
+|   PCD files    | How NDT loads map(s) |
+| :------------: | :------------------: |
+|  single file   |  at once (standard)  |
+| multiple files |     dynamically      |
 
 ## Scan matching score based on de-grounded LiDAR scan
 

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -1,8 +1,5 @@
 /**:
   ros__parameters:
-    # Use dynamic map loading
-    use_dynamic_map_loading: true
-
     # Vehicle reference frame
     base_frame: "base_link"
 

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -19,7 +19,6 @@
 
 #include "localization_util/smart_pose_buffer.hpp"
 #include "localization_util/tf2_listener_module.hpp"
-#include "ndt_scan_matcher/map_module.hpp"
 #include "ndt_scan_matcher/map_update_module.hpp"
 
 #include <rclcpp/rclcpp.hpp>
@@ -215,16 +214,13 @@ class NDTScanMatcher : public rclcpp::Node
 
   std::atomic<bool> is_activated_;
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
-  std::unique_ptr<MapModule> map_module_;
   std::unique_ptr<MapUpdateModule> map_update_module_;
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
   // cspell: ignore degrounded
   bool estimate_scores_for_degrounded_scan_;
   double z_margin_for_ground_removal_;
 
-  bool use_dynamic_map_loading_;
-
   // The execution time which means probably NDT cannot matches scans properly
   int64_t critical_upper_bound_exe_time_ms_;
 };

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -57,10 +57,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
   request->cached_ids = ndt_ptr_->getCurrentMapIDs();
 
   while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
-    RCLCPP_INFO(
-      logger_,
-      "Waiting for pcd loader service. Check if the enable_differential_load in "
-      "pointcloud_map_loader is set `true`.");
+    RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader.");
   }
 
   // send a request to map_loader

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -294,12 +294,7 @@
 
   tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);
 
-  use_dynamic_map_loading_ = this->declare_parameter<bool>("use_dynamic_map_loading");
-  if (use_dynamic_map_loading_) {
-    map_update_module_ = std::make_unique<MapUpdateModule>(this, &ndt_ptr_mtx_, ndt_ptr_);
-  } else {
-    map_module_ = std::make_unique<MapModule>(this, &ndt_ptr_mtx_, ndt_ptr_, sensor_callback_group);
-  }
+  map_update_module_ = std::make_unique<MapUpdateModule>(this, &ndt_ptr_mtx_, ndt_ptr_);
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 }
@@ -377,9 +372,6 @@
   if (!is_activated_) {
     return;
   }
-  if (!use_dynamic_map_loading_) {
-    return;
-  }
   std::lock_guard<std::mutex> lock(latest_ekf_position_mtx_);
   if (latest_ekf_position_ == std::nullopt) {
     RCLCPP_ERROR_STREAM_THROTTLE(
@@ -410,10 +402,8 @@
         << ". Please check the frame_id in the input topic and ensure it is correct.");
   }
 
-  if (use_dynamic_map_loading_) {
-    std::lock_guard<std::mutex> lock(latest_ekf_position_mtx_);
-    latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position;
-  }
+  std::lock_guard<std::mutex> lock(latest_ekf_position_mtx_);
+  latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position;
 }
 
 void NDTScanMatcher::callback_regularization_pose(
@@ -874,9 +864,7 @@
   // transform pose_frame to map_frame
   const auto initial_pose_msg_in_map_frame =
     transform(req->pose_with_covariance, *tf_pose_to_map_ptr);
-  if (use_dynamic_map_loading_) {
-    map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position);
-  }
+  map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position);
 
   // mutex Map
   std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);

map/map_loader/README.md

@@ -20,7 +20,7 @@ NOTE: **We strongly recommend to use divided maps when using large pointcloud ma
 
 #### Prerequisites on pointcloud map file(s)
 
-You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules:
+You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules:
 
 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md).
 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines.
@@ -29,8 +29,6 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi
 5. **All the split maps should not overlap with each other.**
 6. **Metadata file should also be provided.** The metadata structure description is provided below.
 
-Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023).
-
 #### Metadata structure
 
 The metadata should look like this:
@@ -118,7 +116,6 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co
 | enable_whole_load             | bool        | A flag to enable raw pointcloud map publishing                                    | true          |
 | enable_downsampled_whole_load | bool        | A flag to enable downsampled pointcloud map publishing                            | false         |
 | enable_partial_load           | bool        | A flag to enable partial pointcloud map server                                    | false         |
-| enable_differential_load      | bool        | A flag to enable differential pointcloud map server                               | false         |
 | enable_selected_load          | bool        | A flag to enable selected pointcloud map server                                   | false         |
 | leaf_size                     | float       | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0           |
 | pcd_paths_or_directory        | std::string | Path(s) to pointcloud map file or directory                                       |               |

map/map_loader/config/pointcloud_map_loader.param.yaml

@@ -3,7 +3,6 @@
     enable_whole_load: true
     enable_downsampled_whole_load: false
     enable_partial_load: true
-    enable_differential_load: true
     enable_selected_load: false
 
     # only used when downsample_whole_load enabled

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -52,42 +52,36 @@
   std::string pcd_metadata_path = declare_parameter<std::string>("pcd_metadata_path");
   bool enable_whole_load = declare_parameter<bool>("enable_whole_load");
   bool enable_downsample_whole_load = declare_parameter<bool>("enable_downsampled_whole_load");
   bool enable_partial_load = declare_parameter<bool>("enable_partial_load");
-  bool enable_differential_load = declare_parameter<bool>("enable_differential_load");
   bool enable_selected_load = declare_parameter<bool>("enable_selected_load");
 
   if (enable_whole_load) {
     std::string publisher_name = "output/pointcloud_map";
     pcd_map_loader_ =
       std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, false);
   }
 
   if (enable_downsample_whole_load) {
     std::string publisher_name = "output/debug/downsampled_pointcloud_map";
     downsampled_pcd_map_loader_ =
       std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
   }
 
-  if (enable_partial_load || enable_differential_load || enable_selected_load) {
-    std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
-    try {
-      pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);
-    } catch (std::runtime_error & e) {
-      RCLCPP_ERROR_STREAM(get_logger(), e.what());
-    }
+  std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
+  try {
+    pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);
+  } catch (std::runtime_error & e) {
+    RCLCPP_ERROR_STREAM(get_logger(), e.what());
+  }
 
-    if (enable_partial_load) {
-      partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict);
-    }
+  if (enable_partial_load) {
+    partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict);
+  }
 
-    if (enable_differential_load) {
-      differential_map_loader_ =
-        std::make_unique<DifferentialMapLoaderModule>(this, pcd_metadata_dict);
-    }
+  differential_map_loader_ = std::make_unique<DifferentialMapLoaderModule>(this, pcd_metadata_dict);
 
-    if (enable_selected_load) {
-      selected_map_loader_ = std::make_unique<SelectedMapLoaderModule>(this, pcd_metadata_dict);
-    }
+  if (enable_selected_load) {
+    selected_map_loader_ = std::make_unique<SelectedMapLoaderModule>(this, pcd_metadata_dict);
   }
 }