Skip to content

Commit 8d261d7

Browse files
wakabameTaikiYamada4pre-commit-ci[bot]yn-mrse
authored
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 <taiki.yamada@tier4.jp> * Change the starting pose for DR to EKF odometry, even if the comparison target is ndt pose. Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Calculate on odometry callback not timer callback. This feature is for odometry messages only now. Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Added a checking code whether the twist buffer size is non-zero Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Removed timer related stuff Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Enable to output only diff_x, y, yaw Removed unnecessary logs Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Removed pose input stuff. Please use nav_msgs/msg/Odometry type messages for pose_instability_detector. Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Stop publishing diagnostics if the twist haven't been updated too long. Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Renew README.md Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * ci(pre-commit): autofix * Delete autoware_universe_utils from packages.xml Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> * Apply suggestions from code review Co-authored-by: Yuma Nihei <yuma.nihei@tier4.jp> * Update localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp Co-authored-by: Yuma Nihei <yuma.nihei@tier4.jp> --------- Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp> Co-authored-by: TaikiYamada4 <taiki.yamada@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yuma Nihei <yuma.nihei@tier4.jp>
1 parent cc1ae5f commit 8d261d7

16 files changed

+1421
-0
lines changed

‎localization/pose_instability_detector/CMakeLists.txt

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(pose_instability_detector)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
8+
src/pose_instability_detector.cpp
9+
)
10+
11+
rclcpp_components_register_node(${PROJECT_NAME}
12+
PLUGIN "PoseInstabilityDetector"
13+
EXECUTABLE ${PROJECT_NAME}_node
14+
EXECUTOR SingleThreadedExecutor
15+
)
16+
17+
if(BUILD_TESTING)
18+
find_package(ament_cmake_gtest REQUIRED)
19+
ament_auto_add_gtest(test_${PROJECT_NAME}
20+
test/test.cpp
21+
src/pose_instability_detector.cpp
22+
)
23+
endif()
24+
25+
ament_auto_package(
26+
INSTALL_TO_SHARE
27+
launch
28+
config
29+
)

‎localization/pose_instability_detector/README.md

