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));