Skip to content

Commit 76d11bb

Browse files
committed
refactor: Remove default value from source code and launch.xml
with arrangement of param .yaml Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp>
1 parent 4c5b559 commit 76d11bb

File tree

4 files changed

+74
-57
lines changed

4 files changed

+74
-57
lines changed
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/sub/misc.sub_schema.json

+7-1
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,16 @@
99
"type": "number",
1010
"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)",
1111
"default": 0.0
12+
},
13+
"pose_frame_id": {
14+
"type": "string",
15+
"description": "Parent frame_id of EKF output pose",
16+
"default": "map"
1217
}
1318
},
1419
"required": [
15-
"threshold_observable_velocity_mps"
20+
"threshold_observable_velocity_mps",
21+
"pose_frame_id"
1622
],
1723
"additionalProperties": false
1824
}

0 commit comments

Comments
 (0)