+168
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
# pose_instability_detector
2+
3+
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).
4+
5+
This node triggers periodic timer callbacks to compare two poses:
6+
7+
- The pose calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago.
8+
- The latest pose from `/localization/kinematic_state`.
9+
10+
The results of this comparison are then output to the `/diagnostics` topic.
11+
12+
![overview](./media/pose_instability_detector_overview.png)
13+
14+
![rqt_runtime_monitor](./media/rqt_runtime_monitor.png)
15+
16+
If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values.
17+
In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the twist values.
18+
This discrepancy suggests that there may be an issue with either the estimated pose or the input twist.
19+
20+
The following diagram provides an overview of how the procedure looks like:
21+
22+
![procedure](./media/pose_instabilty_detector_procedure.svg)
23+
24+
## Dead reckoning algorithm
25+
26+
Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity.
27+
The procedure for dead reckoning is as follows:
28+
29+
1. Capture the necessary twist values from the `/input/twist` topic.
30+
2. Integrate the twist values to calculate the pose transition.
31+
3. Apply the pose transition to the previous pose to obtain the current pose.
32+
33+
### Collecting twist values
34+
35+
The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning.
36+
Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose.
37+
Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time.
38+
39+
![how_to_snip_necessary_twist](./media/how_to_snip_twist.png)
40+
41+
### Linear transition and angular transition
42+
43+
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.
44+
45+
## Threshold definition
46+
47+
The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output.
48+
These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation.
49+
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.
50+
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.
51+
52+
### `diff_position_x`
53+
54+
This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error.
55+
This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance".
56+
57+
$$
58+
\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\
59+
$$
60+
61+
| Symbol | Description | Unit |
62+
| ------------- | -------------------------------------------------------------------------------- | ----- |
63+
| $\tau_x$ | Threshold for the difference in the longitudinal axis | $m$ |
64+
| $v_{\rm max}$ | Maximum velocity | $m/s$ |
65+
| $\beta_v$ | Scale factor tolerance for the maximum velocity | $\%$ |
66+
| $\Delta t$ | Time interval | $s$ |
67+
| $\epsilon_x$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$ |
68+
69+
### `diff_position_y` and `diff_position_z`
70+
71+
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.
72+
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.
73+
74+
![lateral_threshold_calculation](./media/lateral_threshold_calculation.png)
75+
76+
Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold.
77+
78+
$$
79+
\tau_y = l + \epsilon_y
80+
$$
81+
82+
| Symbol | Description | Unit |
83+
| ------------ | ----------------------------------------------------------------------------------------------- | ---- |
84+
| $\tau_y$ | Threshold for the difference in the lateral axis | $m$ |
85+
| $l$ | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$ |
86+
| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis | $m$ |
87+
88+
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.
89+
90+
### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z`
91+
92+
These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses.
93+
This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance".
94+
95+
$$
96+
\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi
97+
$$
98+
99+
| Symbol | Description | Unit |
100+
| ------------------ | ------------------------------------------------------------------------ | ------------- |
101+
| $\tau_\phi$ | Threshold for the difference in the roll angle | ${\rm rad}$ |
102+
| $\tau_\theta$ | Threshold for the difference in the pitch angle | ${\rm rad}$ |
103+
| $\tau_\psi$ | Threshold for the difference in the yaw angle | ${\rm rad}$ |
104+
| $\omega_{\rm max}$ | Maximum angular velocity | ${\rm rad}/s$ |
105+
| $\beta_\omega$ | Scale factor tolerance for the maximum angular velocity | $\%$ |
106+
| $b$ | Bias tolerance of the angular velocity | ${\rm rad}/s$ |
107+
| $\Delta t$ | Time interval | $s$ |
108+
| $\epsilon_\psi$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$ |
109+
110+
## Parameters
111+
112+
{{ json_to_markdown("localization/pose_instability_detector/schema/pose_instability_detector.schema.json") }}
113+
114+
## Input
115+
116+
| Name | Type | Description |
117+
| ------------------ | ---------------------------------------------- | --------------------- |
118+
| `~/input/odometry` | nav_msgs::msg::Odometry | Pose estimated by EKF |
119+
| `~/input/twist` | geometry_msgs::msg::TwistWithCovarianceStamped | Twist |
120+
121+
## Output
122+
123+
| Name | Type | Description |
124+
| ------------------- | ------------------------------------- | ----------- |
125+
| `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose |
126+
| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics |
127+
128+
## Appendix
129+
130+
On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses.
131+
132+
| Pose | heading velocity $v$ | angular velocity $\omega$ |
133+
| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- |
134+
| Nominal dead reckoning pose | $v_{\rm max}$ | $\omega_{\rm max}$ |
135+
| 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$ |
136+
| 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$ |
137+
| 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$ |
138+
| 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$ |
139+
140+
Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows:
141+
142+
$$
143+
\begin{align*}
144+
\left[
145+
\begin{matrix}
146+
\Delta x\\
147+
\Delta y
148+
\end{matrix}
149+
\right]
150+
&=
151+
\left[
152+
\begin{matrix}
153+
\int_{0}^{\Delta t} v \cos(\omega t) dt\\
154+
\int_{0}^{\Delta t} v \sin(\omega t) dt
155+
\end{matrix}
156+
\right]
157+
\\
158+
&=
159+
\left[
160+
\begin{matrix}
161+
\frac{v}{\omega} \sin(\omega \Delta t)\\
162+
\frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right)
163+
\end{matrix}
164+
\right]
165+
\end{align*}
166+
$$
167+
168+
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

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
/**:
2+
ros__parameters:
3+
window_length: 0.5 # [sec]
4+
5+
output_x_y_yaw_only: true # true/false
6+
7+
heading_velocity_maximum: 2.778 # [m/s]
8+
heading_velocity_scale_factor_tolerance: 3.0 # [%]
9+
10+
angular_velocity_maximum: 0.523 # [rad/s]
11+
angular_velocity_scale_factor_tolerance: 0.2 # [%]
12+
angular_velocity_bias_tolerance: 0.00698 # [rad/s]
13+
14+
pose_estimator_longitudinal_tolerance: 0.11 # [m]
15+
pose_estimator_lateral_tolerance: 0.11 # [m]
16+
pose_estimator_vertical_tolerance: 0.11 # [m]
17+
pose_estimator_angular_tolerance: 0.0175 # [rad]

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

+107
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
// Copyright 2024- Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_
16+
#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
21+
#include <geometry_msgs/msg/pose_stamped.hpp>
22+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
23+
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
24+
#include <nav_msgs/msg/odometry.hpp>
25+
26+
#include <deque>
27+
#include <tuple>
28+
#include <vector>
29+
30+
class PoseInstabilityDetector : public rclcpp::Node
31+
{
32+
using Quaternion = geometry_msgs::msg::Quaternion;
33+
using Twist = geometry_msgs::msg::Twist;
34+
using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
35+
using Pose = geometry_msgs::msg::Pose;
36+
using PoseStamped = geometry_msgs::msg::PoseStamped;
37+
using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped;
38+
using Odometry = nav_msgs::msg::Odometry;
39+
using KeyValue = diagnostic_msgs::msg::KeyValue;
40+
using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus;
41+
using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
42+
43+
public:
44+
struct ThresholdValues
45+
{
46+
double position_x;
47+
double position_y;
48+
double position_z;
49+
double angle_x;
50+
double angle_y;
51+
double angle_z;
52+
};
53+
54+
explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
55+
ThresholdValues calculate_threshold(double interval_sec) const;
56+
static void dead_reckon(
57+
PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time,
58+
const std::deque<TwistWithCovarianceStamped> & twist_deque, Pose::SharedPtr & estimated_pose);
59+
60+
private:
61+
void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr);
62+
void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr);
63+
64+
static std::deque<TwistWithCovarianceStamped> clip_out_necessary_twist(
65+
const std::deque<TwistWithCovarianceStamped> & twist_buffer, const rclcpp::Time & start_time,
66+
const rclcpp::Time & end_time);
67+
68+
Pose inverseTransformPose(const Pose & pose, const Pose & transform_pose);
69+
70+
void updatePoseBuffer(const PoseStamped & pose);
71+
72+
std::optional<PoseStamped> getPoseAt(const rclcpp::Time & target_time);
73+
74+
std::tuple<double, double, double> quatToRPY(const Quaternion & quat);
75+
76+
// subscribers and timer
77+
rclcpp::Subscription<Odometry>::SharedPtr odometry_sub_;
78+
rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr twist_sub_;
79+
80+
// publisher
81+
rclcpp::Publisher<PoseStamped>::SharedPtr diff_pose_pub_;
82+
rclcpp::Publisher<DiagnosticArray>::SharedPtr diagnostics_pub_;
83+
84+
// parameters
85+
const double window_length_; // [sec]
86+
bool output_x_y_yaw_only_;
87+
88+
const double heading_velocity_maximum_; // [m/s]
89+
const double heading_velocity_scale_factor_tolerance_; // [%]
90+
91+
const double angular_velocity_maximum_; // [rad/s]
92+
const double angular_velocity_scale_factor_tolerance_; // [%]
93+
const double angular_velocity_bias_tolerance_; // [rad/s]
94+
95+
const double pose_estimator_longitudinal_tolerance_; // [m]
96+
const double pose_estimator_lateral_tolerance_; // [m]
97+
const double pose_estimator_vertical_tolerance_; // [m]
98+
const double pose_estimator_angular_tolerance_; // [rad]
99+
100+
// variables
101+
std::optional<Odometry> latest_odometry_ = std::nullopt;
102+
std::optional<Odometry> prev_odometry_ = std::nullopt;
103+
std::deque<TwistWithCovarianceStamped> twist_buffer_;
104+
std::deque<PoseStamped> pose_buffer_;
105+
};
106+
107+
#endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_

‎localization/pose_instability_detector/launch/pose_instability_detector.launch.xml

+14
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<launch>
2+
<arg name="node_name" default="pose_instability_detector"/>
3+
<arg name="param_file" default="$(find-pkg-share pose_instability_detector)/config/pose_instability_detector.param.yaml"/>
4+
5+
<!-- Topics -->
6+
<arg name="input_odometry" default="~/input/odometry"/>
7+
<arg name="input_twist" default="~/input/twist"/>
8+
9+
<node pkg="pose_instability_detector" exec="pose_instability_detector_node" name="$(var node_name)" output="both">
10+
<remap from="~/input/odometry" to="$(var input_odometry)"/>
11+
<remap from="~/input/twist" to="$(var input_twist)"/>
12+
<param from="$(var param_file)"/>
13+
</node>
14+
</launch>

‎localization/pose_instability_detector/media/how_to_snip_twist.png

175 KB
Loading

‎localization/pose_instability_detector/media/lateral_threshold_calculation.png

66.1 KB
Loading

‎localization/pose_instability_detector/media/pose_instability_detector_overview.png

133 KB
Loading

‎localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg

+4
Loading

‎localization/pose_instability_detector/media/rqt_runtime_monitor.png

84.9 KB
Loading

0 commit comments

Comments
 (0)