Use exceptions instead of booleans to handle failures in commands

This should make using this easier and debug information / stack traces
should be more explicit.

Author: urfeex

include/ur_client_library/exceptions.h

@@ -120,6 +120,13 @@ class TimeoutException : public UrException
 {
 public:
   explicit TimeoutException() = delete;
+  explicit TimeoutException(const std::string& text, std::chrono::milliseconds timeout) : std::runtime_error(text)
+  {
+    std::stringstream ss;
+    ss << text << "(Configured timeout: " << timeout.count() / 1000.0 << " sec)";
+    text_ = ss.str();
+  }
+
   explicit TimeoutException(const std::string& text, timeval timeout) : std::runtime_error(text)
   {
     std::stringstream ss;

include/ur_client_library/helpers.h

@@ -65,7 +65,18 @@ static inline int sched_get_priority_max(int policy)
 namespace urcl
 {
 bool setFiFoScheduling(pthread_t& thread, const int priority);
-bool waitFor(std::function<bool()> condition, const std::chrono::milliseconds timeout,
+
+/*!
+ * \brief Wait for a condition to be true.
+ *
+ * This function will wait for a condition to be true. The condition is checked in intervals of \p check_interval.
+ * If the condition is not met after \p timeout, the function will throw a urcl::TimeoutException.
+ *
+ * \param condition The condition to be checked.
+ * \param timeout The maximum time to wait for the condition to be true.
+ * \param check_interval The interval in which the condition is checked.
+ */
+void waitFor(std::function<bool()> condition, const std::chrono::milliseconds timeout,
              const std::chrono::milliseconds check_interval = std::chrono::milliseconds(50));
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED

include/ur_client_library/primary/primary_client.h

@@ -97,9 +97,10 @@ class PrimaryClient
    * passed by.
    * \param timeout The maximum time to wait for the robot to confirm the power on command.
    *
-   * \returns true on successful power on, false otherwise.
+   * \throws urcl::UrException if the command cannot be sent to the robot.
+   * \throws urcl::TimeoutException if the robot doesn't power on within the given timeout.
    */
-  bool commandPowerOn(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30));
+  void commandPowerOn(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30));
 
   /*!
    * \brief Commands the robot to power off.
@@ -108,9 +109,10 @@ class PrimaryClient
    * passed by.
    * \param timeout The maximum time to wait for the robot to confirm the power off command.
    *
-   * \returns true on successful power off, false otherwise.
+   * \throws urcl::UrException if the command cannot be sent to the robot.
+   * \throws urcl::TimeoutException if the robot doesn't power off within the given timeout.
    */
-  bool commandPowerOff(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30));
+  void commandPowerOff(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30));
 
   /*!
    * \brief Commands the robot to release the brakes
@@ -119,9 +121,11 @@ class PrimaryClient
    * passed by.
    * \param timeout The maximum time to wait for the robot to confirm it is running.
    *
-   * \returns true on successful brake release, false otherwise.
+   * \throws urcl::UrException if the command cannot be sent to the robot.
+   * \throws urcl::TimeoutException if the robot doesn't release the brakes within the given
+   * timeout.
    */
-  bool commandBrakeRelease(const bool validate = true,
+  void commandBrakeRelease(const bool validate = true,
                            const std::chrono::milliseconds timeout = std::chrono::seconds(30));
 
   /*!

src/helpers.cpp

@@ -26,6 +26,7 @@
  */
 //----------------------------------------------------------------------
 
+#include <ur_client_library/exceptions.h>
 #include <ur_client_library/helpers.h>
 #include <ur_client_library/log.h>
 
@@ -101,19 +102,19 @@ bool setFiFoScheduling(pthread_t& thread, const int priority)
 #endif
 }
 
-bool waitFor(std::function<bool()> condition, const std::chrono::milliseconds timeout,
+void waitFor(std::function<bool()> condition, const std::chrono::milliseconds timeout,
              const std::chrono::milliseconds check_interval)
 {
   auto start_time = std::chrono::system_clock::now();
   while (std::chrono::system_clock::now() - start_time < timeout)
   {
     if (condition())
     {
-      return true;
+      return;
     }
     URCL_LOG_DEBUG("Waiting for condition to be met...");
     std::this_thread::sleep_for(check_interval);
   }
-  return false;
+  throw urcl::TimeoutException("Timeout while waiting for condition to be met", timeout);
 }
 }  // namespace urcl

src/primary/primary_client.cpp

@@ -31,6 +31,7 @@
 #include <ur_client_library/primary/primary_client.h>
 #include <ur_client_library/primary/robot_message.h>
 #include <ur_client_library/primary/robot_state.h>
+#include "ur_client_library/exceptions.h"
 
 namespace urcl
 {
@@ -160,44 +161,62 @@ bool PrimaryClient::checkCalibration(const std::string& checksum)
   return kin_info->toHash() == checksum;
 }
 
