Add power_on, power_off and brake_release to primary client

Author: urfeex

include/ur_client_library/helpers.h

@@ -29,6 +29,8 @@
 #ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED
 #define UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED
 
+#include <chrono>
+#include <functional>
 #ifdef _WIN32
 
 #  define NOMINMAX
@@ -63,5 +65,7 @@ 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,
+             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/abstract_primary_consumer.h

@@ -34,6 +34,7 @@
 #include "ur_client_library/primary/robot_message/version_message.h"
 #include "ur_client_library/primary/robot_message/error_code_message.h"
 #include "ur_client_library/primary/robot_state/kinematics_info.h"
+#include "ur_client_library/primary/robot_state/robot_mode_data.h"
 
 namespace urcl
 {
@@ -75,6 +76,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual bool consume(VersionMessage& pkg) = 0;
   virtual bool consume(KinematicsInfo& pkg) = 0;
   virtual bool consume(ErrorCodeMessage& pkg) = 0;
+  virtual bool consume(RobotModeData& pkg) = 0;
 
 private:
   /* data */

include/ur_client_library/primary/primary_client.h

@@ -31,6 +31,7 @@
 #ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED
 #define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED
 
+#include <chrono>
 #include <memory>
 #include <deque>
 
@@ -89,6 +90,58 @@ class PrimaryClient
 
   bool checkCalibration(const std::string& checksum);
 
+  /*!
+   * \brief Commands the robot to power on.
+   *
+   * \param validate If true, the function will block until the robot is powered on or the timeout
+   * 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.
+   */
+  bool commandPowerOn(const bool validate = true,
+                      const std::chrono::milliseconds timeout = std::chrono::milliseconds(5000));
+
+  /*!
+   * \brief Commands the robot to power off.
+   *
+   * \param validate If true, the function will block until the robot is powered off or the timeout
+   * 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.
+   */
+  bool commandPowerOff(const bool validate = true,
+                       const std::chrono::milliseconds timeout = std::chrono::milliseconds(5000));
+
+  /*!
+   * \brief Commands the robot to release the brakes
+   *
+   * \param validate If true, the function will block until the robot is running or the timeout
+   * 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.
+   */
+  bool commandBrakeRelease(const bool validate = true,
+                           const std::chrono::milliseconds timeout = std::chrono::milliseconds(5000));
+
+  /*!
+   * \brief Get the latest robot mode.
+   *
+   * The robot mode will be updated in the background. This will always show the latest received
+   * robot mode independent of the time that has passed since receiving it.
+   */
+  RobotMode getRobotMode()
+  {
+    std::shared_ptr<RobotModeData> robot_mode_data = consumer_->getRobotModeData();
+    if (robot_mode_data == nullptr)
+    {
+      return RobotMode::UNKNOWN;
+    }
+    return static_cast<RobotMode>(consumer_->getRobotModeData()->robot_mode_);
+  }
+
 private:
   /*!
    * \brief Reconnects the primary stream used to send program to the robot.
@@ -99,6 +152,9 @@ class PrimaryClient
    */
   bool reconnectStream();
 
+  bool waitFor(std::function<bool()> condition, const std::chrono::milliseconds timeout,
+               const std::chrono::milliseconds check_interval = std::chrono::milliseconds(50));
+
   // The function is called whenever an error code message is received
   void errorMessageCallback(ErrorCode& code);
 

include/ur_client_library/primary/primary_consumer.h

@@ -29,6 +29,8 @@
 #define UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED
 
 #include "ur_client_library/primary/abstract_primary_consumer.h"
+#include "ur_client_library/primary/robot_state/robot_mode_data.h"
+#include "ur_client_library/ur/datatypes.h"
 
 #include <functional>
 #include <mutex>
@@ -153,6 +155,14 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return true;
   }
 
