feat(localization): add pose_instability_detector (#5439)

* Added pose_instability_detector Signed-off-by: Shintaro Sakoda <[email protected]> * Renamed files Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed parameter name Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to launch Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to run normally Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to publish diagnostics Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed a variable name Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed Copyright Signed-off-by: Shintaro Sakoda <[email protected]> * Added test Signed-off-by: Shintaro Sakoda <[email protected]> * Added maintainer Signed-off-by: Shintaro Sakoda <[email protected]> * Added maintainer Signed-off-by: Shintaro Sakoda <[email protected]> * Removed log output Signed-off-by: Shintaro Sakoda <[email protected]> * Modified test Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed comment Signed-off-by: Shintaro Sakoda <[email protected]> * Added a test case Signed-off-by: Shintaro Sakoda <[email protected]> * Added set_first_odometry_ Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored test Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed test Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed topic name Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed position Signed-off-by: Shintaro Sakoda <[email protected]> * Added twist message2 Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed launch Signed-off-by: Shintaro Sakoda <[email protected]> * Updated README.md Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed as pointed out by clang-tidy Signed-off-by: Shintaro Sakoda <[email protected]> * Renamed parameters Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed timer Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed README.md Signed-off-by: Shintaro Sakoda <[email protected]> * Added debug publishers Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed parameters Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed tests Signed-off-by: Shintaro Sakoda <[email protected]> * Changed the type of ekf_to_odom and add const Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed DiagnosticStatus Signed-off-by: Shintaro Sakoda <[email protected]> * Changed odometry_data to std::optional Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored debug output in pose instability detector Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Remove warning message for negative time difference in PoseInstabilityDetector Signed-off-by: Shintaro Sakoda <[email protected]> * Updated rqt_runtime_monitor.png Signed-off-by: Shintaro SAKODA <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: Shintaro SAKODA <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Nov 15, 2023 · 765a596 · 765a596
1 parent 846a1cd
commit 765a596
Show file tree

Hide file tree

Showing 15 changed files with 879 additions and 0 deletions.
diff --git a/...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/...4_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>
diff --git a/launch/tier4_localization_launch/package.xml b/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>

diff --git a/localization/pose_instability_detector/CMakeLists.txt b/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
+)
diff --git a/localization/pose_instability_detector/README.md b/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)
diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/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]
diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/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>
diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png
diff --git a/localization/pose_instability_detector/media/timeline.drawio.svg b/localization/pose_instability_detector/media/timeline.drawio.svg
diff --git a/localization/pose_instability_detector/package.xml b/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="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Kento Yabuuchi</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Ryu Yamamoto</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">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>