Skip to content

Commit

Permalink
refactor: Remove default value from source code and launch.xml
Browse files Browse the repository at this point in the history 
with arrangement of param .yaml

Signed-off-by: Motsu-san <[email protected]>
  • Loading branch information
@Motsu-san
Motsu-san committed Feb 5, 2024
1 parent 4c5b559 commit 76d11bb
Show file tree
Hide file tree
Showing 4 changed files with 74 additions and 57 deletions.
68 changes: 38 additions & 30 deletions localization/ekf_localizer/config/ekf_localizer.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -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"
53 changes: 29 additions & 24 deletions localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool>("node.show_debug_info")),
ekf_rate(node->declare_parameter<double>("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<double>("node.tf_rate")),
publish_tf_(node->declare_parameter<bool>("node.publish_tf")),
enable_yaw_bias_estimation(node->declare_parameter<bool>("node.enable_yaw_bias_estimation")),
extend_state_step(node->declare_parameter<int>("node.extend_state_step")),
pose_frame_id(node->declare_parameter<std::string>("misc.pose_frame_id")),
pose_additional_delay(
node->declare_parameter<double>("pose_measurement.pose_additional_delay")),
pose_gate_dist(node->declare_parameter<double>("pose_measurement.pose_gate_dist")),
pose_smoothing_steps(node->declare_parameter<int>("pose_measurement.pose_smoothing_steps")),
twist_additional_delay(
node->declare_parameter<double>("twist_measurement.twist_additional_delay")),
twist_gate_dist(node->declare_parameter<double>("twist_measurement.twist_gate_dist")),
twist_smoothing_steps(node->declare_parameter<int>("twist_measurement.twist_smoothing_steps")),
proc_stddev_vx_c(node->declare_parameter<double>("process_noise.proc_stddev_vx_c")),
proc_stddev_wz_c(node->declare_parameter<double>("process_noise.proc_stddev_wz_c")),
proc_stddev_yaw_c(node->declare_parameter<double>("process_noise.proc_stddev_yaw_c")),
z_filter_proc_dev(
node->declare_parameter<double>("simple_1d_filter_parameters.z_filter_proc_dev")),
roll_filter_proc_dev(
node->declare_parameter<double>("simple_1d_filter_parameters.roll_filter_proc_dev")),
pitch_filter_proc_dev(
node->declare_parameter<double>("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<int>("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<int>("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<int>("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<int>("diagnostics.twist_no_update_count_threshold_error")),
threshold_observable_velocity_mps(
node->declare_parameter("threshold_observable_velocity_mps", 0.5))
node->declare_parameter<double>("misc.threshold_observable_velocity_mps"))
{
}

Expand Down
2 changes: 0 additions & 2 deletions localization/ekf_localizer/launch/ekf_localizer.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,6 @@
<remap from="initialpose" to="$(var input_initial_pose_name)"/>
<remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>

<param name="pose_frame_id" value="map"/>

<remap from="ekf_odom" to="$(var output_odom_name)"/>
<remap from="ekf_pose" to="$(var output_pose_name)"/>
<remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)"/>
Expand Down
8 changes: 7 additions & 1 deletion localization/ekf_localizer/schema/sub/misc.sub_schema.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down

0 comments on commit 76d11bb

Please sign in to comment.