refactor(ekf_localizer): rework parameters

localization/ekf_localizer/README.md

@@ -78,65 +78,33 @@ The parameters are set in `launch/ekf_localizer.launch` .
 
 ### For Node
 
-| Name                       | Type   | Description                                                                               | Default value |
-| :------------------------- | :----- | :---------------------------------------------------------------------------------------- | :------------ |
-| show_debug_info            | bool   | Flag to display debug info                                                                | false         |
-| predict_frequency          | double | Frequency for filtering and publishing [Hz]                                               | 50.0          |
-| tf_rate                    | double | Frequency for tf broadcasting [Hz]                                                        | 10.0          |
-| publish_tf                 | bool   | Whether to publish tf                                                                     | true          |
-| extend_state_step          | int    | Max delay step which can be dealt with in EKF. Large number increases computational cost. | 50            |
-| enable_yaw_bias_estimation | bool   | Flag to enable yaw bias estimation                                                        | true          |
+{{ json_to_markdown("localization/ekf_localizer/schema/sub/node.sub_schema.json") }}
 
 ### For pose measurement
 
-| Name                          | Type   | Description                                                   | Default value |
-| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ |
-| pose_additional_delay         | double | Additional delay time for pose measurement [s]                | 0.0           |
-| pose_measure_uncertainty_time | double | Measured time uncertainty used for covariance calculation [s] | 0.01          |
-| pose_smoothing_steps          | int    | A value for smoothing steps                                   | 5             |
-| pose_gate_dist                | double | Limit of Mahalanobis distance used for outliers detection     | 10000.0       |
+{{ json_to_markdown("localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }}
 
 ### For twist measurement
 
-| Name                   | Type   | Description                                               | Default value |
-| :--------------------- | :----- | :-------------------------------------------------------- | :------------ |
-| twist_additional_delay | double | Additional delay time for twist [s]                       | 0.0           |
-| twist_smoothing_steps  | int    | A value for smoothing steps                               | 2             |
-| twist_gate_dist        | double | Limit of Mahalanobis distance used for outliers detection | 10000.0       |
+{{ json_to_markdown("localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }}
 
 ### For process noise
 
-| Name                   | Type   | Description                                                                                                      | Default value |
-| :--------------------- | :----- | :--------------------------------------------------------------------------------------------------------------- | :------------ |
-| proc_stddev_vx_c       | double | Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0  | 2.0           |
-| proc_stddev_wz_c       | double | Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0 | 0.2           |
-| proc_stddev_yaw_c      | double | Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega           | 0.005         |
-| proc_stddev_yaw_bias_c | double | Standard deviation of process noise in time differentiation expression of yaw_bias, noise for d_yaw_bias = 0     | 0.001         |
+{{ json_to_markdown("localization/ekf_localizer/schema/sub/process_noise.sub_schema.json") }}
 
 note: process noise for positions x & y are calculated automatically from nonlinear dynamics.
 
 ### Simple 1D Filter Parameters
 
-| Name                  | Type   | Description                                     | Default value |
-| :-------------------- | :----- | :---------------------------------------------- | :------------ |
-| z_filter_proc_dev     | double | Simple1DFilter - Z filter process deviation     | 1.0           |
-| roll_filter_proc_dev  | double | Simple1DFilter - Roll filter process deviation  | 0.01          |
-| pitch_filter_proc_dev | double | Simple1DFilter - Pitch filter process deviation | 0.01          |
+{{ json_to_markdown("localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }}
 
 ### For diagnostics
 
-| Name                                  | Type   | Description                                                                                                                                | Default value |
-| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| pose_no_update_count_threshold_warn   | size_t | The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times.    | 50            |
-| pose_no_update_count_threshold_error  | size_t | The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times.  | 250           |
-| twist_no_update_count_threshold_warn  | size_t | The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times.   | 50            |
-| twist_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 250           |
+{{ json_to_markdown("localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json") }}
 
 ### Misc
 
-| Name                              | Type   | Description                                                                                        | Default value  |
-| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- |
-| threshold_observable_velocity_mps | double | Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor | 0.0 (disabled) |
+{{ json_to_markdown("localization/ekf_localizer/schema/sub/misc.sub_schema.json") }}
 
 ## How to tune EKF parameters
 

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"

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"))
   {
   }
 

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)"/>

localization/ekf_localizer/schema/ekf_localizer.schema.json

@@ -0,0 +1,52 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration",
+  "type": "object",
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "type": "object",
+          "properties": {
+            "node": {
+              "$ref": "sub/node.sub_schema.json#/definitions/node"
+            },
+            "pose_measurement": {
+              "$ref": "sub/pose_measurement.sub_schema.json#/definitions/pose_measurement"
+            },
+            "twist_measurement": {
+              "$ref": "sub/twist_measurement.sub_schema.json#/definitions/twist_measurement"
+            },
+            "process_noise": {
+              "$ref": "sub/process_noise.sub_schema.json#/definitions/process_noise"
+            },
+            "simple_1d_filter_parameters": {
+              "$ref": "sub/simple_1d_filter_parameters.sub_schema.json#/definitions/simple_1d_filter_parameters"
+            },
+            "diagnostics": {
+              "$ref": "sub/diagnostics.sub_schema.json#/definitions/diagnostics"
+            },
+            "misc": {
+              "$ref": "sub/misc.sub_schema.json#/definitions/misc"
+            }
+          },
+          "required": [
+            "node",
+            "pose_measurement",
+            "twist_measurement",
+            "process_noise",
+            "simple_1d_filter_parameters",
+            "diagnostics",
+            "misc"
+          ],
+          "additionalProperties": false
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json

@@ -0,0 +1,38 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for Diagnostics",
+  "definitions": {
+    "diagnostics": {
+      "type": "object",
+      "properties": {
+        "pose_no_update_count_threshold_warn": {
+          "type": "integer",
+          "description": "The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times",
+          "default": 50
+        },
+        "pose_no_update_count_threshold_error": {
+          "type": "integer",
+          "description": "The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times",
+          "default": 100
+        },
+        "twist_no_update_count_threshold_warn": {
+          "type": "integer",
+          "description": "The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times",
+          "default": 50
+        },
+        "twist_no_update_count_threshold_error": {
+          "type": "integer",
+          "description": "The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times",
+          "default": 100
+        }
+      },
+      "required": [
+        "pose_no_update_count_threshold_warn",
+        "pose_no_update_count_threshold_error",
+        "twist_no_update_count_threshold_warn",
+        "twist_no_update_count_threshold_error"
+      ],
+      "additionalProperties": false
+    }
+  }
+}

localization/ekf_localizer/schema/sub/misc.sub_schema.json

@@ -0,0 +1,23 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for MISC",
+  "definitions": {
+    "misc": {
+      "type": "object",
+      "properties": {
+        "threshold_observable_velocity_mps": {
+          "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", "pose_frame_id"],
+      "additionalProperties": false
+    }
+  }
+}

localization/ekf_localizer/schema/sub/node.sub_schema.json

@@ -0,0 +1,50 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for node",
+  "definitions": {
+    "node": {
+      "type": "object",
+      "properties": {
+        "show_debug_info": {
+          "type": "boolean",
+          "description": "Flag to display debug info",
+          "default": false
+        },
+        "predict_frequency": {
+          "type": "number",
+          "description": "Frequency for filtering and publishing [Hz]",
+          "default": 50.0
+        },
+        "tf_rate": {
+          "type": "number",
+          "description": "Frequency for tf broadcasting [Hz]",
+          "default": 50.0
+        },
+        "publish_tf": {
+          "type": "boolean",
+          "description": "Whether to publish tf",
+          "default": true
+        },
+        "extend_state_step": {
+          "type": "integer",
+          "description": "Max delay step which can be dealt with in EKF. Large number increases computational cost.",
+          "default": 50
+        },
+        "enable_yaw_bias_estimation": {
+          "type": "boolean",
+          "description": "Flag to enable yaw bias estimation",
+          "default": true
+        }
+      },
+      "required": [
+        "show_debug_info",
+        "predict_frequency",
+        "tf_rate",
+        "publish_tf",
+        "extend_state_step",
+        "enable_yaw_bias_estimation"
+      ],
+      "additionalProperties": false
+    }
+  }
+}