Apply review suggestions

Auterion · Dec 15, 2023 · cbeab2c · cbeab2c
1 parent e2db92a
commit cbeab2c
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 25 deletions.
diff --git a/px4_ros2_cpp/test/integration/global_navigation.cpp b/px4_ros2_cpp/test/integration/global_navigation.cpp
@@ -41,12 +41,21 @@ class GlobalPositionInterfaceTest : public BaseTest
       });
   }
 
-  void resetFlags()
+  void waitUntilFlagsReset()
   {
+    auto start_time = _node->now();
+
     // Wait for PX4 to stop fusing external measurements
     while (_estimator_status_flags->cs_aux_gpos) {
       rclcpp::spin_some(_node);
-      std::this_thread::sleep_for(_k_sleep_interval);
+      rclcpp::sleep_for(kSleepInterval);
+
+      const auto elapsed_time = _node->now() - start_time;
+
+      if (elapsed_time >= kTimeoutDuration) {
+        FAIL() << "Timeout while waiting for PX4 estimator flags to reset.";
+        break;
+      }
     }
 
     _estimator_status_flags.reset();
@@ -57,7 +66,7 @@ class GlobalPositionInterfaceTest : public BaseTest
     const std::function<bool(const EstimatorStatusFlags::UniquePtr &)> & is_fused_getter
   )
   {
-    auto start_time = std::chrono::steady_clock::now();
+    auto start_time = _node->now();
 
     // Wait for PX4 estimator flags to confirm proper measurement fusion
     while (!_estimator_status_flags || !is_fused_getter(_estimator_status_flags)) {
@@ -70,33 +79,30 @@ class GlobalPositionInterfaceTest : public BaseTest
 
       rclcpp::spin_some(_node);
 
-      const auto current_time = std::chrono::steady_clock::now();
-      const auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(
-        current_time - start_time
-      );
-
       // Check timeout
-      if (elapsed_time >= _k_timeout_duration) {
+      const auto elapsed_time = _node->now() - start_time;
+
+      if (elapsed_time >= kTimeoutDuration) {
         ASSERT_NE(_estimator_status_flags, nullptr) <<
           "Missing feedback from PX4: no estimator status flags published over /fmu/out/estimator_status_flags.";
         EXPECT_TRUE(is_fused_getter(_estimator_status_flags)) <<
           "Position measurement update was not fused into the PX4 EKF.";
         break;
       }
 
-      std::this_thread::sleep_for(_k_sleep_interval);
+      rclcpp::sleep_for(kSleepInterval);
     }
 
-    resetFlags();
+    waitUntilFlagsReset();
   }
 
   std::shared_ptr<rclcpp::Node> _node;
   std::shared_ptr<GlobalPositionMeasurementInterface> _global_navigation_interface;
   rclcpp::Subscription<EstimatorStatusFlags>::SharedPtr _subscriber;
   EstimatorStatusFlags::UniquePtr _estimator_status_flags;
 
-  const std::chrono::seconds _k_timeout_duration = 60s;
-  const std::chrono::milliseconds _k_sleep_interval = 50ms;
+  static constexpr std::chrono::seconds kTimeoutDuration = 60s;
+  static constexpr std::chrono::milliseconds kSleepInterval = 50ms;
 };
 
 TEST_F(GlobalPositionInterfaceTest, fuseAll) {

diff --git a/px4_ros2_cpp/test/integration/local_navigation.cpp b/px4_ros2_cpp/test/integration/local_navigation.cpp
@@ -46,14 +46,23 @@ class LocalPositionInterfaceTest : public BaseTest
       });
   }
 
-  void resetFlags()
+  void waitUntilFlagsReset()
   {
+    auto start_time = _node->now();
+
     // Wait for PX4 to stop fusing external measurements
     while (_estimator_status_flags->cs_ev_pos || _estimator_status_flags->cs_ev_hgt ||
       _estimator_status_flags->cs_ev_vel || _estimator_status_flags->cs_ev_yaw)
     {
       rclcpp::spin_some(_node);
-      std::this_thread::sleep_for(_k_sleep_interval);
+      rclcpp::sleep_for(kSleepInterval);
+
+      const auto elapsed_time = _node->now() - start_time;
+
+      if (elapsed_time >= kTimeoutDuration) {
+        FAIL() << "Timeout while waiting for PX4 estimator flags to reset.";
+        break;
+      }
     }
 
     _estimator_status_flags.reset();
@@ -64,7 +73,7 @@ class LocalPositionInterfaceTest : public BaseTest
     const std::function<bool(const EstimatorStatusFlags::UniquePtr &)> & is_fused_getter
   )
   {
-    auto start_time = std::chrono::steady_clock::now();
+    auto start_time = _node->now();
 
     // Wait for PX4 estimator flags to confirm proper measurement fusion
     while (!_estimator_status_flags || !is_fused_getter(_estimator_status_flags)) {
@@ -78,32 +87,29 @@ class LocalPositionInterfaceTest : public BaseTest
       rclcpp::spin_some(_node);
 
       // Check timeout
-      const auto current_time = std::chrono::steady_clock::now();
-      const auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(
-        current_time - start_time
-      );
+      const auto elapsed_time = _node->now() - start_time;
 
-      if (elapsed_time >= _k_timeout_duration) {
+      if (elapsed_time >= kTimeoutDuration) {
         ASSERT_NE(_estimator_status_flags, nullptr) <<
           "Missing feedback from PX4: no estimator status flags published over /fmu/out/estimator_status_flags.";
         EXPECT_TRUE(is_fused_getter(_estimator_status_flags)) <<
           "Position measurement update was not fused into the PX4 EKF.";
         break;
       }
 
-      std::this_thread::sleep_for(_k_sleep_interval);
+      rclcpp::sleep_for(kSleepInterval);
     }
 
-    resetFlags();
+    waitUntilFlagsReset();
   }
 
   std::shared_ptr<rclcpp::Node> _node;
   std::shared_ptr<LocalPositionMeasurementInterface> _local_navigation_interface;
   rclcpp::Subscription<EstimatorStatusFlags>::SharedPtr _subscriber;
   EstimatorStatusFlags::UniquePtr _estimator_status_flags;
 
-  const std::chrono::seconds _k_timeout_duration = 60s;
-  const std::chrono::milliseconds _k_sleep_interval = 50ms;
+  static constexpr std::chrono::seconds kTimeoutDuration = 60s;
+  static constexpr std::chrono::milliseconds kSleepInterval = 50ms;
 };
 
 TEST_F(LocalPositionInterfaceTest, fuseEvPos) {