chore(ekf_localizer): move parameters to its dedicated yaml file (aut…

…owarefoundation#3039) * chores(ekf_localizer): move parameters to its dedicated yaml file Signed-off-by: Vincent Richard <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Vincent Richard <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
badai-nguyen · Apr 4, 2023 · ebf636e · ebf636e
1 parent f44bafd
commit ebf636e
Show file tree

Hide file tree

Showing 5 changed files with 32 additions and 45 deletions.
diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -6,6 +6,9 @@
   <arg name="random_downsample_filter_param_path"/>
   <arg name="ndt_scan_matcher_param_path"/>
   <arg name="localization_error_monitor_param_path"/>
+  <arg name="ekf_localizer_param_path"/>
+  <arg name="pose_initializer_param_path"/>
+  <arg name="pose_initializer_common_param_path"/>
 
   <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud" description="The topic will be used in the localization util module"/>
   <arg name="use_pointcloud_container" default="true" description="launch pointcloud container"/>

diff --git a/...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
@@ -2,9 +2,6 @@
 <launch>
   <group>
     <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
-      <arg name="enable_yaw_bias_estimation" value="true"/>
-      <arg name="tf_rate" value="50.0"/>
-      <arg name="twist_smoothing_steps" value="2"/>
       <arg name="input_initial_pose_name" value="/initialpose3d"/>
       <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
       <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
@@ -15,8 +12,7 @@
       <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
       <arg name="output_twist_name" value="twist"/>
       <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
-      <arg name="proc_stddev_vx_c" value="10.0"/>
-      <arg name="proc_stddev_wz_c" value="5.0"/>
+      <arg name="param_file" value="$(var ekf_localizer_param_path)"/>
     </include>
   </group>
 

diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt
@@ -75,5 +75,6 @@ endif()
 
 ament_auto_package(
   INSTALL_TO_SHARE
+  config
   launch
 )
diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml
@@ -0,0 +1,23 @@
+/**:
+  ros__parameters:
+    show_debug_info: false
+    enable_yaw_bias_estimation: True
+    predict_frequency: 50.0
+    tf_rate: 50.0
+    extend_state_step: 50
+
+    # for Pose measurement
+    pose_additional_delay: 0.0
+    pose_measure_uncertainty_time: 0.01
+    pose_smoothing_steps: 5
+    pose_gate_dist: 10000.0
+
+    # for twist measurement
+    twist_additional_delay: 0.0
+    twist_smoothing_steps: 2
+    twist_gate_dist: 10000.0
+
+    # for process model
+    proc_stddev_yaw_c: 0.005
+    proc_stddev_vx_c: 10.0
+    proc_stddev_wz_c: 5.0
diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml
@@ -1,30 +1,12 @@
 <launch>
-  <arg name="show_debug_info" default="false"/>
-  <arg name="enable_yaw_bias_estimation" default="True"/>
-  <arg name="predict_frequency" default="50.0"/>
-  <arg name="tf_rate" default="10.0"/>
-  <arg name="extend_state_step" default="50"/>
+  <arg name="param_file" default="$(find-pkg-share ekf_localizer)/config/ekf_localizer.param.yaml"/>
 
   <arg name="input_initial_pose_name" default="initialpose3d"/>
   <arg name="input_trigger_node_service_name" default="trigger_node" description="trigger node service"/>
 
-  <!-- for Pose measurement -->
+  <!-- input topic name -->
   <arg name="input_pose_with_cov_name" default="in_pose_with_covariance"/>
-  <arg name="pose_additional_delay" default="0.0"/>
-  <arg name="pose_measure_uncertainty_time" default="0.01"/>
-  <arg name="pose_smoothing_steps" default="5"/>
-  <arg name="pose_gate_dist" default="10000.0"/>
-
-  <!-- for twist measurement -->
   <arg name="input_twist_with_cov_name" default="in_twist_with_covariance"/>
-  <arg name="twist_additional_delay" default="0.0"/>
-  <arg name="twist_smoothing_steps" default="2"/>
-  <arg name="twist_gate_dist" default="10000.0"/>
-
-  <!-- for process model -->
-  <arg name="proc_stddev_yaw_c" default="0.005"/>
-  <arg name="proc_stddev_vx_c" default="5.0"/>
-  <arg name="proc_stddev_wz_c" default="1.0"/>
 
   <!-- output topic name -->
   <arg name="output_odom_name" default="ekf_odom"/>
@@ -45,32 +27,14 @@
 
     <param name="pose_frame_id" value="map"/>
 
-    <param name="show_debug_info" value="$(var show_debug_info)"/>
-    <param name="enable_yaw_bias_estimation" value="$(var enable_yaw_bias_estimation)"/>
-
-    <param name="predict_frequency" value="$(var predict_frequency)"/>
-    <param name="tf_rate" value="$(var tf_rate)"/>
-    <param name="extend_state_step" value="$(var extend_state_step)"/>
-
-    <param name="pose_additional_delay" value="$(var pose_additional_delay)"/>
-    <param name="pose_measure_uncertainty_time" value="$(var pose_measure_uncertainty_time)"/>
-    <param name="pose_smoothing_steps" value="$(var pose_smoothing_steps)"/>
-    <param name="pose_gate_dist" value="$(var pose_gate_dist)"/>
-
-    <param name="twist_additional_delay" value="$(var twist_additional_delay)"/>
-    <param name="twist_smoothing_steps" value="$(var twist_smoothing_steps)"/>
-    <param name="twist_gate_dist" value="$(var twist_gate_dist)"/>
-
-    <param name="proc_stddev_yaw_c" value="$(var proc_stddev_yaw_c)"/>
-    <param name="proc_stddev_vx_c" value="$(var proc_stddev_vx_c)"/>
-    <param name="proc_stddev_wz_c" value="$(var proc_stddev_wz_c)"/>
-
     <remap from="ekf_odom" to="$(var output_odom_name)"/>
     <remap from="ekf_pose" to="$(var output_pose_name)"/>
     <remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)"/>
     <remap from="ekf_biased_pose" to="$(var output_biased_pose_name)"/>
     <remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)"/>
     <remap from="ekf_twist" to="$(var output_twist_name)"/>
     <remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)"/>
+
+    <param from="$(var param_file)"/>
   </node>
 </launch>