diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt new file mode 100644 index 0000000000000..c6f94ab7df16e --- /dev/null +++ b/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 +) diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md new file mode 100644 index 0000000000000..4ced0fa8eb97b --- /dev/null +++ b/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. diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml new file mode 100644 index 0000000000000..90ace2250f5b2 --- /dev/null +++ b/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] diff --git a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp new file mode 100644 index 0000000000000..9e2410cbfc8ea --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include + +#include +#include +#include + +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 & 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 clip_out_necessary_twist( + const std::deque & 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 getPoseAt(const rclcpp::Time & target_time); + + std::tuple quatToRPY(const Quaternion & quat); + + // subscribers and timer + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + + // publisher + rclcpp::Publisher::SharedPtr diff_pose_pub_; + rclcpp::Publisher::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 latest_odometry_ = std::nullopt; + std::optional prev_odometry_ = std::nullopt; + std::deque twist_buffer_; + std::deque pose_buffer_; +}; + +#endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml new file mode 100644 index 0000000000000..5ebe7d7e429e0 --- /dev/null +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/pose_instability_detector/media/how_to_snip_twist.png new file mode 100644 index 0000000000000..6dc66a480e769 Binary files /dev/null and b/localization/pose_instability_detector/media/how_to_snip_twist.png differ diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/pose_instability_detector/media/lateral_threshold_calculation.png new file mode 100644 index 0000000000000..cd919cdcb0383 Binary files /dev/null and b/localization/pose_instability_detector/media/lateral_threshold_calculation.png differ diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/pose_instability_detector/media/pose_instability_detector_overview.png new file mode 100644 index 0000000000000..ea8f7a81700ae Binary files /dev/null and b/localization/pose_instability_detector/media/pose_instability_detector_overview.png differ diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg new file mode 100644 index 0000000000000..9f45778441907 --- /dev/null +++ b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg @@ -0,0 +1,4 @@ + + + +
Get necessary subsequence
from twist buffer
Calculate integration of
the twist subsequence
Calculate relative difference between
latest pose
and
previous pose + twist integration
Compare pose difference with thresholds
Output results as /diagnostics
Store odometry msg to odometry buffer
Odometry Subscription Callback
Get previous pose from odometry buffer
that is window_length seconds before 
\ No newline at end of file diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png new file mode 100644 index 0000000000000..9bad665db17e5 Binary files /dev/null and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/media/timeline.drawio.svg b/localization/pose_instability_detector/media/timeline.drawio.svg new file mode 100644 index 0000000000000..8fcdef3401025 --- /dev/null +++ b/localization/pose_instability_detector/media/timeline.drawio.svg @@ -0,0 +1,157 @@ + + + + + + + + Time + + + + + + timer_callback + + + + + + timer_callback + + + + + + prev_ekf + + + + + + latest_ekf + + + + + + ekf(ignored) + + + + + 1 sec + + + + + + ekf(ignored) + + + + + + ekf(ignored) + + + + ... + + + + About ekf + + + + + + Time + + + + + + timer_callback + + + + + + timer_callback + + + + + + prev_ekf + + + + + + latest_ekf + + + + + + twist + + + + + 1 sec + + + + + + twist + + + + + + twist + + + + ... + + + + About twist + + + + + + twist + (ignored) + + + diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml new file mode 100644 index 0000000000000..c0e1226c7fca7 --- /dev/null +++ b/localization/pose_instability_detector/package.xml @@ -0,0 +1,34 @@ + + + + pose_instability_detector + 0.1.0 + The pose_instability_detector package + Yamato Ando + Kento Yabuuchi + Masahiro Sakamoto + Taiki Yamada + Ryu Yamamoto + Shintaro Sakoda + NGUYEN Viet Anh + Apache License 2.0 + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + ament_cmake_cppcheck + ament_index_cpp + ament_lint_auto + diagnostic_msgs + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + + + ament_cmake + + diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json new file mode 100644 index 0000000000000..8a20e3973a9fc --- /dev/null +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -0,0 +1,102 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pose Instability Detector Node", + "type": "object", + "definitions": { + "pose_instability_detector_node": { + "type": "object", + "properties": { + "window_length": { + "type": "number", + "default": 0.5, + "exclusiveMinimum": 0, + "description": "How many seconds before to start dead reckoning (sec)" + }, + "output_x_y_yaw_only": { + "type": "boolean", + "default": true, + "description": "Whether to output diff_x, diff_y, diff_yaw only" + }, + "heading_velocity_maximum": { + "type": "number", + "default": 2.778, + "minimum": 0.0, + "description": "The maximum of heading velocity (m/s)." + }, + "heading_velocity_scale_factor_tolerance": { + "type": "number", + "default": 3.0, + "minimum": 0.0, + "description": "The tolerance of heading velocity scale factor (%)." + }, + "angular_velocity_maximum": { + "type": "number", + "default": 0.523, + "minimum": 0.0, + "description": "The maximum of angular velocity (rad/s)." + }, + "angular_velocity_scale_factor_tolerance": { + "type": "number", + "default": 0.2, + "minimum": 0.0, + "description": "The tolerance of angular velocity scale factor (%)." + }, + "angular_velocity_bias_tolerance": { + "type": "number", + "default": 0.00698, + "minimum": 0.0, + "description": "The tolerance of angular velocity bias (rad/s)." + }, + "pose_estimator_longitudinal_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of longitudinal position of pose estimator (m)." + }, + "pose_estimator_lateral_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of lateral position of pose estimator (m)." + }, + "pose_estimator_vertical_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of vertical position of pose estimator (m)." + }, + "pose_estimator_angular_tolerance": { + "type": "number", + "default": 0.0175, + "minimum": 0.0, + "description": "The tolerance of roll angle of pose estimator (rad)." + } + }, + "required": [ + "window_length", + "output_x_y_yaw_only", + "heading_velocity_maximum", + "heading_velocity_scale_factor_tolerance", + "angular_velocity_maximum", + "angular_velocity_scale_factor_tolerance", + "angular_velocity_bias_tolerance", + "pose_estimator_longitudinal_tolerance", + "pose_estimator_lateral_tolerance", + "pose_estimator_vertical_tolerance", + "pose_estimator_angular_tolerance" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pose_instability_detector_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp new file mode 100644 index 0000000000000..02d58e7621f3c --- /dev/null +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -0,0 +1,478 @@ +// 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. + +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" + +#include + +#include + +#include +#include +#include +#include + +PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) +: rclcpp::Node("pose_instability_detector", options), + window_length_(this->declare_parameter("window_length")), + output_x_y_yaw_only_(this->declare_parameter("output_x_y_yaw_only")), + heading_velocity_maximum_(this->declare_parameter("heading_velocity_maximum")), + heading_velocity_scale_factor_tolerance_( + this->declare_parameter("heading_velocity_scale_factor_tolerance")), + angular_velocity_maximum_(this->declare_parameter("angular_velocity_maximum")), + angular_velocity_scale_factor_tolerance_( + this->declare_parameter("angular_velocity_scale_factor_tolerance")), + angular_velocity_bias_tolerance_( + this->declare_parameter("angular_velocity_bias_tolerance")), + pose_estimator_longitudinal_tolerance_( + this->declare_parameter("pose_estimator_longitudinal_tolerance")), + pose_estimator_lateral_tolerance_( + this->declare_parameter("pose_estimator_lateral_tolerance")), + pose_estimator_vertical_tolerance_( + this->declare_parameter("pose_estimator_vertical_tolerance")), + pose_estimator_angular_tolerance_( + this->declare_parameter("pose_estimator_angular_tolerance")) +{ + // Define subscribers and publishers + + odometry_sub_ = this->create_subscription( + "~/input/odometry", 10, + std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); + + twist_sub_ = this->create_subscription( + "~/input/twist", 10, + std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); + + diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); + diagnostics_pub_ = this->create_publisher("/diagnostics", 10); +} + +void PoseInstabilityDetector::callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr) +{ + latest_odometry_ = *odometry_msg_ptr; + + PoseStamped current_pose = PoseStamped(); + current_pose.header = latest_odometry_->header; + current_pose.pose = latest_odometry_->pose.pose; + rclcpp::Time current_pose_time = rclcpp::Time(current_pose.header.stamp); + updatePoseBuffer(current_pose); + + rclcpp::Time one_step_back_time = + rclcpp::Time(current_pose.header.stamp) - rclcpp::Duration::from_seconds(window_length_); + std::optional one_step_back_pose = getPoseAt(one_step_back_time); + + if (one_step_back_pose == std::nullopt) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "pose was nullopt"); + return; + } + + // delete twist data older than one_step_back_pose (but preserve the one right before + // one_step_back_pose) + while (twist_buffer_.size() > 1) { + if (rclcpp::Time(twist_buffer_[1].header.stamp) < one_step_back_time) { + twist_buffer_.pop_front(); + } else { + break; + } + } + + // If twist_buffer_ is empty OR the latest twist is too old, skip the following. + double latest_twist_age = + (current_pose_time - rclcpp::Time(twist_buffer_.back().header.stamp)).seconds(); + if (twist_buffer_.empty() || latest_twist_age > window_length_ * 1.5) { + return; + } + + PoseStamped::SharedPtr one_step_back_pose_ptr = + std::make_shared(one_step_back_pose.value()); + Pose::SharedPtr dr_pose_ptr = std::make_shared(); + dead_reckon(one_step_back_pose_ptr, current_pose_time, twist_buffer_, dr_pose_ptr); + + const Pose comparison_target_to_dr = inverseTransformPose(*dr_pose_ptr, current_pose.pose); + const geometry_msgs::msg::Point pos = comparison_target_to_dr.position; + const auto [ang_x, ang_y, ang_z] = quatToRPY(comparison_target_to_dr.orientation); + std::vector values; + std::vector thresholds; + std::vector labels; + + // publish diff_pose for debug + PoseStamped diff_pose; + diff_pose.header.stamp = current_pose_time; + diff_pose.header.frame_id = "base_link"; + diff_pose.pose = comparison_target_to_dr; + diff_pose_pub_->publish(diff_pose); + + // publish diagnostics + ThresholdValues threshold_values = + calculate_threshold((current_pose_time - one_step_back_time).seconds()); + + if (output_x_y_yaw_only_) { + values = {pos.x, pos.y, ang_z}; + thresholds = { + threshold_values.position_x, threshold_values.position_y, threshold_values.angle_z}; + labels = {"diff_position_x", "diff_position_y", "diff_angle_z"}; + } else { + values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; + thresholds = {threshold_values.position_x, threshold_values.position_y, + threshold_values.position_z, threshold_values.angle_x, + threshold_values.angle_y, threshold_values.angle_z}; + labels = {"diff_position_x", "diff_position_y", "diff_position_z", + "diff_angle_x", "diff_angle_y", "diff_angle_z"}; + } + + DiagnosticStatus status; + status.name = "localization: pose_instability_detector"; + status.hardware_id = this->get_name(); + bool all_ok = true; + + for (size_t i = 0; i < values.size(); ++i) { + const bool ok = (std::abs(values[i]) < thresholds[i]); + all_ok &= ok; + diagnostic_msgs::msg::KeyValue kv; + kv.key = labels[i] + ":threshold"; + kv.value = std::to_string(thresholds[i]); + status.values.push_back(kv); + kv.key = labels[i] + ":value"; + kv.value = std::to_string(values[i]); + status.values.push_back(kv); + kv.key = labels[i] + ":status"; + kv.value = (ok ? "OK" : "WARN"); + status.values.push_back(kv); + } + status.level = (all_ok ? DiagnosticStatus::OK : DiagnosticStatus::WARN); + status.message = (all_ok ? "OK" : "WARN"); + + DiagnosticArray diagnostics; + diagnostics.header.stamp = current_pose_time; + diagnostics.status.emplace_back(status); + diagnostics_pub_->publish(diagnostics); +} + +void PoseInstabilityDetector::callback_twist( + TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + twist_buffer_.push_back(*twist_msg_ptr); +} + +PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( + double interval_sec) const +{ + // Calculate maximum longitudinal difference + const double longitudinal_difference = + heading_velocity_maximum_ * heading_velocity_scale_factor_tolerance_ * 0.01 * interval_sec; + + // Calculate maximum lateral and vertical difference + double lateral_difference = 0.0; + + const std::vector heading_velocity_signs = {1.0, -1.0, -1.0, 1.0}; + const std::vector angular_velocity_signs = {1.0, 1.0, -1.0, -1.0}; + + const double nominal_variation_x = heading_velocity_maximum_ / angular_velocity_maximum_ * + sin(angular_velocity_maximum_ * interval_sec); + const double nominal_variation_y = heading_velocity_maximum_ / angular_velocity_maximum_ * + (1 - cos(angular_velocity_maximum_ * interval_sec)); + + for (int i = 0; i < 4; i++) { + const double edge_heading_velocity = + heading_velocity_maximum_ * + (1 + heading_velocity_signs[i] * heading_velocity_scale_factor_tolerance_ * 0.01); + const double edge_angular_velocity = + angular_velocity_maximum_ * + (1 + angular_velocity_signs[i] * angular_velocity_scale_factor_tolerance_ * 0.01) + + angular_velocity_signs[i] * angular_velocity_bias_tolerance_; + + const double edge_variation_x = + edge_heading_velocity / edge_angular_velocity * sin(edge_angular_velocity * interval_sec); + const double edge_variation_y = edge_heading_velocity / edge_angular_velocity * + (1 - cos(edge_angular_velocity * interval_sec)); + + const double diff_variation_x = edge_variation_x - nominal_variation_x; + const double diff_variation_y = edge_variation_y - nominal_variation_y; + + const double lateral_difference_candidate = abs( + diff_variation_x * sin(angular_velocity_maximum_ * interval_sec) - + diff_variation_y * cos(angular_velocity_maximum_ * interval_sec)); + lateral_difference = std::max(lateral_difference, lateral_difference_candidate); + } + + const double vertical_difference = lateral_difference; + + // Calculate maximum angular difference + const double roll_difference = + (angular_velocity_maximum_ * angular_velocity_scale_factor_tolerance_ * 0.01 + + angular_velocity_bias_tolerance_) * + interval_sec; + const double pitch_difference = roll_difference; + const double yaw_difference = roll_difference; + + // Set thresholds + ThresholdValues result_values{}; + result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_; + result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_; + result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_; + result_values.angle_x = roll_difference + pose_estimator_angular_tolerance_; + result_values.angle_y = pitch_difference + pose_estimator_angular_tolerance_; + result_values.angle_z = yaw_difference + pose_estimator_angular_tolerance_; + + return result_values; +} + +void PoseInstabilityDetector::dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose) +{ + // get start time + rclcpp::Time start_time = rclcpp::Time(initial_pose->header.stamp); + + // initialize estimated_pose + estimated_pose->position = initial_pose->pose.position; + estimated_pose->orientation = initial_pose->pose.orientation; + + // cut out necessary twist data + std::deque sliced_twist_deque = + clip_out_necessary_twist(twist_deque, start_time, end_time); + + // dead reckoning + rclcpp::Time prev_odometry_time = rclcpp::Time(sliced_twist_deque.front().header.stamp); + tf2::Quaternion prev_orientation; + tf2::fromMsg(estimated_pose->orientation, prev_orientation); + + for (size_t i = 1; i < sliced_twist_deque.size(); ++i) { + const rclcpp::Time curr_time = rclcpp::Time(sliced_twist_deque[i].header.stamp); + const double time_diff_sec = (curr_time - prev_odometry_time).seconds(); + + const Twist twist = sliced_twist_deque[i - 1].twist.twist; + + // variation of orientation (rpy update) + tf2::Quaternion delta_orientation; + tf2::Vector3 rotation_axis(twist.angular.x, twist.angular.y, twist.angular.z); + if (rotation_axis.length() > 0.0) { + delta_orientation.setRotation( + rotation_axis.normalized(), rotation_axis.length() * time_diff_sec); + } else { + delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); + } + + tf2::Quaternion curr_orientation; + curr_orientation = prev_orientation * delta_orientation; + curr_orientation.normalize(); + + // average quaternion of two frames + tf2::Quaternion average_quat = prev_orientation.slerp(curr_orientation, 0.5); + + // Convert twist to world frame (take average of two frames) + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(average_quat, linear_velocity); + + // xyz update + estimated_pose->position.x += linear_velocity.x() * time_diff_sec; + estimated_pose->position.y += linear_velocity.y() * time_diff_sec; + estimated_pose->position.z += linear_velocity.z() * time_diff_sec; + + // update previous variables + prev_odometry_time = curr_time; + prev_orientation = curr_orientation; + } + estimated_pose->orientation.x = prev_orientation.x(); + estimated_pose->orientation.y = prev_orientation.y(); + estimated_pose->orientation.z = prev_orientation.z(); + estimated_pose->orientation.w = prev_orientation.w(); +} + +std::deque +PoseInstabilityDetector::clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time) +{ + // If there is only one element in the twist_buffer, return a deque that has the same twist + // from the start till the end + if (twist_buffer.size() == 1) { + TwistWithCovarianceStamped twist = twist_buffer.front(); + std::deque simple_twist_deque; + + twist.header.stamp = start_time; + simple_twist_deque.push_back(twist); + + twist.header.stamp = end_time; + simple_twist_deque.push_back(twist); + + return simple_twist_deque; + } + + // get iterator to the element that is right before start_time (if it does not exist, start_it = + // twist_buffer.begin()) + auto start_it = twist_buffer.begin(); + for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) { + if (rclcpp::Time(it->header.stamp) > start_time) { + break; + } + start_it = it; + } + + // get iterator to the element that is right after end_time (if it does not exist, end_it = + // twist_buffer.end()) + auto end_it = twist_buffer.end(); + end_it--; + for (auto it = end_it; it != twist_buffer.begin(); --it) { + if (rclcpp::Time(it->header.stamp) < end_time) { + break; + } + end_it = it; + } + + // Create result deque + std::deque result_deque(start_it, end_it); + + // If the first element is later than start_time, add the first element to the front of the + // result_deque + if (rclcpp::Time(result_deque.front().header.stamp) > start_time) { + TwistWithCovarianceStamped start_twist = *start_it; + start_twist.header.stamp = start_time; + result_deque.push_front(start_twist); + } else { + if (result_deque.size() < 2) { + return result_deque; + } + // If the first element is earlier than start_time, interpolate the first element + rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp); + double ratio = (start_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[0].twist.twist; + Twist twist1 = result_deque[1].twist.twist; + result_deque[0].twist.twist.linear.x = twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[0].twist.twist.linear.y = twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[0].twist.twist.linear.z = twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[0].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[0].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[0].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[0].header.stamp = start_time; + } + + // If the last element is earlier than end_time, add the last element to the back of the + // result_deque + if (rclcpp::Time(result_deque.back().header.stamp) < end_time) { + TwistWithCovarianceStamped end_twist = *end_it; + end_twist.header.stamp = end_time; + result_deque.push_back(end_twist); + } else { + if (result_deque.size() < 2) { + return result_deque; + } + // If the last element is later than end_time, interpolate the last element + rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp); + double ratio = (end_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[result_deque.size() - 2].twist.twist; + Twist twist1 = result_deque[result_deque.size() - 1].twist.twist; + result_deque[result_deque.size() - 1].twist.twist.linear.x = + twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.y = + twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.z = + twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[result_deque.size() - 1].header.stamp = end_time; + } + return result_deque; +} + +PoseInstabilityDetector::Pose PoseInstabilityDetector::inverseTransformPose( + const Pose & pose, const Pose & transform_pose) +{ + tf2::Transform transform; + tf2::convert(transform_pose, transform); + + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.transform = tf2::toMsg(transform.inverse()); + + Pose result_pose; + tf2::doTransform(pose, result_pose, transform_stamped); + + return result_pose; +} + +void PoseInstabilityDetector::updatePoseBuffer(const PoseStamped & pose) +{ + // Add new pose + pose_buffer_.push_back(pose); + + // Discard old poses + const rclcpp::Time latest_time = rclcpp::Time(pose.header.stamp); + while (!pose_buffer_.empty()) { + rclcpp::Time pose_time = pose_buffer_.front().header.stamp; + double age = (latest_time - pose_time).seconds(); + + if (age <= window_length_ * 1.5) { + break; + } + + pose_buffer_.pop_front(); + } +} + +std::optional PoseInstabilityDetector::getPoseAt( + const rclcpp::Time & target_time) +{ + if (pose_buffer_.empty()) { + RCLCPP_INFO(this->get_logger(), "pose_buffer_ doesn't have poses!"); + return std::nullopt; + } + + if ( + target_time < rclcpp::Time(pose_buffer_.front().header.stamp) || + rclcpp::Time(pose_buffer_.back().header.stamp) < target_time) { + RCLCPP_INFO(this->get_logger(), "target_time must be within the pose_buffer_!"); + return std::nullopt; + } + + std::optional closest_pose = std::nullopt; + for (size_t i = 1; i < pose_buffer_.size(); i++) { + if (target_time < rclcpp::Time(pose_buffer_[i].header.stamp)) { + if ( + (rclcpp::Time(pose_buffer_[i].header.stamp) - target_time).seconds() < + (target_time - rclcpp::Time(pose_buffer_[i - 1].header.stamp)).seconds()) { + closest_pose = pose_buffer_[i]; + } else { + closest_pose = pose_buffer_[i - 1]; + } + break; + } + } + + return closest_pose; +} + +std::tuple PoseInstabilityDetector::quatToRPY(const Quaternion & quat) +{ + tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); + tf2::Matrix3x3 mat(tf2_quat); + double roll{}; + double pitch{}; + double yaw{}; + mat.getRPY(roll, pitch, yaw); + return std::make_tuple(roll, pitch, yaw); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp new file mode 100644 index 0000000000000..a0e0decb57fc0 --- /dev/null +++ b/localization/pose_instability_detector/test/test.cpp @@ -0,0 +1,231 @@ +// 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. + +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" +#include "test_message_helper_node.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +class TestPoseInstabilityDetector : public ::testing::Test +{ + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Odometry = nav_msgs::msg::Odometry; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +protected: + void SetUp() override + { + const std::string yaml_path = + ament_index_cpp::get_package_share_directory("pose_instability_detector") + + "/config/pose_instability_detector.param.yaml"; + + rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); + if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { + std::cerr << "Failed to parse yaml file : " << yaml_path << std::endl; + std::exit(1); + } + + const rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(params_st, ""); + rclcpp::NodeOptions node_options; + for (const auto & param_pair : param_map) { + for (const auto & param : param_pair.second) { + node_options.parameter_overrides().push_back(param); + } + } + + subject_ = std::make_shared(node_options); + executor_.add_node(subject_); + + helper_ = std::make_shared(); + executor_.add_node(helper_); + + rcl_yaml_node_struct_fini(params_st); + } + + void TearDown() override + { + executor_.remove_node(subject_); + executor_.remove_node(helper_); + } + + rclcpp::executors::SingleThreadedExecutor executor_; + std::shared_ptr subject_; + std::shared_ptr helper_; +}; + +TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // NOLINT +{ + // send the first odometry message (start x = 10) + builtin_interfaces::msg::Time timestamp{}; + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the twist message2 (move 1m in x direction) + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + + // send the second odometry message (finish x = 12) + timestamp.sec = 2; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // check result + const diagnostic_msgs::msg::DiagnosticStatus & diagnostic_status = + helper_->received_diagnostic_array.status[0]; + EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOLINT +{ + // send the first odometry message (start x = 10) + builtin_interfaces::msg::Time timestamp{}; + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the twist message2 (move 0.1m in x direction) + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the second odometry message (finish x = 12) + timestamp.sec = 2; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // check result + const diagnostic_msgs::msg::DiagnosticStatus & diagnostic_status = + helper_->received_diagnostic_array.status[0]; + EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_data_comes) // NOLINT +{ + // [Condition] There is no twist_msg between the two target odometry_msgs. + // Normally this doesn't seem to happen. + // As far as I can think, this happens when the odometry msg stops (so the next timer callback + // will refer to the same odometry msg, and the timestamp difference will be calculated as 0) + // This test case shows that an error occurs when two odometry msgs come in close succession and + // there is no other odometry msg. + // Referring again, this doesn't normally seem to happen in usual operation. + + builtin_interfaces::msg::Time timestamp{}; + + // send the twist message1 + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the first odometry message after the first twist message + timestamp.sec = 0; + timestamp.nanosec = 5e8 + 1; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the second odometry message before the second twist message + timestamp.sec = 0; + timestamp.nanosec = 5e8 + 1e7; + helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + + // send the twist message2 + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // provoke timer callback again + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // This test is OK if pose_instability_detector does not crash. The diagnostics status is not + // checked. + SUCCEED(); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/pose_instability_detector/test/test_message_helper_node.hpp b/localization/pose_instability_detector/test/test_message_helper_node.hpp new file mode 100644 index 0000000000000..2bbce1e91a27b --- /dev/null +++ b/localization/pose_instability_detector/test/test_message_helper_node.hpp @@ -0,0 +1,80 @@ +// 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 TEST_MESSAGE_HELPER_NODE_HPP_ +#define TEST_MESSAGE_HELPER_NODE_HPP_ + +#include + +#include +#include +#include + +class TestMessageHelperNode : public rclcpp::Node +{ + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Odometry = nav_msgs::msg::Odometry; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + TestMessageHelperNode() : Node("test_message_helper_node") + { + odometry_publisher_ = + this->create_publisher("/pose_instability_detector/input/odometry", 10); + twist_publisher_ = this->create_publisher( + "/pose_instability_detector/input/twist", 10); + diagnostic_subscriber_ = this->create_subscription( + "/diagnostics", 10, + std::bind(&TestMessageHelperNode::callback_diagnostics, this, std::placeholders::_1)); + } + + void send_odometry_message( + const builtin_interfaces::msg::Time timestamp, const double x, const double y, const double z) + { + Odometry message{}; + message.header.stamp = timestamp; + message.pose.pose.position.x = x; + message.pose.pose.position.y = y; + message.pose.pose.position.z = z; + odometry_publisher_->publish(message); + } + + void send_twist_message( + const builtin_interfaces::msg::Time timestamp, const double x, const double y, const double z) + { + TwistWithCovarianceStamped message{}; + message.header.stamp = timestamp; + message.twist.twist.linear.x = x; + message.twist.twist.linear.y = y; + message.twist.twist.linear.z = z; + twist_publisher_->publish(message); + } + + void callback_diagnostics(const DiagnosticArray::ConstSharedPtr msg) + { + received_diagnostic_array = *msg; + received_diagnostic_array_flag = true; + } + + DiagnosticArray received_diagnostic_array; + bool received_diagnostic_array_flag = false; + +private: + rclcpp::Publisher::SharedPtr odometry_publisher_; + rclcpp::Publisher::SharedPtr twist_publisher_; + rclcpp::Subscription::SharedPtr diagnostic_subscriber_; +}; + +#endif // TEST_MESSAGE_HELPER_NODE_HPP_