From 255f5c8e829978b0ada1585b02abd9934fc97bc0 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 25 Jun 2024 15:02:25 +0900 Subject: [PATCH 1/4] Added an error test case Signed-off-by: Shintaro Sakoda --- .../pose_instability_detector/test/test.cpp | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index 9300984967d4b..c58ba9d7532e0 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/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 + // (case A) the twist msg stops + // (case B) 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) + + builtin_interfaces::msg::Time timestamp{}; + + // send the twist message1 + timestamp.sec = 0; + timestamp.nanosec = 5e8; // timestamp is the same as the first odometry message + 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 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 7e8 + 5e7; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the twist message3 (move 0.1m in x direction) + 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)); + } + + // 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); From 46c3bfc8595dbdd29bc7fcc55d324890cce96e35 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 25 Jun 2024 15:31:35 +0900 Subject: [PATCH 2/4] Fixed to avoid out-of-size access Signed-off-by: Shintaro Sakoda --- .../src/pose_instability_detector.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index 28398588809eb..23362dd13c6bc 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/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); From d3d011c116483f00898b8a86466dc2c95ebf9dcd Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Jun 2024 16:30:36 +0900 Subject: [PATCH 3/4] Fixed the test case Signed-off-by: Shintaro Sakoda --- .../pose_instability_detector/test/test.cpp | 25 ++++++++----------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index c58ba9d7532e0..e659cac680ff8 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -167,16 +167,17 @@ TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_dat { // [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 - // (case A) the twist msg stops - // (case B) 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) + // 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 = 5e8; // timestamp is the same as the first odometry message + timestamp.nanosec = 0; helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); // send the first odometry message after the first twist message @@ -196,12 +197,7 @@ TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_dat timestamp.nanosec = 5e8 + 1e7; helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); - // send the twist message2 (move 0.1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 7e8 + 5e7; - helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); - - // send the twist message3 (move 0.1m in x direction) + // send the twist message2 timestamp.sec = 1; timestamp.nanosec = 0; helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); @@ -215,10 +211,9 @@ TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_dat 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); + // This test is OK if pose_instability_detector does not crash. The diagnostics status is not + // checked. + SUCCEED(); } int main(int argc, char ** argv) From d8e27deff6b254365c7d3e92284bef67707e0911 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Jun 2024 18:16:28 +0900 Subject: [PATCH 4/4] Fixed test Signed-off-by: Shintaro Sakoda --- localization/pose_instability_detector/test/test.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index e659cac680ff8..482e659e7a13c 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -202,8 +202,6 @@ TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_dat 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) { @@ -211,6 +209,13 @@ TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_dat 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();