fix: add WaitForSync call

bdaiinstitute · Jan 3, 2025 · 9e792ea · 9e792ea
1 parent bf06fdc
commit 9e792ea
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/spot_driver/src/api/default_time_sync_api.cpp b/spot_driver/src/api/default_time_sync_api.cpp
@@ -2,6 +2,7 @@
 
 #include <spot_driver/api/default_time_sync_api.hpp>
 #include <tl_expected/expected.hpp>
+#include <chrono>
 
 namespace spot_ros2 {
 
@@ -12,6 +13,9 @@ tl::expected<google::protobuf::Duration, std::string> DefaultTimeSyncApi::getClo
   if (!time_sync_thread_) {
     return tl::make_unexpected("Time sync thread was not initialized.");
   }
+  if (!time_sync_thread_->WaitForSync(std::chrono::seconds(5))) {
+    return tl::make_unexpected("Failed to establish time sync before timing out.");
+  }
   if (!time_sync_thread_->HasEstablishedTimeSync()) {
     return tl::make_unexpected("Time sync not yet established between Spot and the local system.");
   }