-bool PrimaryClient::commandPowerOn(const bool validate, const std::chrono::milliseconds timeout)
+void PrimaryClient::commandPowerOn(const bool validate, const std::chrono::milliseconds timeout)
 {
   if (!sendScript("power on"))
   {
-    return false;
+    throw UrException("Failed to send power on command to robot");
   }
 
   if (validate)
   {
-    return waitFor([this]() { return getRobotMode() == RobotMode::IDLE; }, timeout);
+    try
+    {
+      waitFor([this]() { return getRobotMode() == RobotMode::IDLE; }, timeout);
+    }
+    catch (const TimeoutException& ex)
+    {
+      throw TimeoutException("Robot did not power on within the given timeout", timeout);
+    }
   }
-  return true;
 }
 
-bool PrimaryClient::commandPowerOff(const bool validate, const std::chrono::milliseconds timeout)
+void PrimaryClient::commandPowerOff(const bool validate, const std::chrono::milliseconds timeout)
 {
   if (!sendScript("power off"))
   {
-    return false;
+    throw UrException("Failed to send power off command to robot");
   }
   if (validate)
   {
-    return waitFor([this]() { return getRobotMode() == RobotMode::POWER_OFF; }, timeout);
+    try
+    {
+      waitFor([this]() { return getRobotMode() == RobotMode::POWER_OFF; }, timeout);
+    }
+    catch (const std::exception&)
+    {
+      throw TimeoutException("Robot did not power off within the given timeout", timeout);
+    }
   }
-  return true;
 }
 
-bool PrimaryClient::commandBrakeRelease(const bool validate, const std::chrono::milliseconds timeout)
+void PrimaryClient::commandBrakeRelease(const bool validate, const std::chrono::milliseconds timeout)
 {
   if (!sendScript("set robotmode run"))
   {
-    return false;
+    throw UrException("Failed to send brake release command to robot");
   }
   if (validate)
   {
-    return waitFor([this]() { return getRobotMode() == RobotMode::RUNNING; }, timeout);
+    try
+    {
+      waitFor([this]() { return getRobotMode() == RobotMode::RUNNING; }, timeout);
+    }
+    catch (const std::exception&)
+    {
+      throw TimeoutException("Robot did not release the brakes within the given timeout", timeout);
+    }
   }
-  return true;
 }
 
 }  // namespace primary_interface

tests/test_primary_client.cpp

@@ -35,6 +35,7 @@
 #include <chrono>
 #include <memory>
 #include <thread>
+#include "ur_client_library/exceptions.h"
 #include "ur_client_library/helpers.h"
 
 using namespace urcl;
@@ -83,24 +84,21 @@ TEST_F(PrimaryClientTest, add_and_remove_consumer)
 TEST_F(PrimaryClientTest, test_power_cycle_commands)
 {
   EXPECT_NO_THROW(client_->start());
-  EXPECT_TRUE(client_->commandPowerOff());
-  EXPECT_TRUE(client_->commandPowerOn());
-  EXPECT_TRUE(client_->commandBrakeRelease());
-  EXPECT_TRUE(client_->commandPowerOff());
-  EXPECT_TRUE(client_->commandBrakeRelease());
-  EXPECT_TRUE(client_->commandPowerOff());
+  EXPECT_NO_THROW(client_->commandPowerOff());
+  EXPECT_NO_THROW(client_->commandPowerOn());
+  EXPECT_NO_THROW(client_->commandBrakeRelease());
+  EXPECT_NO_THROW(client_->commandPowerOff());
+  EXPECT_NO_THROW(client_->commandBrakeRelease());
+  EXPECT_NO_THROW(client_->commandPowerOff());
 
   // provoke a timeout
-  EXPECT_FALSE(client_->commandBrakeRelease(true, std::chrono::milliseconds(1)));
+  EXPECT_THROW(client_->commandBrakeRelease(true, std::chrono::milliseconds(1)), urcl::TimeoutException);
 
   auto timeout = std::chrono::seconds(30);
-  waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout);
-  EXPECT_EQ(client_->getRobotMode(), RobotMode::RUNNING);
+  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout));
 
-  EXPECT_TRUE(client_->commandPowerOff(false));
-  waitFor([this]() { return client_->getRobotMode() == RobotMode::POWER_OFF; }, timeout);
-
-  EXPECT_EQ(client_->getRobotMode(), RobotMode::POWER_OFF);
+  EXPECT_NO_THROW(client_->commandPowerOff(false));
+  EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::POWER_OFF; }, timeout));
 }
 
 TEST_F(PrimaryClientTest, test_uninitialized_primary_client)
@@ -122,4 +120,4 @@ int main(int argc, char* argv[])
   }
 
   return RUN_ALL_TESTS();
-}
+}