feat(localization): add pose_instability_detector (#5439)

* Added pose_instability_detector

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Renamed files

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed parameter name

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed to launch

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed to run normally

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed to publish diagnostics

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed a variable name

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed Copyright

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added test

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added maintainer

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added maintainer

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Removed log output

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Modified test

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed comment

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added a test case

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added set_first_odometry_

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Refactored test

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed test

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed topic name

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed position

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added twist message2

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed launch

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Updated README.md

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* style(pre-commit): autofix

* Fixed as pointed out by clang-tidy

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Renamed parameters

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed timer

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed README.md

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added debug publishers

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed parameters

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* style(pre-commit): autofix

* Fixed tests

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Changed the type of ekf_to_odom and add const

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed DiagnosticStatus

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Changed odometry_data to std::optional

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Refactored debug output in pose instability detector

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* style(pre-commit): autofix

* Remove warning message for negative time
difference in PoseInstabilityDetector

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Updated rqt_runtime_monitor.png

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -33,4 +33,12 @@
       <arg name="out_accel" value="/localization/acceleration"/>
     </include>
   </group>
+
+  <group>
+    <include file="$(find-pkg-share pose_instability_detector)/launch/pose_instability_detector.launch.xml">
+      <arg name="input_odometry" value="/localization/kinematic_state"/>
+      <arg name="input_twist" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="param_file" value="$(find-pkg-share pose_instability_detector)/config/pose_instability_detector.param.yaml"/>
+    </include>
+  </group>
 </launch>

launch/tier4_localization_launch/package.xml

@@ -25,6 +25,7 @@
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pose_initializer</exec_depend>
+  <exec_depend>pose_instability_detector</exec_depend>
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>yabloc_common</exec_depend>
   <exec_depend>yabloc_image_processing</exec_depend>

localization/pose_instability_detector/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.14)
+project(pose_instability_detector)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(pose_instability_detector
+  src/main.cpp
+  src/pose_instability_detector.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_pose_instability_detector
+    test/test.cpp
+    src/pose_instability_detector.cpp
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

localization/pose_instability_detector/README.md

@@ -0,0 +1,37 @@
+# pose_instability_detector
+
+The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF).
+
+This node triggers periodic timer callbacks to compare two poses:
+
+- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`.
+- The latest pose from `/localization/kinematic_state`.
+
+The results of this comparison are then output to the `/diagnostics` topic.
+
+If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values.
+This discrepancy suggests that there may be an issue with either the estimated pose or the input twist.
+
+The following diagram provides an overview of what the timeline of this process looks like:
+
+![timeline](./media/timeline.drawio.svg)
+
+## Parameters
+
+{{ json_to_markdown("localization/pose_instability_detector/schema/pose_instability_detector.schema.json") }}
+
+## Input
+
+| Name               | Type                                           | Description           |
+| ------------------ | ---------------------------------------------- | --------------------- |
+| `~/input/odometry` | nav_msgs::msg::Odometry                        | Pose estimated by EKF |
+| `~/input/twist`    | geometry_msgs::msg::TwistWithCovarianceStamped | Twist                 |
+
+## Output
+
+| Name                | Type                                  | Description |
+| ------------------- | ------------------------------------- | ----------- |
+| `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped       | diff_pose   |
+| `/diagnostics`      | diagnostic_msgs::msg::DiagnosticArray | Diagnostics |
+
+![rqt_runtime_monitor](./media/rqt_runtime_monitor.png)

localization/pose_instability_detector/config/pose_instability_detector.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    interval_sec: 1.0  # [sec]
+    threshold_diff_position_x: 1.0  # [m]
+    threshold_diff_position_y: 1.0  # [m]
+    threshold_diff_position_z: 1.0  # [m]
+    threshold_diff_angle_x: 1.0  # [rad]
+    threshold_diff_angle_y: 1.0  # [rad]
+    threshold_diff_angle_z: 1.0  # [rad]

localization/pose_instability_detector/launch/pose_instability_detector.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <arg name="node_name" default="pose_instability_detector"/>
+  <arg name="param_file" default="$(find-pkg-share pose_instability_detector)/config/pose_instability_detector.param.yaml"/>
+
+  <!-- Topics -->
+  <arg name="input_odometry" default="~/input/odometry"/>
+  <arg name="input_twist" default="~/input/twist"/>
+
+  <node pkg="pose_instability_detector" exec="pose_instability_detector" name="$(var node_name)" output="log">
+    <remap from="~/input/odometry" to="$(var input_odometry)"/>
+    <remap from="~/input/twist" to="$(var input_twist)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

localization/pose_instability_detector/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pose_instability_detector</name>
+  <version>0.1.0</version>
+  <description>The pose_instability_detector package</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
+
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>