fix(pose_instability_detector): fix a rare error (autowarefoundation#…

…7681)

* Added an error test case

Signed-off-by: Shintaro Sakoda <[email protected]>

* Fixed to avoid out-of-size access

Signed-off-by: Shintaro Sakoda <[email protected]>

* Fixed the test case

Signed-off-by: Shintaro Sakoda <[email protected]>

* Fixed test

Signed-off-by: Shintaro Sakoda <[email protected]>

---------

Signed-off-by: Shintaro Sakoda <[email protected]>

localization/pose_instability_detector/src/pose_instability_detector.cpp

@@ -354,6 +354,9 @@ PoseInstabilityDetector::clip_out_necessary_twist(
     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);
@@ -380,6 +383,9 @@ PoseInstabilityDetector::clip_out_necessary_twist(
     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,64 @@ 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);
+
+  // 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);