feat(pose_instability_detector): add pose_instability_detector module (…

…#1542)

* Add pose_instability_detector from awf/universe on 2024-08-07

Signed-off-by: TaikiYamada4 <[email protected]>

* Change the starting pose for DR to EKF odometry, even if the comparison target is ndt pose.

Signed-off-by: TaikiYamada4 <[email protected]>

* Calculate on odometry callback not timer callback.
This feature is for odometry messages only now.

Signed-off-by: TaikiYamada4 <[email protected]>

* Added a checking code whether the twist buffer size is non-zero

Signed-off-by: TaikiYamada4 <[email protected]>

* Removed timer related stuff

Signed-off-by: TaikiYamada4 <[email protected]>

* Enable to output only diff_x, y, yaw
Removed unnecessary logs

Signed-off-by: TaikiYamada4 <[email protected]>

* Removed pose input stuff.
Please use nav_msgs/msg/Odometry type messages for pose_instability_detector.

Signed-off-by: TaikiYamada4 <[email protected]>

* Stop publishing diagnostics if the twist haven't been updated too long.

Signed-off-by: TaikiYamada4 <[email protected]>

* Renew README.md

Signed-off-by: TaikiYamada4 <[email protected]>

* ci(pre-commit): autofix

* Delete autoware_universe_utils from packages.xml

Signed-off-by: TaikiYamada4 <[email protected]>

* Apply suggestions from code review

Co-authored-by: Yuma Nihei <[email protected]>

* Update localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp

Co-authored-by: Yuma Nihei <[email protected]>

---------

Signed-off-by: TaikiYamada4 <[email protected]>
Co-authored-by: TaikiYamada4 <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Yuma Nihei <[email protected]>

localization/pose_instability_detector/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.14)
+project(pose_instability_detector)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/pose_instability_detector.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "PoseInstabilityDetector"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_${PROJECT_NAME}
+    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,168 @@
+# pose_instability_detector
+
+The `pose_instability_detector` is 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 calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago.
+- The latest pose from `/localization/kinematic_state`.
+
+The results of this comparison are then output to the `/diagnostics` topic.
+
+![overview](./media/pose_instability_detector_overview.png)
+
+![rqt_runtime_monitor](./media/rqt_runtime_monitor.png)
+
+If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values.
+In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the 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 how the procedure looks like:
+
+![procedure](./media/pose_instabilty_detector_procedure.svg)
+
+## Dead reckoning algorithm
+
+Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity.
+The procedure for dead reckoning is as follows:
+
+1. Capture the necessary twist values from the `/input/twist` topic.
+2. Integrate the twist values to calculate the pose transition.
+3. Apply the pose transition to the previous pose to obtain the current pose.
+
+### Collecting twist values
+
+The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning.
+Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose.
+Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time.
+
+![how_to_snip_necessary_twist](./media/how_to_snip_twist.png)
+
+### Linear transition and angular transition
+
+After the twist values are collected, the node calculates the linear transition and angular transition based on the twist values and add them to the previous pose.
+
+## Threshold definition
+
+The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output.
+These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation.
+If these two poses are significantly different so that the absolute value exceeds the threshold, the node outputs a WARN message to the `/diagnostics` topic.
+There are six thresholds (x, y, z, roll, pitch, and yaw) to determine whether the poses are significantly different, and these thresholds are determined by the following subsections.
+
+### `diff_position_x`
+
+This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error.
+This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance".
+
+$$
+\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\
+$$
+
+| Symbol        | Description                                                                      | Unit  |
+| ------------- | -------------------------------------------------------------------------------- | ----- |
+| $\tau_x$      | Threshold for the difference in the longitudinal axis                            | $m$   |
+| $v_{\rm max}$ | Maximum velocity                                                                 | $m/s$ |
+| $\beta_v$     | Scale factor tolerance for the maximum velocity                                  | $\%$  |
+| $\Delta t$    | Time interval                                                                    | $s$   |
+| $\epsilon_x$  | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$   |
+
+### `diff_position_y` and `diff_position_z`
+
+These thresholds examine the difference in the lateral and vertical axes between the two poses, and check whether the vehicle goes beyond the expected error.
+The `pose_instability_detector` calculates the possible range where the vehicle goes, and get the maximum difference between the nominal dead reckoning pose and the maximum limit pose.
+
+![lateral_threshold_calculation](./media/lateral_threshold_calculation.png)
+
+Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold.
+
+$$
+\tau_y = l + \epsilon_y
+$$
+
+| Symbol       | Description                                                                                     | Unit |
+| ------------ | ----------------------------------------------------------------------------------------------- | ---- |
+| $\tau_y$     | Threshold for the difference in the lateral axis                                                | $m$  |
+| $l$          | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$  |
+| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis                     | $m$  |
+
+Note that `pose_instability_detector` sets the threshold for the vertical axis as the same as the lateral axis. Only the pose estimator error tolerance is different.
+
+### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z`
+
+These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses.
+This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance".
+
+$$
+\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi
+$$
+
+| Symbol             | Description                                                              | Unit          |
+| ------------------ | ------------------------------------------------------------------------ | ------------- |
+| $\tau_\phi$        | Threshold for the difference in the roll angle                           | ${\rm rad}$   |
+| $\tau_\theta$      | Threshold for the difference in the pitch angle                          | ${\rm rad}$   |
+| $\tau_\psi$        | Threshold for the difference in the yaw angle                            | ${\rm rad}$   |
+| $\omega_{\rm max}$ | Maximum angular velocity                                                 | ${\rm rad}/s$ |
+| $\beta_\omega$     | Scale factor tolerance for the maximum angular velocity                  | $\%$          |
+| $b$                | Bias tolerance of the angular velocity                                   | ${\rm rad}/s$ |
+| $\Delta t$         | Time interval                                                            | $s$           |
+| $\epsilon_\psi$    | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$   |
+
+## 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 |
+
+## Appendix
+
+On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses.
+
+| Pose                            | heading velocity $v$                             | angular velocity $\omega$                                      |
+| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- |
+| Nominal dead reckoning pose     | $v_{\rm max}$                                    | $\omega_{\rm max}$                                             |
+| Dead reckoning pose of corner A | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ |
+| Dead reckoning pose of corner B | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ |
+| Dead reckoning pose of corner C | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ |
+| Dead reckoning pose of corner D | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ |
+
+Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows:
+
+$$
+\begin{align*}
+\left[
+    \begin{matrix}
+    \Delta x\\
+    \Delta y
+    \end{matrix}
+\right]
+&=
+\left[
+    \begin{matrix}
+    \int_{0}^{\Delta t} v \cos(\omega t) dt\\
+    \int_{0}^{\Delta t} v \sin(\omega t) dt
+    \end{matrix}
+\right]
+\\
+&=
+\left[
+    \begin{matrix}
+    \frac{v}{\omega} \sin(\omega \Delta t)\\
+    \frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right)
+    \end{matrix}
+\right]
+\end{align*}
+$$
+
+We calculate this variation for each corner and get the maximum value of the lateral distance $l$ by comparing the distance between the nominal dead reckoning pose and the corner poses.

