fix(pose_instability_detector): fix a rare error

localization/pose_instability_detector/src/pose_instability_detector.cpp

@@ -1,4 +1,4 @@
 // 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.
@@ -354,32 +354,38 @@
     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);

localization/pose_instability_detector/test/test.cpp

@@ -163,6 +163,59 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small)  // NOL
   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);
+
+  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));
+  }
+
+  // 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);