diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 59fd5174fa38c..23a1201a84958 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -33,4 +33,12 @@ + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 1126914d58c5a..c4de9c04dcaf2 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -25,6 +25,7 @@ ndt_scan_matcher pointcloud_preprocessor pose_initializer + pose_instability_detector topic_tools yabloc_common yabloc_image_processing diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt new file mode 100644 index 0000000000000..5270df636d791 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..89cf6ca3be684 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..29a25849d6b1c --- /dev/null +++ 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 new file mode 100644 index 0000000000000..4a390ee2854d7 --- /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/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png new file mode 100644 index 0000000000000..b3ad402e48ba7 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..9f38cf6e00262 --- /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 + Apache License 2.0 + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + geometry_msgs + nav_msgs + rclcpp + tf2 + tf2_geometry_msgs + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + 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..b45b43102444d --- /dev/null +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -0,0 +1,75 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pose Instability Detector Nodes", + "type": "object", + "definitions": { + "pose_instability_detector_node": { + "type": "object", + "properties": { + "interval_sec": { + "type": "number", + "default": 1.0, + "exclusiveMinimum": 0, + "description": "The interval of timer_callback in seconds." + }, + "threshold_diff_position_x": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position x (m)." + }, + "threshold_diff_position_y": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position y (m)." + }, + "threshold_diff_position_z": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position z (m)." + }, + "threshold_diff_angle_x": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle x (rad)." + }, + "threshold_diff_angle_y": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle y (rad)." + }, + "threshold_diff_angle_z": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle z (rad)." + } + }, + "required": [ + "interval_sec", + "threshold_diff_position_x", + "threshold_diff_position_y", + "threshold_diff_position_z", + "threshold_diff_angle_x", + "threshold_diff_angle_y", + "threshold_diff_angle_z" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pose_instability_detector_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp new file mode 100644 index 0000000000000..34e679e86730f --- /dev/null +++ b/localization/pose_instability_detector/src/main.cpp @@ -0,0 +1,26 @@ +// Copyright 2023- 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 "pose_instability_detector.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto pose_instability_detector = std::make_shared(); + rclcpp::spin(pose_instability_detector); + rclcpp::shutdown(); + return 0; +} 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..afb7d6e007db2 --- /dev/null +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -0,0 +1,176 @@ +// Copyright 2023- 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 "pose_instability_detector.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +#include + +PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) +: Node("pose_instability_detector", options), + threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), + threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), + threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), + threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), + threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), + threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) +{ + 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)); + + const double interval_sec = this->declare_parameter("interval_sec"); + timer_ = rclcpp::create_timer( + this, this->get_clock(), std::chrono::duration(interval_sec), + std::bind(&PoseInstabilityDetector::callback_timer, this)); + + 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; +} + +void PoseInstabilityDetector::callback_twist( + TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + twist_buffer_.push_back(*twist_msg_ptr); +} + +void PoseInstabilityDetector::callback_timer() +{ + if (latest_odometry_ == std::nullopt) { + return; + } + if (prev_odometry_ == std::nullopt) { + prev_odometry_ = latest_odometry_; + return; + } + + auto quat_to_rpy = [](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); + }; + + Pose pose = prev_odometry_->pose.pose; + rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); + for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { + const Twist twist = twist_with_cov.twist.twist; + + const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); + if (curr_time > latest_odometry_->header.stamp) { + break; + } + + const rclcpp::Duration time_diff = curr_time - prev_time; + const double time_diff_sec = time_diff.seconds(); + if (time_diff_sec < 0.0) { + continue; + } + + // quat to rpy + auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); + + // rpy update + ang_x += twist.angular.x * time_diff_sec; + ang_y += twist.angular.y * time_diff_sec; + ang_z += twist.angular.z * time_diff_sec; + tf2::Quaternion quat; + quat.setRPY(ang_x, ang_y, ang_z); + + // Convert twist to world frame + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(quat, linear_velocity); + + // update + pose.position.x += linear_velocity.x() * time_diff_sec; + pose.position.y += linear_velocity.y() * time_diff_sec; + pose.position.z += linear_velocity.z() * time_diff_sec; + pose.orientation.x = quat.x(); + pose.orientation.y = quat.y(); + pose.orientation.z = quat.z(); + pose.orientation.w = quat.w(); + prev_time = curr_time; + } + + // compare pose and latest_odometry_ + const Pose latest_ekf_pose = latest_odometry_->pose.pose; + const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_odom.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation); + const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; + + const rclcpp::Time stamp = latest_odometry_->header.stamp; + + // publish diff_pose for debug + PoseStamped diff_pose; + diff_pose.header = latest_odometry_->header; + diff_pose.pose = ekf_to_odom; + diff_pose_pub_->publish(diff_pose); + + const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, + threshold_diff_position_z_, threshold_diff_angle_x_, + threshold_diff_angle_y_, threshold_diff_angle_z_}; + + const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", + "diff_angle_x", "diff_angle_y", "diff_angle_z"}; + + // publish diagnostics + 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 = stamp; + diagnostics.status.emplace_back(status); + diagnostics_pub_->publish(diagnostics); + + // prepare for next loop + prev_odometry_ = latest_odometry_; + twist_buffer_.clear(); +} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/src/pose_instability_detector.hpp new file mode 100644 index 0000000000000..761a10b7a6bf7 --- /dev/null +++ b/localization/pose_instability_detector/src/pose_instability_detector.hpp @@ -0,0 +1,70 @@ +// Copyright 2023- 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 POSE_INSTABILITY_DETECTOR_HPP_ +#define POSE_INSTABILITY_DETECTOR_HPP_ + +#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 Odometry = nav_msgs::msg::Odometry; + using KeyValue = diagnostic_msgs::msg::KeyValue; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); + void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + void callback_timer(); + + // subscribers and timer + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + // publisher + rclcpp::Publisher::SharedPtr diff_pose_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + // parameters + const double threshold_diff_position_x_; + const double threshold_diff_position_y_; + const double threshold_diff_position_z_; + const double threshold_diff_angle_x_; + const double threshold_diff_angle_y_; + const double threshold_diff_angle_z_; + + // variables + std::optional latest_odometry_ = std::nullopt; + std::optional prev_odometry_ = std::nullopt; + std::vector twist_buffer_; +}; + +#endif // POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp new file mode 100644 index 0000000000000..4747582cdd163 --- /dev/null +++ b/localization/pose_instability_detector/test/test.cpp @@ -0,0 +1,168 @@ +// Copyright 2023- 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 "../src/pose_instability_detector.hpp" +#include "test_message_helper_node.hpp" + +#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 = + "../../install/pose_instability_detector/share/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); + + // 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 message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + + // 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, 12.0, 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)); + } + + // 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); + + // 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 message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // 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, 12.0, 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)); + } + + // 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); +} + +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..51444f9736b02 --- /dev/null +++ b/localization/pose_instability_detector/test/test_message_helper_node.hpp @@ -0,0 +1,80 @@ +// Copyright 2023- 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_