From eb771b49637eefa1322423a7ebaff87576b26d77 Mon Sep 17 00:00:00 2001 From: Motsu-san Date: Sat, 27 Jan 2024 21:44:22 +0900 Subject: [PATCH 1/6] refactor: Create JSON Schema files Signed-off-by: Motsu-san --- .../schema/ekf_localizer.schema.json | 56 +++++++++++++++++++ .../schema/sub/diagnostics.sub_schema.json | 38 +++++++++++++ .../schema/sub/misc.sub_schema.json | 20 +++++++ .../schema/sub/node.sub_schema.json | 50 +++++++++++++++++ .../sub/pose_measurement.sub_schema.json | 38 +++++++++++++ .../schema/sub/process_noise.sub_schema.json | 32 +++++++++++ ...imple_1d_filter_parameters.sub_schema.json | 32 +++++++++++ .../sub/twist_measurement.sub_schema.json | 32 +++++++++++ 8 files changed, 298 insertions(+) create mode 100644 localization/ekf_localizer/schema/ekf_localizer.schema.json create mode 100644 localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json create mode 100644 localization/ekf_localizer/schema/sub/misc.sub_schema.json create mode 100644 localization/ekf_localizer/schema/sub/node.sub_schema.json create mode 100644 localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json create mode 100644 localization/ekf_localizer/schema/sub/process_noise.sub_schema.json create mode 100644 localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json create mode 100644 localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json diff --git a/localization/ekf_localizer/schema/ekf_localizer.schema.json b/localization/ekf_localizer/schema/ekf_localizer.schema.json new file mode 100644 index 0000000000000..e160aeaf8ec37 --- /dev/null +++ b/localization/ekf_localizer/schema/ekf_localizer.schema.json @@ -0,0 +1,56 @@ +{ + "$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 +} diff --git a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json new file mode 100644 index 0000000000000..a6c271f23253e --- /dev/null +++ b/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": "Warning threshold for pose update count.", + "default": 50 + }, + "pose_no_update_count_threshold_error": { + "type": "integer", + "description": "Error threshold for pose update count.", + "default": 100 + }, + "twist_no_update_count_threshold_warn": { + "type": "integer", + "description": "Warning threshold for twist update count.", + "default": 50 + }, + "twist_no_update_count_threshold_error": { + "type": "integer", + "description": "Error threshold for twist update count.", + "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 + } + } +} diff --git a/localization/ekf_localizer/schema/sub/misc.sub_schema.json b/localization/ekf_localizer/schema/sub/misc.sub_schema.json new file mode 100644 index 0000000000000..88ea6ce08a78b --- /dev/null +++ b/localization/ekf_localizer/schema/sub/misc.sub_schema.json @@ -0,0 +1,20 @@ +{ + "$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": "Velocity threshold for observability in meters per second.", + "default": 0.0 + } + }, + "required": [ + "threshold_observable_velocity_mps" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/node.sub_schema.json b/localization/ekf_localizer/schema/sub/node.sub_schema.json new file mode 100644 index 0000000000000..20ffdb3029e08 --- /dev/null +++ b/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": "Displays debug information if set to true.", + "default": false + }, + "predict_frequency": { + "type": "number", + "description": "Frequency of prediction cycles.", + "default": 50.0 + }, + "tf_rate": { + "type": "number", + "description": "Frequency at which transform frames are published.", + "default": 50.0 + }, + "publish_tf": { + "type": "boolean", + "description": "Determines whether transform frames are published.", + "default": true + }, + "extend_state_step": { + "type": "integer", + "description": "Number of steps for extending the state.", + "default": 50 + }, + "enable_yaw_bias_estimation": { + "type": "boolean", + "description": "Enables yaw bias estimation.", + "default": true + } + }, + "required": [ + "show_debug_info", + "predict_frequency", + "tf_rate", + "publish_tf", + "extend_state_step", + "enable_yaw_bias_estimation" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json new file mode 100644 index 0000000000000..0264de14b9719 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Pose Measurement", + "definitions": { + "pose_measurement": { + "type": "object", + "properties": { + "pose_additional_delay": { + "type": "number", + "description": "Additional delay for pose measurements.", + "default": 0.0 + }, + "pose_measure_uncertainty_time": { + "type": "number", + "description": "Time uncertainty for pose measurements.", + "default": 0.01 + }, + "pose_smoothing_steps": { + "type": "integer", + "description": "Number of steps for pose smoothing.", + "default": 5 + }, + "pose_gate_dist": { + "type": "number", + "description": "Gate distance for pose measurements.", + "default": 10000.0 + } + }, + "required": [ + "pose_additional_delay", + "pose_measure_uncertainty_time", + "pose_smoothing_steps", + "pose_gate_dist" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json new file mode 100644 index 0000000000000..9ebe53b81d621 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json @@ -0,0 +1,32 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Process Noise", + "definitions": { + "process_noise": { + "type": "object", + "properties": { + "proc_stddev_yaw_c": { + "type": "number", + "description": "Standard deviation of yaw in the process model.", + "default": 0.005 + }, + "proc_stddev_vx_c": { + "type": "number", + "description": "Standard deviation of linear velocity in the process model.", + "default": 10.0 + }, + "proc_stddev_wz_c": { + "type": "number", + "description": "Standard deviation of angular velocity in the process model.", + "default": 5.0 + } + }, + "required": [ + "proc_stddev_yaw_c", + "proc_stddev_vx_c", + "proc_stddev_wz_c" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json new file mode 100644 index 0000000000000..a26ea2ed173f4 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json @@ -0,0 +1,32 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration of Simple 1D Filter Parameters", + "definitions": { + "simple_1d_filter_parameters": { + "type": "object", + "properties": { + "z_filter_proc_dev": { + "type": "number", + "description": "Process deviation for the Z-axis filter.", + "default": 1.0 + }, + "roll_filter_proc_dev": { + "type": "number", + "description": "Process deviation for the roll filter.", + "default": 0.01 + }, + "pitch_filter_proc_dev": { + "type": "number", + "description": "Process deviation for the pitch filter.", + "default": 0.01 + } + }, + "required": [ + "z_filter_proc_dev", + "roll_filter_proc_dev", + "pitch_filter_proc_dev" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json new file mode 100644 index 0000000000000..83a75706b1005 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json @@ -0,0 +1,32 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Twist Measurement", + "definitions": { + "twist_measurement": { + "type": "object", + "properties": { + "twist_additional_delay": { + "type": "number", + "description": "Additional delay for twist measurements.", + "default": 0.0 + }, + "twist_smoothing_steps": { + "type": "integer", + "description": "Number of steps for twist smoothing.", + "default": 2 + }, + "twist_gate_dist": { + "type": "number", + "description": "Gate distance for twist measurements.", + "default": 10000.0 + } + }, + "required": [ + "twist_additional_delay", + "twist_smoothing_steps", + "twist_gate_dist" + ], + "additionalProperties": false + } + } +} From 42d33fc6f047199a9f95b0fa743459d49114648e Mon Sep 17 00:00:00 2001 From: Motsu-san Date: Mon, 29 Jan 2024 10:05:41 +0900 Subject: [PATCH 2/6] Fix: Modify the descriptions of parameters Signed-off-by: Motsu-san --- .../schema/sub/diagnostics.sub_schema.json | 8 ++++---- .../ekf_localizer/schema/sub/misc.sub_schema.json | 2 +- .../ekf_localizer/schema/sub/node.sub_schema.json | 10 +++++----- .../schema/sub/pose_measurement.sub_schema.json | 8 ++++---- .../schema/sub/process_noise.sub_schema.json | 14 +++++++------- .../simple_1d_filter_parameters.sub_schema.json | 6 +++--- .../schema/sub/twist_measurement.sub_schema.json | 6 +++--- 7 files changed, 27 insertions(+), 27 deletions(-) diff --git a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json index a6c271f23253e..89adf721241df 100644 --- a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json @@ -7,22 +7,22 @@ "properties": { "pose_no_update_count_threshold_warn": { "type": "integer", - "description": "Warning threshold for pose update count.", + "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": "Error threshold for pose update count.", + "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": "Warning threshold for twist update count.", + "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": "Error threshold for twist update count.", + "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 } }, diff --git a/localization/ekf_localizer/schema/sub/misc.sub_schema.json b/localization/ekf_localizer/schema/sub/misc.sub_schema.json index 88ea6ce08a78b..7980c1a825f44 100644 --- a/localization/ekf_localizer/schema/sub/misc.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/misc.sub_schema.json @@ -7,7 +7,7 @@ "properties": { "threshold_observable_velocity_mps": { "type": "number", - "description": "Velocity threshold for observability in meters per second.", + "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 } }, diff --git a/localization/ekf_localizer/schema/sub/node.sub_schema.json b/localization/ekf_localizer/schema/sub/node.sub_schema.json index 20ffdb3029e08..537fcef452fbf 100644 --- a/localization/ekf_localizer/schema/sub/node.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/node.sub_schema.json @@ -7,27 +7,27 @@ "properties": { "show_debug_info": { "type": "boolean", - "description": "Displays debug information if set to true.", + "description": "Displays debug information if set to true", "default": false }, "predict_frequency": { "type": "number", - "description": "Frequency of prediction cycles.", + "description": "Frequency of prediction cycles for filtering and publishing [Hz]", "default": 50.0 }, "tf_rate": { "type": "number", - "description": "Frequency at which transform frames are published.", + "description": "Frequency at which transform frames are published [Hz]", "default": 50.0 }, "publish_tf": { "type": "boolean", - "description": "Determines whether transform frames are published.", + "description": "Determines whether transform frames are published", "default": true }, "extend_state_step": { "type": "integer", - "description": "Number of steps for extending the state.", + "description": "Number of max delay steps which can be dealt with in EKF. Large number increases computational cost.", "default": 50 }, "enable_yaw_bias_estimation": { diff --git a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json index 0264de14b9719..a323b6e76e9c5 100644 --- a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json @@ -7,22 +7,22 @@ "properties": { "pose_additional_delay": { "type": "number", - "description": "Additional delay for pose measurements.", + "description": "Additional delay for pose measurements [s]", "default": 0.0 }, "pose_measure_uncertainty_time": { "type": "number", - "description": "Time uncertainty for pose measurements.", + "description": "Time uncertainty for covariance calculation [s]", "default": 0.01 }, "pose_smoothing_steps": { "type": "integer", - "description": "Number of steps for pose smoothing.", + "description": "Number of steps for pose smoothing", "default": 5 }, "pose_gate_dist": { "type": "number", - "description": "Gate distance for pose measurements.", + "description": "Limit of Mahalanobis distance used for outliers detection", "default": 10000.0 } }, diff --git a/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json index 9ebe53b81d621..9fa84ca6fdb39 100644 --- a/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json @@ -5,20 +5,20 @@ "process_noise": { "type": "object", "properties": { - "proc_stddev_yaw_c": { - "type": "number", - "description": "Standard deviation of yaw in the process model.", - "default": 0.005 - }, "proc_stddev_vx_c": { "type": "number", - "description": "Standard deviation of linear velocity in the process model.", + "description": "Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0", "default": 10.0 }, "proc_stddev_wz_c": { "type": "number", - "description": "Standard deviation of angular velocity in the process model.", + "description": "Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0", "default": 5.0 + }, + "proc_stddev_yaw_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega", + "default": 0.005 } }, "required": [ diff --git a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json index a26ea2ed173f4..29d1609aecd8c 100644 --- a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json @@ -7,17 +7,17 @@ "properties": { "z_filter_proc_dev": { "type": "number", - "description": "Process deviation for the Z-axis filter.", + "description": "Simple1DFilter - Z filter process deviation", "default": 1.0 }, "roll_filter_proc_dev": { "type": "number", - "description": "Process deviation for the roll filter.", + "description": "Simple1DFilter - Roll filter process deviation", "default": 0.01 }, "pitch_filter_proc_dev": { "type": "number", - "description": "Process deviation for the pitch filter.", + "description": "Simple1DFilter - Pitch filter process deviation", "default": 0.01 } }, diff --git a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json index 83a75706b1005..47212573c08a1 100644 --- a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json @@ -7,17 +7,17 @@ "properties": { "twist_additional_delay": { "type": "number", - "description": "Additional delay for twist measurements.", + "description": "Additional delay for twist calculation [s]", "default": 0.0 }, "twist_smoothing_steps": { "type": "integer", - "description": "Number of steps for twist smoothing.", + "description": "Number of steps for twist smoothing", "default": 2 }, "twist_gate_dist": { "type": "number", - "description": "Gate distance for twist measurements.", + "description": "Limit of Mahalanobis distance used for outliers detection", "default": 10000.0 } }, From 1ac9b36da33df04248207a9f4889a085eacccd25 Mon Sep 17 00:00:00 2001 From: Motsu-san Date: Mon, 29 Jan 2024 10:11:37 +0900 Subject: [PATCH 3/6] fix: Redo modification of the descriptions Signed-off-by: Motsu-san --- .../ekf_localizer/schema/sub/node.sub_schema.json | 12 ++++++------ .../schema/sub/pose_measurement.sub_schema.json | 6 +++--- .../schema/sub/twist_measurement.sub_schema.json | 4 ++-- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/localization/ekf_localizer/schema/sub/node.sub_schema.json b/localization/ekf_localizer/schema/sub/node.sub_schema.json index 537fcef452fbf..faab80573d9ad 100644 --- a/localization/ekf_localizer/schema/sub/node.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/node.sub_schema.json @@ -7,32 +7,32 @@ "properties": { "show_debug_info": { "type": "boolean", - "description": "Displays debug information if set to true", + "description": "Flag to display debug info", "default": false }, "predict_frequency": { "type": "number", - "description": "Frequency of prediction cycles for filtering and publishing [Hz]", + "description": "Frequency for filtering and publishing [Hz]", "default": 50.0 }, "tf_rate": { "type": "number", - "description": "Frequency at which transform frames are published [Hz]", + "description": "Frequency for tf broadcasting [Hz]", "default": 50.0 }, "publish_tf": { "type": "boolean", - "description": "Determines whether transform frames are published", + "description": "Whether to publish tf", "default": true }, "extend_state_step": { "type": "integer", - "description": "Number of max delay steps which can be dealt with in EKF. Large number increases computational cost.", + "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": "Enables yaw bias estimation.", + "description": "Flag to enable yaw bias estimation", "default": true } }, diff --git a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json index a323b6e76e9c5..2a6699a768123 100644 --- a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json @@ -7,17 +7,17 @@ "properties": { "pose_additional_delay": { "type": "number", - "description": "Additional delay for pose measurements [s]", + "description": "Additional delay time for pose measurement [s]", "default": 0.0 }, "pose_measure_uncertainty_time": { "type": "number", - "description": "Time uncertainty for covariance calculation [s]", + "description": "Measured time uncertainty used for covariance calculation [s]", "default": 0.01 }, "pose_smoothing_steps": { "type": "integer", - "description": "Number of steps for pose smoothing", + "description": "A value for smoothing steps", "default": 5 }, "pose_gate_dist": { diff --git a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json index 47212573c08a1..cfaa7edfb3155 100644 --- a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json @@ -7,12 +7,12 @@ "properties": { "twist_additional_delay": { "type": "number", - "description": "Additional delay for twist calculation [s]", + "description": "Additional delay time for twist [s]", "default": 0.0 }, "twist_smoothing_steps": { "type": "integer", - "description": "Number of steps for twist smoothing", + "description": "A value for smoothing steps", "default": 2 }, "twist_gate_dist": { From 6f0ff05ac15633964435bb62ba36f74da4c7adee Mon Sep 17 00:00:00 2001 From: Motsu-san Date: Mon, 29 Jan 2024 10:16:39 +0900 Subject: [PATCH 4/6] doc: Replace parameter tables to JSON Schema ones in README Signed-off-by: Motsu-san --- localization/ekf_localizer/README.md | 46 +++++----------------------- 1 file changed, 7 insertions(+), 39 deletions(-) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 02f14a86cd7df..1b09420f5047d 100644 --- a/localization/ekf_localizer/README.md +++ b/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 From 2c2ae33b637e72355dd0410c992fa3aa48f83b92 Mon Sep 17 00:00:00 2001 From: Motsu-san Date: Mon, 5 Feb 2024 10:04:42 +0900 Subject: [PATCH 5/6] refactor: Remove default value from source code and launch.xml with arrangement of param .yaml Signed-off-by: Motsu-san --- .../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("node.show_debug_info")), + ekf_rate(node->declare_parameter("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("node.tf_rate")), + publish_tf_(node->declare_parameter("node.publish_tf")), + enable_yaw_bias_estimation(node->declare_parameter("node.enable_yaw_bias_estimation")), + extend_state_step(node->declare_parameter("node.extend_state_step")), + pose_frame_id(node->declare_parameter("misc.pose_frame_id")), + pose_additional_delay( + node->declare_parameter("pose_measurement.pose_additional_delay")), + pose_gate_dist(node->declare_parameter("pose_measurement.pose_gate_dist")), + pose_smoothing_steps(node->declare_parameter("pose_measurement.pose_smoothing_steps")), + twist_additional_delay( + node->declare_parameter("twist_measurement.twist_additional_delay")), + twist_gate_dist(node->declare_parameter("twist_measurement.twist_gate_dist")), + twist_smoothing_steps(node->declare_parameter("twist_measurement.twist_smoothing_steps")), + proc_stddev_vx_c(node->declare_parameter("process_noise.proc_stddev_vx_c")), + proc_stddev_wz_c(node->declare_parameter("process_noise.proc_stddev_wz_c")), + proc_stddev_yaw_c(node->declare_parameter("process_noise.proc_stddev_yaw_c")), + z_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.z_filter_proc_dev")), + roll_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.roll_filter_proc_dev")), + pitch_filter_proc_dev( + node->declare_parameter("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("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("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("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("diagnostics.twist_no_update_count_threshold_error")), threshold_observable_velocity_mps( - node->declare_parameter("threshold_observable_velocity_mps", 0.5)) + node->declare_parameter("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 @@ - - 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 } From a363f869b01c360657b099220e2af430601040a5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 5 Feb 2024 02:08:25 +0000 Subject: [PATCH 6/6] style(pre-commit): autofix --- .../schema/ekf_localizer.schema.json | 100 +++++++++--------- .../schema/sub/diagnostics.sub_schema.json | 68 ++++++------ .../schema/sub/misc.sub_schema.json | 41 ++++--- .../schema/sub/node.sub_schema.json | 92 ++++++++-------- .../sub/pose_measurement.sub_schema.json | 68 ++++++------ .../schema/sub/process_noise.sub_schema.json | 52 +++++---- ...imple_1d_filter_parameters.sub_schema.json | 52 +++++---- .../sub/twist_measurement.sub_schema.json | 52 +++++---- 8 files changed, 253 insertions(+), 272 deletions(-) diff --git a/localization/ekf_localizer/schema/ekf_localizer.schema.json b/localization/ekf_localizer/schema/ekf_localizer.schema.json index e160aeaf8ec37..61fbcc2973aae 100644 --- a/localization/ekf_localizer/schema/ekf_localizer.schema.json +++ b/localization/ekf_localizer/schema/ekf_localizer.schema.json @@ -1,56 +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 - } + "$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" }, - "required": [ - "ros__parameters" - ], - "additionalProperties": false + "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": [ - "/**" - ], - "additionalProperties": false + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false } diff --git a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json index 89adf721241df..2e2dca411971e 100644 --- a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json @@ -1,38 +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 + "$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 } + } } diff --git a/localization/ekf_localizer/schema/sub/misc.sub_schema.json b/localization/ekf_localizer/schema/sub/misc.sub_schema.json index 9066afceb07fe..cc36a5bf41ec6 100644 --- a/localization/ekf_localizer/schema/sub/misc.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/misc.sub_schema.json @@ -1,26 +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 + "$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 } + } } diff --git a/localization/ekf_localizer/schema/sub/node.sub_schema.json b/localization/ekf_localizer/schema/sub/node.sub_schema.json index faab80573d9ad..92e083b92d3e7 100644 --- a/localization/ekf_localizer/schema/sub/node.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/node.sub_schema.json @@ -1,50 +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 + "$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 } + } } diff --git a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json index 2a6699a768123..8b931d5f68d12 100644 --- a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json @@ -1,38 +1,38 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for Pose Measurement", - "definitions": { - "pose_measurement": { - "type": "object", - "properties": { - "pose_additional_delay": { - "type": "number", - "description": "Additional delay time for pose measurement [s]", - "default": 0.0 - }, - "pose_measure_uncertainty_time": { - "type": "number", - "description": "Measured time uncertainty used for covariance calculation [s]", - "default": 0.01 - }, - "pose_smoothing_steps": { - "type": "integer", - "description": "A value for smoothing steps", - "default": 5 - }, - "pose_gate_dist": { - "type": "number", - "description": "Limit of Mahalanobis distance used for outliers detection", - "default": 10000.0 - } - }, - "required": [ - "pose_additional_delay", - "pose_measure_uncertainty_time", - "pose_smoothing_steps", - "pose_gate_dist" - ], - "additionalProperties": false + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Pose Measurement", + "definitions": { + "pose_measurement": { + "type": "object", + "properties": { + "pose_additional_delay": { + "type": "number", + "description": "Additional delay time for pose measurement [s]", + "default": 0.0 + }, + "pose_measure_uncertainty_time": { + "type": "number", + "description": "Measured time uncertainty used for covariance calculation [s]", + "default": 0.01 + }, + "pose_smoothing_steps": { + "type": "integer", + "description": "A value for smoothing steps", + "default": 5 + }, + "pose_gate_dist": { + "type": "number", + "description": "Limit of Mahalanobis distance used for outliers detection", + "default": 10000.0 } + }, + "required": [ + "pose_additional_delay", + "pose_measure_uncertainty_time", + "pose_smoothing_steps", + "pose_gate_dist" + ], + "additionalProperties": false } + } } diff --git a/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json index 9fa84ca6fdb39..37a8c248edd2c 100644 --- a/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json @@ -1,32 +1,28 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for Process Noise", - "definitions": { - "process_noise": { - "type": "object", - "properties": { - "proc_stddev_vx_c": { - "type": "number", - "description": "Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0", - "default": 10.0 - }, - "proc_stddev_wz_c": { - "type": "number", - "description": "Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0", - "default": 5.0 - }, - "proc_stddev_yaw_c": { - "type": "number", - "description": "Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega", - "default": 0.005 - } - }, - "required": [ - "proc_stddev_yaw_c", - "proc_stddev_vx_c", - "proc_stddev_wz_c" - ], - "additionalProperties": false + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Process Noise", + "definitions": { + "process_noise": { + "type": "object", + "properties": { + "proc_stddev_vx_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0", + "default": 10.0 + }, + "proc_stddev_wz_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0", + "default": 5.0 + }, + "proc_stddev_yaw_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega", + "default": 0.005 } + }, + "required": ["proc_stddev_yaw_c", "proc_stddev_vx_c", "proc_stddev_wz_c"], + "additionalProperties": false } + } } diff --git a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json index 29d1609aecd8c..ad2f638a18d5f 100644 --- a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json @@ -1,32 +1,28 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration of Simple 1D Filter Parameters", - "definitions": { - "simple_1d_filter_parameters": { - "type": "object", - "properties": { - "z_filter_proc_dev": { - "type": "number", - "description": "Simple1DFilter - Z filter process deviation", - "default": 1.0 - }, - "roll_filter_proc_dev": { - "type": "number", - "description": "Simple1DFilter - Roll filter process deviation", - "default": 0.01 - }, - "pitch_filter_proc_dev": { - "type": "number", - "description": "Simple1DFilter - Pitch filter process deviation", - "default": 0.01 - } - }, - "required": [ - "z_filter_proc_dev", - "roll_filter_proc_dev", - "pitch_filter_proc_dev" - ], - "additionalProperties": false + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration of Simple 1D Filter Parameters", + "definitions": { + "simple_1d_filter_parameters": { + "type": "object", + "properties": { + "z_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Z filter process deviation", + "default": 1.0 + }, + "roll_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Roll filter process deviation", + "default": 0.01 + }, + "pitch_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Pitch filter process deviation", + "default": 0.01 } + }, + "required": ["z_filter_proc_dev", "roll_filter_proc_dev", "pitch_filter_proc_dev"], + "additionalProperties": false } + } } diff --git a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json index cfaa7edfb3155..727830a90a288 100644 --- a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json @@ -1,32 +1,28 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for Twist Measurement", - "definitions": { - "twist_measurement": { - "type": "object", - "properties": { - "twist_additional_delay": { - "type": "number", - "description": "Additional delay time for twist [s]", - "default": 0.0 - }, - "twist_smoothing_steps": { - "type": "integer", - "description": "A value for smoothing steps", - "default": 2 - }, - "twist_gate_dist": { - "type": "number", - "description": "Limit of Mahalanobis distance used for outliers detection", - "default": 10000.0 - } - }, - "required": [ - "twist_additional_delay", - "twist_smoothing_steps", - "twist_gate_dist" - ], - "additionalProperties": false + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Twist Measurement", + "definitions": { + "twist_measurement": { + "type": "object", + "properties": { + "twist_additional_delay": { + "type": "number", + "description": "Additional delay time for twist [s]", + "default": 0.0 + }, + "twist_smoothing_steps": { + "type": "integer", + "description": "A value for smoothing steps", + "default": 2 + }, + "twist_gate_dist": { + "type": "number", + "description": "Limit of Mahalanobis distance used for outliers detection", + "default": 10000.0 } + }, + "required": ["twist_additional_delay", "twist_smoothing_steps", "twist_gate_dist"], + "additionalProperties": false } + } }