refactor(ndt_scan_matcher, map_loader): remove map_module (#5873)

* Removed use_dynamic_map_loading Signed-off-by: Shintaro SAKODA <[email protected]> * Removed enable_differential_load option Signed-off-by: Shintaro SAKODA <[email protected]> * style(pre-commit): autofix * Fixed title Signed-off-by: Shintaro Sakoda <[email protected]> * Emphasized lock scope Signed-off-by: Shintaro Sakoda <[email protected]> * Removed pointcloud_map and input_ekf_odom Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro SAKODA <[email protected]> Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jan 9, 2024 · d8767dd · d8767dd
1 parent 412b0e7
commit d8767dd
Show file tree

Hide file tree

Showing 12 changed files with 29 additions and 175 deletions.
diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/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
 )
 

diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md
@@ -18,7 +18,6 @@ One optional function is regularization. Please see the regularization chapter i
 | Name                                | Type                                            | Description                           |
 | ----------------------------------- | ----------------------------------------------- | ------------------------------------- |
 | `ekf_pose_with_covariance`          | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose                          |
-| `pointcloud_map`                    | `sensor_msgs::msg::PointCloud2`                 | map pointcloud                        |
 | `points_raw`                        | `sensor_msgs::msg::PointCloud2`                 | sensor pointcloud                     |
 | `sensing/gnss/pose_with_covariance` | `sensor_msgs::msg::PoseWithCovarianceStamped`   | base position for regularization term |
 
@@ -193,12 +192,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma
 
 ### Additional interfaces
 
-#### Additional inputs
-
-| Name             | Type                      | Description                                                 |
-| ---------------- | ------------------------- | ----------------------------------------------------------- |
-| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) |
-
 #### Additional outputs
 
 | Name                          | Type                            | Description                                       |
@@ -213,20 +206,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)         |
-
-### Enabling the dynamic map loading feature
+| 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) |
 
-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.
+### Notes for dynamic map loading
 
-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 +223,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
 

diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/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"
 

diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -18,7 +18,6 @@
 #define FMT_HEADER_ONLY
 
 #include "localization_util/smart_pose_buffer.hpp"
-#include "ndt_scan_matcher/map_module.hpp"
 #include "ndt_scan_matcher/map_update_module.hpp"
 
 #include <rclcpp/rclcpp.hpp>
@@ -216,16 +215,13 @@ class NDTScanMatcher : public rclcpp::Node
   std::unique_ptr<SmartPoseBuffer> regularization_pose_buffer_;
 
   std::atomic<bool> is_activated_;
-  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_;
 };

diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
@@ -4,7 +4,6 @@
   <!-- Topics -->
   <arg name="input/pointcloud" default="/points_raw" description="Sensor points topic"/>
   <arg name="input_initial_pose_topic" default="/ekf_pose_with_covariance" description="Initial position topic to align"/>
-  <arg name="input_map_points_topic" default="/pointcloud_map" description="Map points topic"/>
   <arg name="input_regularization_pose_topic" default="/sensing/gnss/pose_with_covariance" description="Regularization pose topic"/>
   <arg name="input_service_trigger_node" default="trigger_node" description="Trigger node service name"/>
 
@@ -19,7 +18,6 @@
     <remap from="points_raw" to="$(var input/pointcloud)"/>
 
     <remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>
-    <remap from="pointcloud_map" to="$(var input_map_points_topic)"/>
 
     <remap from="ndt_pose" to="$(var output_pose_topic)"/>
     <remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)"/>

diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/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

diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -264,12 +264,7 @@ NDTScanMatcher::NDTScanMatcher()
       &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
     rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);
 
-  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);
 }
@@ -347,9 +342,6 @@ void NDTScanMatcher::callback_timer()
   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(
@@ -380,7 +372,8 @@ void NDTScanMatcher::callback_initial_pose(
         << ". Please check the frame_id in the input topic and ensure it is correct.");
   }
 
-  if (use_dynamic_map_loading_) {
+  {
+    // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock
     std::lock_guard<std::mutex> lock(latest_ekf_position_mtx_);
     latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position;
   }
@@ -898,9 +891,7 @@ void NDTScanMatcher::service_ndt_align(
 
   // transform pose_frame to map_frame
   const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t);
-  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_);

diff --git a/map/map_loader/README.md b/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                                       |               |

diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/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

diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
@@ -53,7 +53,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
   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) {
@@ -68,26 +67,21 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
       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);
   }
 }