From 20bc643d507eb90678912ad5736a9cf4015d57d0 Mon Sep 17 00:00:00 2001 From: Motsu-san Date: Fri, 2 Feb 2024 06:38:53 +0900 Subject: [PATCH] Revert "refactor: Add the classification names to yaml file" This reverts commit 5d11b723dc994dc279164dd690ff9d1d73592d03. --- .../config/ekf_localizer.param.yaml | 67 +++++++++---------- .../ekf_localizer/hyper_parameters.hpp | 51 +++++++------- 2 files changed, 53 insertions(+), 65 deletions(-) diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index d465fa77cdcae..9de7f56f5c006 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,45 +1,38 @@ /**: ros__parameters: - 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 + show_debug_info: false + enable_yaw_bias_estimation: true + predict_frequency: 50.0 + tf_rate: 50.0 + publish_tf: true + extend_state_step: 50 - 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 Pose measurement + pose_additional_delay: 0.0 + pose_measure_uncertainty_time: 0.01 + pose_smoothing_steps: 5 + pose_gate_dist: 10000.0 - twist_measurement: - # for twist measurement - twist_additional_delay: 0.0 - twist_smoothing_steps: 2 - twist_gate_dist: 10000.0 + # for twist measurement + twist_additional_delay: 0.0 + twist_smoothing_steps: 2 + twist_gate_dist: 10000.0 - process_noise: - # for process model - proc_stddev_yaw_c: 0.005 - proc_stddev_vx_c: 10.0 - proc_stddev_wz_c: 5.0 + # for process model + proc_stddev_yaw_c: 0.005 + proc_stddev_vx_c: 10.0 + proc_stddev_wz_c: 5.0 - simple_1d_filter_parameters: - #Simple1DFilter parameters - z_filter_proc_dev: 1.0 - roll_filter_proc_dev: 0.01 - pitch_filter_proc_dev: 0.01 + #Simple1DFilter parameters + z_filter_proc_dev: 1.0 + roll_filter_proc_dev: 0.01 + pitch_filter_proc_dev: 0.01 - 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 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 - misc: - # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] + # 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 2b40aa0b313ed..5139f900a339e 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -24,41 +24,36 @@ class HyperParameters { public: explicit HyperParameters(rclcpp::Node * node) - : show_debug_info(node->declare_parameter("node.show_debug_info")), - ekf_rate(node->declare_parameter("node.predict_frequency")), + : show_debug_info(node->declare_parameter("show_debug_info", false)), + ekf_rate(node->declare_parameter("predict_frequency", 50.0)), ekf_dt(1.0 / std::max(ekf_rate, 0.1)), - 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")), + 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_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_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_no_update_count_threshold_warn( - node->declare_parameter("diagnostics.pose_no_update_count_threshold_warn")), + node->declare_parameter("pose_no_update_count_threshold_warn", 50)), pose_no_update_count_threshold_error( - node->declare_parameter("diagnostics.pose_no_update_count_threshold_error")), + node->declare_parameter("pose_no_update_count_threshold_error", 250)), twist_no_update_count_threshold_warn( - node->declare_parameter("diagnostics.twist_no_update_count_threshold_warn")), + node->declare_parameter("twist_no_update_count_threshold_warn", 50)), twist_no_update_count_threshold_error( - node->declare_parameter("diagnostics.twist_no_update_count_threshold_error")), + node->declare_parameter("twist_no_update_count_threshold_error", 250)), threshold_observable_velocity_mps( - node->declare_parameter("misc.threshold_observable_velocity_mps")) + node->declare_parameter("threshold_observable_velocity_mps", 0.5)) { }