From 97e63c65d2cfa4d44b0643a4c3850800d5d32f50 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 20 Sep 2023 10:08:49 +0900 Subject: [PATCH 1/6] feat(ekf_localizer): ignore dead band of velocity sensor Signed-off-by: kminoda --- localization/ekf_localizer/config/ekf_localizer.param.yaml | 3 +++ .../ekf_localizer/include/ekf_localizer/hyper_parameters.hpp | 5 ++++- localization/ekf_localizer/src/ekf_localizer.cpp | 5 +++++ 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 4d3f5b9643462..3e83b72da8cbd 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -21,3 +21,6 @@ proc_stddev_yaw_c: 0.005 proc_stddev_vx_c: 10.0 proc_stddev_wz_c: 5.0 + + # 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 c74fa9be79525..1500667ca0e11 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -39,7 +39,9 @@ class HyperParameters 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)) + proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)), + threshold_observable_velocity_mps( + node->declare_parameter("threshold_observable_velocity_mps", 0.5)) { } @@ -59,6 +61,7 @@ class HyperParameters const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise + 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 0e46a26add852..7e640c4af1150 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -353,6 +353,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) { + return; + } twist_queue_.push(msg); } From 54d2e8a0ef2f648ca8c9a4037c978772a978711d Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 20 Sep 2023 10:09:46 +0900 Subject: [PATCH 2/6] update Signed-off-by: kminoda --- localization/ekf_localizer/src/ekf_localizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 7e640c4af1150..e283ddf9f3a89 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -356,7 +356,7 @@ void EKFLocalizer::callbackTwistWithCovariance( // 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) { - return; + msg->twist.covariance[0 * 6 + 0] = 10000.0; } twist_queue_.push(msg); } From a17e88fdbca66a7e796e8a7a1def70120dc072df Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 20 Sep 2023 10:11:45 +0900 Subject: [PATCH 3/6] update readme Signed-off-by: kminoda --- localization/ekf_localizer/README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 748b5ee5becc0..40e581b8e99a8 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -148,6 +148,12 @@ The parameters are set in `launch/ekf_localizer.launch` . note: process noise for positions x & y are calculated automatically from nonlinear dynamics. +### 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 From c6a87a8bdd7a4f0c323a3ea7585c3dcadba5ac11 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 20 Sep 2023 01:22:20 +0000 Subject: [PATCH 4/6] style(pre-commit): autofix --- localization/ekf_localizer/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 40e581b8e99a8..b46d23e386ec2 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -150,9 +150,9 @@ note: process noise for positions x & y are calculated automatically from nonlin ### 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) | +| 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 From 3a0fa48328a3e21b37f6445e4e90d6d3fe404b07 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 20 Sep 2023 11:18:06 +0900 Subject: [PATCH 5/6] update stop_filter as well Signed-off-by: kminoda --- 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 ++-- 4 files changed, 9 insertions(+), 6 deletions(-) create mode 100644 localization/stop_filter/config/stop_filter.param.yaml 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 @@ - - + @@ -10,7 +9,6 @@ - - + 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("vx_threshold"); + wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); From d13c107718325c6238f5ace64a1914eb68341da0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 20 Sep 2023 02:38:53 +0000 Subject: [PATCH 6/6] style(pre-commit): autofix --- localization/ekf_localizer/config/ekf_localizer.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index f23b8b4c0e811..667217d2591dc 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -29,4 +29,4 @@ 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] \ No newline at end of file + threshold_observable_velocity_mps: 0.0 # [m/s]