[amcl] add our params to dynamic reconfigure

cmrobotics · Jan 4, 2023 · 1ef4fa8 · 1ef4fa8
1 parent 3106d1e
commit 1ef4fa8
Showing 1 changed file with 20 additions and 0 deletions.
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -1339,6 +1339,7 @@ AmclNode::dynamicParametersCallback(
   bool reinit_odom = false;
   bool reinit_laser = false;
   bool reinit_map = false;
+  bool reinit_ext_pose = false;
 
   for (auto parameter : parameters) {
     const auto & param_type = parameter.get_type();
@@ -1419,6 +1420,21 @@ AmclNode::dynamicParametersCallback(
       } else if (param_name == "z_short") {
         z_short_ = parameter.as_double();
         reinit_laser = true;
+      } else if (param_name == "std_warn_level_x") {
+        std_warn_level_x_ = parameter.as_double();
+      } else if (param_name == "std_warn_level_y") {
+        std_warn_level_y_ = parameter.as_double();
+      } else if (param_name == "std_warn_level_yaw") {
+        std_warn_level_yaw_ = parameter.as_double();
+      } else if (param_name == "laser_importance_factor") {
+        laser_importance_factor_ = parameter.as_double();
+        reinit_laser = true;
+      } else if (param_name == "max_particle_gen_prob_ext_pose") {
+        max_particle_gen_prob_ext_pose_ = parameter.as_double();
+        reinit_pf = true;
+      } else if (param_name == "ext_pose_search_tolerance_sec") {
+        ext_pose_search_tolerance_sec_ = parameter.as_double();
+        reinit_ext_pose = true;
       }
     } else if (param_type == ParameterType::PARAMETER_STRING) {
       if (param_name == "base_frame_id") {
@@ -1524,6 +1540,10 @@ AmclNode::dynamicParametersCallback(
       std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
   }
 
+  if(reinit_ext_pose) {
+    ext_pose_buffer_.reset(new ExternalPoseBuffer(ext_pose_search_tolerance_sec_));
+  }
+
   result.successful = true;
   return result;
 }