+  virtual bool consume(RobotModeData& pkg) override
+  {
+    URCL_LOG_DEBUG("Robot mode is now %s", robotModeString(static_cast<RobotMode>(pkg.robot_mode_)).c_str());
+    std::scoped_lock lock(robot_mode_mutex_);
+    robot_mode_ = std::make_shared<RobotModeData>(pkg);
+    return true;
+  }
+
   /*!
    * \brief Set callback function which will be triggered whenever error code messages are received
    *
@@ -174,9 +184,23 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return kinematics_info_;
   }
 
+  /*!
+   * \brief Get the latest robot mode.
+   *
+   * The robot mode will be updated in the background. This will always show the latest received
+   * robot mode independent of the time that has passed since receiving it.
+   */
+  std::shared_ptr<RobotModeData> getRobotModeData()
+  {
+    std::scoped_lock lock(robot_mode_mutex_);
+    return robot_mode_;
+  }
+
 private:
   std::function<void(ErrorCode&)> error_code_message_callback_;
   std::shared_ptr<KinematicsInfo> kinematics_info_;
+  std::mutex robot_mode_mutex_;
+  std::shared_ptr<RobotModeData> robot_mode_;
 };
 
 }  // namespace primary_interface

src/helpers.cpp

@@ -32,6 +32,7 @@
 #include <cstring>
 #include <fstream>
 #include <iostream>
+#include <thread>
 
 // clang-format off
 // We want to keep the URL in one line to avoid formatting issues. This will make it easier to
@@ -99,4 +100,20 @@ bool setFiFoScheduling(pthread_t& thread, const int priority)
   return true;
 #endif
 }
+
+bool 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;
+    }
+    URCL_LOG_DEBUG("Waiting for condition to be met...");
+    std::this_thread::sleep_for(check_interval);
+  }
+  return false;
+}
 }  // namespace urcl

src/primary/primary_client.cpp

@@ -160,5 +160,60 @@ bool PrimaryClient::checkCalibration(const std::string& checksum)
   return kin_info->toHash() == checksum;
 }
 
+bool PrimaryClient::commandPowerOn(const bool validate, const std::chrono::milliseconds timeout)
+{
+  if (!sendScript("power on"))
+  {
+    return false;
+  }
+
+  if (validate)
+  {
+    return waitFor([this]() { return getRobotMode() == RobotMode::IDLE; }, timeout);
+  }
+  return true;
+}
+
+bool PrimaryClient::commandPowerOff(const bool validate, const std::chrono::milliseconds timeout)
+{
+  if (!sendScript("power off"))
+  {
+    return false;
+  }
+  if (validate)
+  {
+    return waitFor([this]() { return getRobotMode() == RobotMode::POWER_OFF; }, timeout);
+  }
+  return true;
+}
+
+bool PrimaryClient::commandBrakeRelease(const bool validate, const std::chrono::milliseconds timeout)
+{
+  if (!sendScript("set robotmode run"))
+  {
+    return false;
+  }
+  if (validate)
+  {
+    return waitFor([this]() { return getRobotMode() == RobotMode::RUNNING; }, timeout);
+  }
+  return true;
+}
+
+bool PrimaryClient::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;
+    }
+    URCL_LOG_DEBUG("Waiting for condition to be met...");
+    std::this_thread::sleep_for(check_interval);
+  }
+  return false;
+}
 }  // namespace primary_interface
 }  // namespace urcl

tests/test_primary_client.cpp

@@ -32,7 +32,10 @@
 
 #include <ur_client_library/primary/primary_client.h>
 #include <ur_client_library/ur/calibration_checker.h>
+#include <chrono>
 #include <memory>
+#include <thread>
+#include "ur_client_library/helpers.h"
 
 using namespace urcl;
 
@@ -77,9 +80,33 @@ TEST_F(PrimaryClientTest, add_and_remove_consumer)
   client_->removePrimaryConsumer(calibration_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());
+
+  // provoke a timeout
+  EXPECT_FALSE(client_->commandBrakeRelease(true, std::chrono::milliseconds(1)));
+
+  auto timeout = std::chrono::milliseconds(1000);
+  waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout);
+  EXPECT_EQ(client_->getRobotMode(), RobotMode::RUNNING);
+
+  EXPECT_TRUE(client_->commandPowerOff(false));
+  waitFor([this]() { return client_->getRobotMode() == RobotMode::POWER_OFF; }, timeout);
+
+  EXPECT_EQ(client_->getRobotMode(), RobotMode::POWER_OFF);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);
+  urcl::setLogLevel(urcl::LogLevel::DEBUG);
 
   return RUN_ALL_TESTS();
 }