CR suggestions - use future in roboteq driver boot

panther_hardware_interfaces/include/panther_hardware_interfaces/roboteq_driver.hpp

@@ -17,8 +17,8 @@
 
 #include <atomic>
 #include <chrono>
-#include <condition_variable>
 #include <cstdint>
+#include <future>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -74,18 +74,11 @@ class RoboteqDriver : public lely::canopen::LoopDriver
     const std::chrono::milliseconds & sdo_operation_timeout_ms);
 
   /**
-   * @brief Trigger boot operations
-   */
-  bool Boot();
-
-  /**
-   * @brief Waits until the booting procedure finishes
+   * @brief Triggers boot operations
    *
-   * @exception std::runtime_error if boot fails
+   * @exception std::runtime_error if triggering boot fails
    */
-  bool WaitForBoot();
-
-  bool IsBooted() const { return booted_.load(); }
+  std::future<void> Boot();
 
   bool IsCANError() const { return can_error_.load(); }
 
@@ -157,10 +150,7 @@ class RoboteqDriver : public lely::canopen::LoopDriver
     can_error_.store(true);
   }
 
-  std::atomic_bool booted_ = false;
-  std::condition_variable boot_cond_var_;
-  std::mutex boot_mtx_;
-  std::string boot_error_str_;
+  std::promise<void> boot_promise_;
 
   std::atomic_bool can_error_;
 

panther_hardware_interfaces/src/canopen_controller.cpp

@@ -16,6 +16,7 @@
 
 #include <condition_variable>
 #include <filesystem>
+#include <future>
 #include <iostream>
 #include <memory>
 #include <mutex>
@@ -161,29 +162,28 @@ void CANopenController::NotifyCANCommunicationStarted(const bool result)
 void CANopenController::BootDrivers()
 {
   try {
-    front_driver_->Boot();
-  } catch (const std::system_error & e) {
-    throw std::runtime_error(
-      "Exception caught when trying to Boot front driver " + std::string(e.what()));
-  }
+    auto front_driver_future = front_driver_->Boot();
+    auto rear_driver_future = rear_driver_->Boot();
+
+    auto front_driver_status = front_driver_future.wait_for(std::chrono::seconds(5));
+    auto rear_driver_status = rear_driver_future.wait_for(std::chrono::seconds(5));
+
+    if (
+      front_driver_status == std::future_status::ready &&
+      rear_driver_status == std::future_status::ready) {
+      try {
+        front_driver_future.get();
+        rear_driver_future.get();
+      } catch (const std::exception & e) {
+        throw std::runtime_error("Boot failed with exception: " + std::string(e.what()));
+      }
+    } else {
+      throw std::runtime_error("Boot timed out or failed.");
+    }
 
-  try {
-    rear_driver_->Boot();
   } catch (const std::system_error & e) {
     throw std::runtime_error(
-      "Exception caught when trying to Boot rear driver " + std::string(e.what()));
-  }
-
-  try {
-    front_driver_->WaitForBoot();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error("Front driver boot failed " + std::string(e.what()));
-  }
-
-  try {
-    rear_driver_->WaitForBoot();
-  } catch (const std::runtime_error & e) {
-    throw std::runtime_error("Rear driver boot failed " + std::string(e.what()));
+      "Exception caught when trying to Boot driver " + std::string(e.what()));
   }
 }
 

panther_hardware_interfaces/src/roboteq_driver.cpp

@@ -16,7 +16,6 @@
 
 #include <chrono>
 #include <cmath>
-#include <condition_variable>
 #include <cstdint>
 #include <memory>
 #include <mutex>
@@ -74,28 +73,13 @@ RoboteqDriver::RoboteqDriver(
 {
 }
 
-bool RoboteqDriver::Boot()
+std::future<void> RoboteqDriver::Boot()
 {
-  booted_.store(false);
-  return LoopDriver::Boot();
-}
-
-bool RoboteqDriver::WaitForBoot()
-{
-  if (booted_.load()) {
-    return true;
-  }
-  std::unique_lock<std::mutex> lck(boot_mtx_);
-
-  if (boot_cond_var_.wait_for(lck, std::chrono::seconds(5)) == std::cv_status::timeout) {
-    throw std::runtime_error("Timeout while waiting for boot");
-  }
-
-  if (booted_.load()) {
-    return true;
-  } else {
-    throw std::runtime_error(boot_error_str_);
+  std::future<void> future = boot_promise_.get_future();
+  if (!LoopDriver::Boot()) {
+    throw std::runtime_error("Boot failed");
   }
+  return future;
 }
 
 RoboteqMotorsStates RoboteqDriver::ReadRoboteqMotorsStates()
@@ -268,13 +252,9 @@ void RoboteqDriver::OnBoot(
   LoopDriver::OnBoot(st, es, what);
 
   if (!es || es == 'L') {
-    booted_.store(true);
-  }
-
-  {
-    std::lock_guard<std::mutex> lck(boot_mtx_);
-    boot_error_str_ = what;
-    boot_cond_var_.notify_all();
+    boot_promise_.set_value();
+  } else {
+    boot_promise_.set_exception(std::make_exception_ptr(std::runtime_error(what)));
   }
 }