From 76d11bb78a91c160079f84282b70de77d3c957ee Mon Sep 17 00:00:00 2001
From: Motsu-san <masahiro.sakamoto@tier4.jp>
Date: Mon, 5 Feb 2024 10:04:42 +0900
Subject: [PATCH] refactor: Remove default value from source code and
 launch.xml with arrangement of param .yaml

Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp>
---
 .../config/ekf_localizer.param.yaml           | 68 +++++++++++--------
 .../ekf_localizer/hyper_parameters.hpp        | 53 ++++++++-------
 .../launch/ekf_localizer.launch.xml           |  2 -
 .../schema/sub/misc.sub_schema.json           |  8 ++-
 4 files changed, 74 insertions(+), 57 deletions(-)

diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml
index 9de7f56f5c006..4cc71e0904ca8 100644
--- a/localization/ekf_localizer/config/ekf_localizer.param.yaml
+++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml
@@ -1,38 +1,46 @@
 /**:
   ros__parameters:
-    show_debug_info: false
-    enable_yaw_bias_estimation: true
-    predict_frequency: 50.0
-    tf_rate: 50.0
-    publish_tf: true
-    extend_state_step: 50
+    node:
+      show_debug_info: false
+      enable_yaw_bias_estimation: true
+      predict_frequency: 50.0
+      tf_rate: 50.0
+      publish_tf: true
+      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
+    pose_measurement:
+      # 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
+    twist_measurement:
+      # 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
+    process_noise:
+      # for process model
+      proc_stddev_yaw_c: 0.005
+      proc_stddev_vx_c: 10.0
+      proc_stddev_wz_c: 5.0
 
-    #Simple1DFilter parameters
-    z_filter_proc_dev: 1.0
-    roll_filter_proc_dev: 0.01
-    pitch_filter_proc_dev: 0.01
+    simple_1d_filter_parameters:
+      #Simple1DFilter parameters
+      z_filter_proc_dev: 1.0
+      roll_filter_proc_dev: 0.01
+      pitch_filter_proc_dev: 0.01
 
-    # for diagnostics
-    pose_no_update_count_threshold_warn: 50
-    pose_no_update_count_threshold_error: 100
-    twist_no_update_count_threshold_warn: 50
-    twist_no_update_count_threshold_error: 100
+    diagnostics:
+      # for diagnostics
+      pose_no_update_count_threshold_warn: 50
+      pose_no_update_count_threshold_error: 100
+      twist_no_update_count_threshold_warn: 50
+      twist_no_update_count_threshold_error: 100
 
-    # for velocity measurement limitation (Set 0.0 if you want to ignore)
-    threshold_observable_velocity_mps: 0.0 # [m/s]
+    misc:
+      # for velocity measurement limitation (Set 0.0 if you want to ignore)
+      threshold_observable_velocity_mps: 0.0 # [m/s]
+      pose_frame_id: "map"
diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
index 5139f900a339e..56a13d1ecaf55 100644
--- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
@@ -24,36 +24,41 @@ class HyperParameters
 {
 public:
   explicit HyperParameters(rclcpp::Node * node)
-  : show_debug_info(node->declare_parameter("show_debug_info", false)),
-    ekf_rate(node->declare_parameter("predict_frequency", 50.0)),
+  : show_debug_info(node->declare_parameter<bool>("node.show_debug_info")),
+    ekf_rate(node->declare_parameter<double>("node.predict_frequency")),
     ekf_dt(1.0 / std::max(ekf_rate, 0.1)),
-    tf_rate_(node->declare_parameter("tf_rate", 10.0)),
-    publish_tf_(node->declare_parameter("publish_tf", true)),
-    enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)),
-    extend_state_step(node->declare_parameter("extend_state_step", 50)),
-    pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))),
-    pose_additional_delay(node->declare_parameter("pose_additional_delay", 0.0)),
-    pose_gate_dist(node->declare_parameter("pose_gate_dist", 10000.0)),
-    pose_smoothing_steps(node->declare_parameter("pose_smoothing_steps", 5)),
-    twist_additional_delay(node->declare_parameter("twist_additional_delay", 0.0)),
-    twist_gate_dist(node->declare_parameter("twist_gate_dist", 10000.0)),
-    twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)),
-    proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)),
-    proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)),
-    proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)),
-    z_filter_proc_dev(node->declare_parameter("z_filter_proc_dev", 1.0)),
-    roll_filter_proc_dev(node->declare_parameter("roll_filter_proc_dev", 0.01)),
-    pitch_filter_proc_dev(node->declare_parameter("pitch_filter_proc_dev", 0.01)),
+    tf_rate_(node->declare_parameter<double>("node.tf_rate")),
+    publish_tf_(node->declare_parameter<bool>("node.publish_tf")),
+    enable_yaw_bias_estimation(node->declare_parameter<bool>("node.enable_yaw_bias_estimation")),
+    extend_state_step(node->declare_parameter<int>("node.extend_state_step")),
+    pose_frame_id(node->declare_parameter<std::string>("misc.pose_frame_id")),
+    pose_additional_delay(
+      node->declare_parameter<double>("pose_measurement.pose_additional_delay")),
+    pose_gate_dist(node->declare_parameter<double>("pose_measurement.pose_gate_dist")),
+    pose_smoothing_steps(node->declare_parameter<int>("pose_measurement.pose_smoothing_steps")),
+    twist_additional_delay(
+      node->declare_parameter<double>("twist_measurement.twist_additional_delay")),
+    twist_gate_dist(node->declare_parameter<double>("twist_measurement.twist_gate_dist")),
+    twist_smoothing_steps(node->declare_parameter<int>("twist_measurement.twist_smoothing_steps")),
+    proc_stddev_vx_c(node->declare_parameter<double>("process_noise.proc_stddev_vx_c")),
+    proc_stddev_wz_c(node->declare_parameter<double>("process_noise.proc_stddev_wz_c")),
+    proc_stddev_yaw_c(node->declare_parameter<double>("process_noise.proc_stddev_yaw_c")),
+    z_filter_proc_dev(
+      node->declare_parameter<double>("simple_1d_filter_parameters.z_filter_proc_dev")),
+    roll_filter_proc_dev(
+      node->declare_parameter<double>("simple_1d_filter_parameters.roll_filter_proc_dev")),
+    pitch_filter_proc_dev(
+      node->declare_parameter<double>("simple_1d_filter_parameters.pitch_filter_proc_dev")),
     pose_no_update_count_threshold_warn(
-      node->declare_parameter("pose_no_update_count_threshold_warn", 50)),
+      node->declare_parameter<int>("diagnostics.pose_no_update_count_threshold_warn")),
     pose_no_update_count_threshold_error(
-      node->declare_parameter("pose_no_update_count_threshold_error", 250)),
+      node->declare_parameter<int>("diagnostics.pose_no_update_count_threshold_error")),
     twist_no_update_count_threshold_warn(
-      node->declare_parameter("twist_no_update_count_threshold_warn", 50)),
+      node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_warn")),
     twist_no_update_count_threshold_error(
-      node->declare_parameter("twist_no_update_count_threshold_error", 250)),
+      node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_error")),
     threshold_observable_velocity_mps(
-      node->declare_parameter("threshold_observable_velocity_mps", 0.5))
+      node->declare_parameter<double>("misc.threshold_observable_velocity_mps"))
   {
   }
 
diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml
index ee0504293602a..b6a1bd06185c2 100644
--- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml
+++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml
@@ -25,8 +25,6 @@
     <remap from="initialpose" to="$(var input_initial_pose_name)"/>
     <remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>
 
-    <param name="pose_frame_id" value="map"/>
-
     <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)"/>
diff --git a/localization/ekf_localizer/schema/sub/misc.sub_schema.json b/localization/ekf_localizer/schema/sub/misc.sub_schema.json
index 7980c1a825f44..9066afceb07fe 100644
--- a/localization/ekf_localizer/schema/sub/misc.sub_schema.json
+++ b/localization/ekf_localizer/schema/sub/misc.sub_schema.json
@@ -9,10 +9,16 @@
                     "type": "number",
                     "description": "Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor [m/s] (0.0 means disabled)",
                     "default": 0.0
+                },
+                "pose_frame_id": {
+                    "type": "string",
+                    "description": "Parent frame_id of EKF output pose",
+                    "default": "map"
                 }
             },
             "required": [
-                "threshold_observable_velocity_mps"
+                "threshold_observable_velocity_mps",
+                "pose_frame_id"
             ],
             "additionalProperties": false
         }