localization/pose_instability_detector/config/pose_instability_detector.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    window_length: 0.5                            # [sec]
+
+    output_x_y_yaw_only: true                     # true/false
+
+    heading_velocity_maximum: 2.778               # [m/s]
+    heading_velocity_scale_factor_tolerance: 3.0  # [%]
+
+    angular_velocity_maximum: 0.523               # [rad/s]
+    angular_velocity_scale_factor_tolerance: 0.2  # [%]
+    angular_velocity_bias_tolerance: 0.00698      # [rad/s]
+
+    pose_estimator_longitudinal_tolerance: 0.11   # [m]
+    pose_estimator_lateral_tolerance: 0.11        # [m]
+    pose_estimator_vertical_tolerance: 0.11       # [m]
+    pose_estimator_angular_tolerance: 0.0175      # [rad]

localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp

@@ -0,0 +1,107 @@
+// Copyright 2024- Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_
+#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <deque>
+#include <tuple>
+#include <vector>
+
+class PoseInstabilityDetector : public rclcpp::Node
+{
+  using Quaternion = geometry_msgs::msg::Quaternion;
+  using Twist = geometry_msgs::msg::Twist;
+  using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
+  using Pose = geometry_msgs::msg::Pose;
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
+  using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using Odometry = nav_msgs::msg::Odometry;
+  using KeyValue = diagnostic_msgs::msg::KeyValue;
+  using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus;
+  using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
+
+public:
+  struct ThresholdValues
+  {
+    double position_x;
+    double position_y;
+    double position_z;
+    double angle_x;
+    double angle_y;
+    double angle_z;
+  };
+
+  explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  ThresholdValues calculate_threshold(double interval_sec) const;
+  static void dead_reckon(
+    PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time,
+    const std::deque<TwistWithCovarianceStamped> & twist_deque, Pose::SharedPtr & estimated_pose);
+
+private:
+  void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr);
+  void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr);
+
+  static std::deque<TwistWithCovarianceStamped> clip_out_necessary_twist(
+    const std::deque<TwistWithCovarianceStamped> & twist_buffer, const rclcpp::Time & start_time,
+    const rclcpp::Time & end_time);
+
+  Pose inverseTransformPose(const Pose & pose, const Pose & transform_pose);
+
+  void updatePoseBuffer(const PoseStamped & pose);
+
+  std::optional<PoseStamped> getPoseAt(const rclcpp::Time & target_time);
+
+  std::tuple<double, double, double> quatToRPY(const Quaternion & quat);
+
+  // subscribers and timer
+  rclcpp::Subscription<Odometry>::SharedPtr odometry_sub_;
+  rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr twist_sub_;
+
+  // publisher
+  rclcpp::Publisher<PoseStamped>::SharedPtr diff_pose_pub_;
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr diagnostics_pub_;
+
+  // parameters
+  const double window_length_;  // [sec]
+  bool output_x_y_yaw_only_;
+
+  const double heading_velocity_maximum_;                 // [m/s]
+  const double heading_velocity_scale_factor_tolerance_;  // [%]
+
+  const double angular_velocity_maximum_;                 // [rad/s]
+  const double angular_velocity_scale_factor_tolerance_;  // [%]
+  const double angular_velocity_bias_tolerance_;          // [rad/s]
+
+  const double pose_estimator_longitudinal_tolerance_;  // [m]
+  const double pose_estimator_lateral_tolerance_;       // [m]
+  const double pose_estimator_vertical_tolerance_;      // [m]
+  const double pose_estimator_angular_tolerance_;       // [rad]
+
+  // variables
+  std::optional<Odometry> latest_odometry_ = std::nullopt;
+  std::optional<Odometry> prev_odometry_ = std::nullopt;
+  std::deque<TwistWithCovarianceStamped> twist_buffer_;
+  std::deque<PoseStamped> pose_buffer_;
+};
+
+#endif  // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_

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_node" name="$(var node_name)" output="both">
+    <remap from="~/input/odometry" to="$(var input_odometry)"/>
+    <remap from="~/input/twist" to="$(var input_twist)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>