From 6a81ed5520477390c432663785e647f3da75363a Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 20 Sep 2023 16:21:45 +0900 Subject: [PATCH] feat(ekf_localizer): ignore dead band of velocity sensor (#5042) * feat(ekf_localizer): ignore dead band of velocity sensor Signed-off-by: kminoda <koji.minoda@tier4.jp> * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update stop_filter as well Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ekf_localizer/README.md | 6 ++++++ localization/ekf_localizer/config/ekf_localizer.param.yaml | 3 +++ .../include/ekf_localizer/hyper_parameters.hpp | 5 ++++- localization/ekf_localizer/src/ekf_localizer.cpp | 5 +++++ localization/stop_filter/CMakeLists.txt | 1 + localization/stop_filter/config/stop_filter.param.yaml | 4 ++++ localization/stop_filter/launch/stop_filter.launch.xml | 6 ++---- localization/stop_filter/src/stop_filter.cpp | 4 ++-- 8 files changed, 27 insertions(+), 7 deletions(-) create mode 100644 localization/stop_filter/config/stop_filter.param.yaml diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 977e0fceafd9e..6492f20331a66 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -161,6 +161,12 @@ note: process noise for positions x & y are calculated automatically from nonlin | 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 | | 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 | +### Misc + +| Name | Type | Description | Default value | +| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- | +| 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) | + ## How to tune EKF parameters ### 0. Preliminaries diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 8b24b79e71829..667217d2591dc 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -27,3 +27,6 @@ pose_no_update_count_threshold_error: 250 twist_no_update_count_threshold_warn: 50 twist_no_update_count_threshold_error: 250 + + # 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 9fa877c8fd2f6..01ef658cf445d 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -47,7 +47,9 @@ class HyperParameters 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("twist_no_update_count_threshold_error", 250)) + node->declare_parameter("twist_no_update_count_threshold_error", 250)), + threshold_observable_velocity_mps( + node->declare_parameter("threshold_observable_velocity_mps", 0.5)) { } @@ -71,6 +73,7 @@ class HyperParameters const size_t pose_no_update_count_threshold_error; const size_t twist_no_update_count_threshold_warn; const size_t twist_no_update_count_threshold_error; + const double threshold_observable_velocity_mps; }; #endif // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 68a31bcdded1a..3c3c38dcb1561 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -385,6 +385,11 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + // Ignore twist if velocity is too small. + // Note that this inequality must not include "equal". + if (msg->twist.twist.linear.x < params_.threshold_observable_velocity_mps) { + msg->twist.covariance[0 * 6 + 0] = 10000.0; + } twist_queue_.push(msg); } diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 2d1867b8cd0bc..97a0443195ae5 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -13,4 +13,5 @@ ament_target_dependencies(stop_filter) ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/localization/stop_filter/config/stop_filter.param.yaml b/localization/stop_filter/config/stop_filter.param.yaml new file mode 100644 index 0000000000000..ded090b75b5bd --- /dev/null +++ b/localization/stop_filter/config/stop_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + vx_threshold: 0.1 # [m/s] + wz_threshold: 0.02 # [rad/s] diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 36a66a2c143c0..0ea92d26c9677 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -1,6 +1,5 @@ <launch> - <arg name="vx_threshold" default="0.01" description="[m/s]"/> - <arg name="wz_threshold" default="0.01" description="[rad/s]"/> + <arg name="param_path" default="$(find-pkg-share stop_filter)/config/stop_filter.param.yaml"/> <arg name="input_odom_name" default="ekf_odom"/> <arg name="output_odom_name" default="stop_filter_odom"/> <arg name="debug_stop_flag" default="debug/stop_flag"/> @@ -10,7 +9,6 @@ <remap from="output/odom" to="$(var output_odom_name)"/> <remap from="debug/stop_flag" to="$(var debug_stop_flag)"/> - <param name="vx_threshold" value="$(var vx_threshold)"/> - <param name="wz_threshold" value="$(var wz_threshold)"/> + <param from="$(var param_path)"/> </node> </launch> diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 111d460be737e..ac0960b8cb67b 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -27,8 +27,8 @@ using std::placeholders::_1; StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) : rclcpp::Node(node_name, node_options) { - vx_threshold_ = declare_parameter("vx_threshold", 0.01); - wz_threshold_ = declare_parameter("wz_threshold", 0.01); + vx_threshold_ = declare_parameter<double>("vx_threshold"); + wz_threshold_ = declare_parameter<double>("wz_threshold"); sub_odom_ = create_subscription<nav_msgs::msg::Odometry>( "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1));