Skip to content

Commit 7286437

Browse files
Motsu-sanpre-commit-ci[bot]
Motsu-san
and
pre-commit-ci[bot]
authored
refactor(ekf_localizer): rework parameters (#6196)
* refactor: Create JSON Schema files Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> * Fix: Modify the descriptions of parameters Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> * fix: Redo modification of the descriptions Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> * doc: Replace parameter tables to JSON Schema ones in README Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> * refactor: Remove default value from source code and launch.xml with arrangement of param .yaml Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 0b90a97 commit 7286437

12 files changed

+359
-95
lines changed

‎localization/ekf_localizer/README.md

+7-39
Original file line numberDiff line numberDiff line change
@@ -78,65 +78,33 @@ The parameters are set in `launch/ekf_localizer.launch` .
7878

7979
### For Node
8080

81-
| Name | Type | Description | Default value |
82-
| :------------------------- | :----- | :---------------------------------------------------------------------------------------- | :------------ |
83-
| show_debug_info | bool | Flag to display debug info | false |
84-
| predict_frequency | double | Frequency for filtering and publishing [Hz] | 50.0 |
85-
| tf_rate | double | Frequency for tf broadcasting [Hz] | 10.0 |
86-
| publish_tf | bool | Whether to publish tf | true |
87-
| extend_state_step | int | Max delay step which can be dealt with in EKF. Large number increases computational cost. | 50 |
88-
| enable_yaw_bias_estimation | bool | Flag to enable yaw bias estimation | true |
81+
{{ json_to_markdown("localization/ekf_localizer/schema/sub/node.sub_schema.json") }}
8982

9083
### For pose measurement
9184

92-
| Name | Type | Description | Default value |
93-
| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ |
94-
| pose_additional_delay | double | Additional delay time for pose measurement [s] | 0.0 |
95-
| pose_measure_uncertainty_time | double | Measured time uncertainty used for covariance calculation [s] | 0.01 |
96-
| pose_smoothing_steps | int | A value for smoothing steps | 5 |
97-
| pose_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 |
85+
{{ json_to_markdown("localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }}
9886

9987
### For twist measurement
10088

101-
| Name | Type | Description | Default value |
102-
| :--------------------- | :----- | :-------------------------------------------------------- | :------------ |
103-
| twist_additional_delay | double | Additional delay time for twist [s] | 0.0 |
104-
| twist_smoothing_steps | int | A value for smoothing steps | 2 |
105-
| twist_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 |
89+
{{ json_to_markdown("localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }}
10690

10791
### For process noise
10892

109-
| Name | Type | Description | Default value |
110-
| :--------------------- | :----- | :--------------------------------------------------------------------------------------------------------------- | :------------ |
111-
| 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 |
112-
| 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 |
113-
| proc_stddev_yaw_c | double | Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega | 0.005 |
114-
| 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 |
93+
{{ json_to_markdown("localization/ekf_localizer/schema/sub/process_noise.sub_schema.json") }}
11594

11695
note: process noise for positions x & y are calculated automatically from nonlinear dynamics.
11796

11897
### Simple 1D Filter Parameters
11998

120-
| Name | Type | Description | Default value |
121-
| :-------------------- | :----- | :---------------------------------------------- | :------------ |
122-
| z_filter_proc_dev | double | Simple1DFilter - Z filter process deviation | 1.0 |
123-
| roll_filter_proc_dev | double | Simple1DFilter - Roll filter process deviation | 0.01 |
124-
| pitch_filter_proc_dev | double | Simple1DFilter - Pitch filter process deviation | 0.01 |
99+
{{ json_to_markdown("localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }}
125100

126101
### For diagnostics
127102

128-
| Name | Type | Description | Default value |
129-
| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
130-
| 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 |
131-
| 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 |
132-
| 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 |
133-
| 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 |
103+
{{ json_to_markdown("localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json") }}
134104

135105
### Misc
136106

137-
| Name | Type | Description | Default value |
138-
| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- |
139-
| 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) |
107+
{{ json_to_markdown("localization/ekf_localizer/schema/sub/misc.sub_schema.json") }}
140108

141109
## How to tune EKF parameters
142110

‎localization/ekf_localizer/config/ekf_localizer.param.yaml

+38-30
Original file line numberDiff line numberDiff line change
@@ -1,38 +1,46 @@
11
/**:
22
ros__parameters:
3-
show_debug_info: false
4-
enable_yaw_bias_estimation: true
5-
predict_frequency: 50.0
6-
tf_rate: 50.0
7-
publish_tf: true
8-
extend_state_step: 50
3+
node:
4+
show_debug_info: false
5+
enable_yaw_bias_estimation: true
6+
predict_frequency: 50.0
7+
tf_rate: 50.0
8+
publish_tf: true
9+
extend_state_step: 50
910

10-
# for Pose measurement
11-
pose_additional_delay: 0.0
12-
pose_measure_uncertainty_time: 0.01
13-
pose_smoothing_steps: 5
14-
pose_gate_dist: 10000.0
11+
pose_measurement:
12+
# for Pose measurement
13+
pose_additional_delay: 0.0
14+
pose_measure_uncertainty_time: 0.01
15+
pose_smoothing_steps: 5
16+
pose_gate_dist: 10000.0
1517

16-
# for twist measurement
17-
twist_additional_delay: 0.0
18-
twist_smoothing_steps: 2
19-
twist_gate_dist: 10000.0
18+
twist_measurement:
19+
# for twist measurement
20+
twist_additional_delay: 0.0
21+
twist_smoothing_steps: 2
22+
twist_gate_dist: 10000.0
2023

21-
# for process model
22-
proc_stddev_yaw_c: 0.005
23-
proc_stddev_vx_c: 10.0
24-
proc_stddev_wz_c: 5.0
24+
process_noise:
25+
# for process model
26+
proc_stddev_yaw_c: 0.005
27+
proc_stddev_vx_c: 10.0
28+
proc_stddev_wz_c: 5.0
2529

26-
#Simple1DFilter parameters
27-
z_filter_proc_dev: 1.0
28-
roll_filter_proc_dev: 0.01
29-
pitch_filter_proc_dev: 0.01
30+
simple_1d_filter_parameters:
31+
#Simple1DFilter parameters
32+
z_filter_proc_dev: 1.0
33+
roll_filter_proc_dev: 0.01
34+
pitch_filter_proc_dev: 0.01
3035

31-
# for diagnostics
32-
pose_no_update_count_threshold_warn: 50
33-
pose_no_update_count_threshold_error: 100
34-
twist_no_update_count_threshold_warn: 50
35-
twist_no_update_count_threshold_error: 100
36+
diagnostics:
37+
# for diagnostics
38+
pose_no_update_count_threshold_warn: 50
39+
pose_no_update_count_threshold_error: 100
40+
twist_no_update_count_threshold_warn: 50
41+
twist_no_update_count_threshold_error: 100
3642

37-
# for velocity measurement limitation (Set 0.0 if you want to ignore)
38-
threshold_observable_velocity_mps: 0.0 # [m/s]
43+
misc:
44+
# for velocity measurement limitation (Set 0.0 if you want to ignore)
45+
threshold_observable_velocity_mps: 0.0 # [m/s]
46+
pose_frame_id: "map"

‎localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

+29-24
Original file line numberDiff line numberDiff line change
@@ -24,36 +24,41 @@ class HyperParameters
2424
{
2525
public:
2626
explicit HyperParameters(rclcpp::Node * node)
27-
: show_debug_info(node->declare_parameter("show_debug_info", false)),
28-
ekf_rate(node->declare_parameter("predict_frequency", 50.0)),
27+
: show_debug_info(node->declare_parameter<bool>("node.show_debug_info")),
28+
ekf_rate(node->declare_parameter<double>("node.predict_frequency")),
2929
ekf_dt(1.0 / std::max(ekf_rate, 0.1)),
30-
tf_rate_(node->declare_parameter("tf_rate", 10.0)),
31-
publish_tf_(node->declare_parameter("publish_tf", true)),
32-
enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)),
33-
extend_state_step(node->declare_parameter("extend_state_step", 50)),
34-
pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))),
35-
pose_additional_delay(node->declare_parameter("pose_additional_delay", 0.0)),
36-
pose_gate_dist(node->declare_parameter("pose_gate_dist", 10000.0)),
37-
pose_smoothing_steps(node->declare_parameter("pose_smoothing_steps", 5)),
38-
twist_additional_delay(node->declare_parameter("twist_additional_delay", 0.0)),
39-
twist_gate_dist(node->declare_parameter("twist_gate_dist", 10000.0)),
40-
twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)),
41-
proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)),
42-
proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)),
43-
proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)),
44-
z_filter_proc_dev(node->declare_parameter("z_filter_proc_dev", 1.0)),
45-
roll_filter_proc_dev(node->declare_parameter("roll_filter_proc_dev", 0.01)),
46-
pitch_filter_proc_dev(node->declare_parameter("pitch_filter_proc_dev", 0.01)),
30+
tf_rate_(node->declare_parameter<double>("node.tf_rate")),
31+
publish_tf_(node->declare_parameter<bool>("node.publish_tf")),
32+
enable_yaw_bias_estimation(node->declare_parameter<bool>("node.enable_yaw_bias_estimation")),
33+
extend_state_step(node->declare_parameter<int>("node.extend_state_step")),
34+
pose_frame_id(node->declare_parameter<std::string>("misc.pose_frame_id")),
35+
pose_additional_delay(
36+
node->declare_parameter<double>("pose_measurement.pose_additional_delay")),
37+
pose_gate_dist(node->declare_parameter<double>("pose_measurement.pose_gate_dist")),
38+
pose_smoothing_steps(node->declare_parameter<int>("pose_measurement.pose_smoothing_steps")),
39+
twist_additional_delay(
40+
node->declare_parameter<double>("twist_measurement.twist_additional_delay")),
41+
twist_gate_dist(node->declare_parameter<double>("twist_measurement.twist_gate_dist")),
42+
twist_smoothing_steps(node->declare_parameter<int>("twist_measurement.twist_smoothing_steps")),
43+
proc_stddev_vx_c(node->declare_parameter<double>("process_noise.proc_stddev_vx_c")),
44+
proc_stddev_wz_c(node->declare_parameter<double>("process_noise.proc_stddev_wz_c")),
45+
proc_stddev_yaw_c(node->declare_parameter<double>("process_noise.proc_stddev_yaw_c")),
46+
z_filter_proc_dev(
47+
node->declare_parameter<double>("simple_1d_filter_parameters.z_filter_proc_dev")),
48+
roll_filter_proc_dev(
49+
node->declare_parameter<double>("simple_1d_filter_parameters.roll_filter_proc_dev")),
50+
pitch_filter_proc_dev(
51+
node->declare_parameter<double>("simple_1d_filter_parameters.pitch_filter_proc_dev")),
4752
pose_no_update_count_threshold_warn(
48-
node->declare_parameter("pose_no_update_count_threshold_warn", 50)),
53+
node->declare_parameter<int>("diagnostics.pose_no_update_count_threshold_warn")),
4954
pose_no_update_count_threshold_error(
50-
node->declare_parameter("pose_no_update_count_threshold_error", 250)),
55+
node->declare_parameter<int>("diagnostics.pose_no_update_count_threshold_error")),
5156
twist_no_update_count_threshold_warn(
52-
node->declare_parameter("twist_no_update_count_threshold_warn", 50)),
57+
node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_warn")),
5358
twist_no_update_count_threshold_error(
54-
node->declare_parameter("twist_no_update_count_threshold_error", 250)),
59+
node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_error")),
5560
threshold_observable_velocity_mps(
56-
node->declare_parameter("threshold_observable_velocity_mps", 0.5))
61+
node->declare_parameter<double>("misc.threshold_observable_velocity_mps"))
5762
{
5863
}
5964

‎localization/ekf_localizer/launch/ekf_localizer.launch.xml

-2
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,6 @@
2525
<remap from="initialpose" to="$(var input_initial_pose_name)"/>
2626
<remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>
2727

28-
<param name="pose_frame_id" value="map"/>
29-
3028
<remap from="ekf_odom" to="$(var output_odom_name)"/>
3129
<remap from="ekf_pose" to="$(var output_pose_name)"/>
3230
<remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)"/>

‎localization/ekf_localizer/schema/ekf_localizer.schema.json

+52
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "EKF Localizer Configuration",
4+
"type": "object",
5+
"properties": {
6+
"/**": {
7+
"type": "object",
8+
"properties": {
9+
"ros__parameters": {
10+
"type": "object",
11+
"properties": {
12+
"node": {
13+
"$ref": "sub/node.sub_schema.json#/definitions/node"
14+
},
15+
"pose_measurement": {
16+
"$ref": "sub/pose_measurement.sub_schema.json#/definitions/pose_measurement"
17+
},
18+
"twist_measurement": {
19+
"$ref": "sub/twist_measurement.sub_schema.json#/definitions/twist_measurement"
20+
},
21+
"process_noise": {
22+
"$ref": "sub/process_noise.sub_schema.json#/definitions/process_noise"
23+
},
24+
"simple_1d_filter_parameters": {
25+
"$ref": "sub/simple_1d_filter_parameters.sub_schema.json#/definitions/simple_1d_filter_parameters"
26+
},
27+
"diagnostics": {
28+
"$ref": "sub/diagnostics.sub_schema.json#/definitions/diagnostics"
29+
},
30+
"misc": {
31+
"$ref": "sub/misc.sub_schema.json#/definitions/misc"
32+
}
33+
},
34+
"required": [
35+
"node",
36+
"pose_measurement",
37+
"twist_measurement",
38+
"process_noise",
39+
"simple_1d_filter_parameters",
40+
"diagnostics",
41+
"misc"
42+
],
43+
"additionalProperties": false
44+
}
45+
},
46+
"required": ["ros__parameters"],
47+
"additionalProperties": false
48+
}
49+
},
50+
"required": ["/**"],
51+
"additionalProperties": false
52+
}

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

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "EKF Localizer Configuration for Diagnostics",
4+
"definitions": {
5+
"diagnostics": {
6+
"type": "object",
7+
"properties": {
8+
"pose_no_update_count_threshold_warn": {
9+
"type": "integer",
10+
"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",
11+
"default": 50
12+
},
13+
"pose_no_update_count_threshold_error": {
14+
"type": "integer",
15+
"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",
16+
"default": 100
17+
},
18+
"twist_no_update_count_threshold_warn": {
19+
"type": "integer",
20+
"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",
21+
"default": 50
22+
},
23+
"twist_no_update_count_threshold_error": {
24+
"type": "integer",
25+
"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",
26+
"default": 100
27+
}
28+
},
29+
"required": [
30+
"pose_no_update_count_threshold_warn",
31+
"pose_no_update_count_threshold_error",
32+
"twist_no_update_count_threshold_warn",
33+
"twist_no_update_count_threshold_error"
34+
],
35+
"additionalProperties": false
36+
}
37+
}
38+
}

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

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "EKF Localizer Configuration for MISC",
4+
"definitions": {
5+
"misc": {
6+
"type": "object",
7+
"properties": {
8+
"threshold_observable_velocity_mps": {
9+
"type": "number",
10+
"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)",
11+
"default": 0.0
12+
},
13+
"pose_frame_id": {
14+
"type": "string",
15+
"description": "Parent frame_id of EKF output pose",
16+
"default": "map"
17+
}
18+
},
19+
"required": ["threshold_observable_velocity_mps", "pose_frame_id"],
20+
"additionalProperties": false
21+
}
22+
}
23+
}

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

+50
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "EKF Localizer Configuration for node",
4+
"definitions": {
5+
"node": {
6+
"type": "object",
7+
"properties": {
8+
"show_debug_info": {
9+
"type": "boolean",
10+
"description": "Flag to display debug info",
11+
"default": false
12+
},
13+
"predict_frequency": {
14+
"type": "number",
15+
"description": "Frequency for filtering and publishing [Hz]",
16+
"default": 50.0
17+
},
18+
"tf_rate": {
19+
"type": "number",
20+
"description": "Frequency for tf broadcasting [Hz]",
21+
"default": 50.0
22+
},
23+
"publish_tf": {
24+
"type": "boolean",
25+
"description": "Whether to publish tf",
26+
"default": true
27+
},
28+
"extend_state_step": {
29+
"type": "integer",
30+
"description": "Max delay step which can be dealt with in EKF. Large number increases computational cost.",
31+
"default": 50
32+
},
33+
"enable_yaw_bias_estimation": {
34+
"type": "boolean",
35+
"description": "Flag to enable yaw bias estimation",
36+
"default": true
37+
}
38+
},
39+
"required": [
40+
"show_debug_info",
41+
"predict_frequency",
42+
"tf_rate",
43+
"publish_tf",
44+
"extend_state_step",
45+
"enable_yaw_bias_estimation"
46+
],
47+
"additionalProperties": false
48+
}
49+
}
50+
}

0 commit comments

Comments
 (0)