From 7bc163b9fd96becfd759ce2227e638b029fdbeb3 Mon Sep 17 00:00:00 2001 From: Motsu-san Date: Mon, 29 Jan 2024 11:24:40 +0900 Subject: [PATCH] refactor: Add the classification names to yaml file Signed-off-by: Motsu-san --- .../config/ekf_localizer.param.yaml | 67 ++++++++++--------- .../ekf_localizer/hyper_parameters.hpp | 51 +++++++------- 2 files changed, 65 insertions(+), 53 deletions(-) diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 9de7f56f5c006..d465fa77cdcae 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,38 +1,45 @@ /**: 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] diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 5139f900a339e..2b40aa0b313ed 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)), + 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("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)), + 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")) { }