minor fixes. Now map update module launches!!!

Signed-off-by: kminoda <koji.minoda@tier4.jp>
kminoda · Jan 6, 2023 · 09aed03 · 09aed03
1 parent 7d115cc
commit 09aed03
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 15 deletions.
diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml
@@ -1,5 +1,7 @@
 /**:
   ros__parameters:
+    # Use dynamic map loading
+    use_dynamic_map_loading: true
 
     # Vehicle reference frame
     base_frame: "base_link"
@@ -41,10 +43,6 @@
     # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
     initial_pose_distance_tolerance_m: 10.0
 
-    # neighborhood search method
-    # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
-    neighborhood_search_method: 0
-
     # Number of threads used for parallel computing
     num_threads: 4
 
@@ -63,5 +61,11 @@
     # Regularization switch
     regularization_enabled: false
 
-    # regularization scale factor
+    # Regularization scale factor
     regularization_scale_factor: 0.01
+
+    # Dynamic map loading distance
+    dml_update_map_distance: 20.0
+
+    # Dynamic map loading loading radius
+    dml_loading_radius: 120.0
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,5 +1,7 @@
 /**:
   ros__parameters:
+    # Use dynamic map loading
+    use_dynamic_map_loading: true
 
     # Vehicle reference frame
     base_frame: "base_link"
@@ -41,10 +43,6 @@
     # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
     initial_pose_distance_tolerance_m: 10.0
 
-    # neighborhood search method
-    # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
-    neighborhood_search_method: 0
-
     # Number of threads used for parallel computing
     num_threads: 4
 
@@ -65,3 +63,9 @@
 
     # Regularization scale factor
     regularization_scale_factor: 0.01
+
+    # Dynamic map loading distance
+    dml_update_map_distance: 20.0
+
+    # Dynamic map loading loading radius
+    dml_loading_radius: 120.0
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
@@ -46,7 +46,6 @@ class MapUpdateModule
     std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
     std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
     rclcpp::CallbackGroup::SharedPtr main_callback_group,
-    rclcpp::CallbackGroup::SharedPtr map_callback_group,
     std::shared_ptr<std::map<std::string, std::string>> state_ptr);
 
 private:
@@ -82,6 +81,7 @@ class MapUpdateModule
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr ekf_odom_sub_;
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
+  rclcpp::CallbackGroup::SharedPtr map_callback_group_;
 
   std::shared_ptr<NormalDistributionsTransform> ndt_ptr_;
   std::mutex * ndt_ptr_mutex_;

diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -27,7 +27,6 @@ MapUpdateModule::MapUpdateModule(
   std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
   rclcpp::CallbackGroup::SharedPtr main_callback_group,
-  rclcpp::CallbackGroup::SharedPtr map_callback_group,
   std::shared_ptr<std::map<std::string, std::string>> state_ptr)
 : ndt_ptr_(ndt_ptr),
   ndt_ptr_mutex_(ndt_ptr_mutex),
@@ -50,6 +49,8 @@ MapUpdateModule::MapUpdateModule(
   auto main_sub_opt = rclcpp::SubscriptionOptions();
   main_sub_opt.callback_group = main_callback_group;
 
+  map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
   ekf_odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
     "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1),
     main_sub_opt);
@@ -61,7 +62,7 @@ MapUpdateModule::MapUpdateModule(
     "ndt_align_srv",
     std::bind(
       &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2),
-    rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group);
+    rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_);
 
   pcd_loader_client_ = node->create_client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>(
     "pcd_loader_service", rmw_qos_profile_services_default);
@@ -76,7 +77,7 @@ MapUpdateModule::MapUpdateModule(
     std::chrono::duration<double>(map_update_dt));
   map_update_timer_ = rclcpp::create_timer(
     node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this),
-    map_callback_group);
+    map_callback_group_);
 }
 
 void MapUpdateModule::service_ndt_align(

diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -213,8 +213,8 @@ NDTScanMatcher::NDTScanMatcher()
   tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);
 
   if (use_dynamic_map_loading_) {
-    map_update_module_ = std::make_unique<MapUpdateModule>(this, &ndt_ptr_mtsx_, ndt_ptr_, tf2_listener_module_,
-      map_frame_, main_callback_group, map_callback_group, state_ptr);
+    map_update_module_ = std::make_unique<MapUpdateModule>(this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_,
+      map_frame_, main_callback_group, state_ptr_);
   } else {
     map_module_ = std::make_unique<MapModule>(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group);
     pose_init_module_ = std::make_unique<PoseInitializationModule>(