Primary client power on (#289)

This allows power-cycling the robot remotely without using the dashboard
server.

Author: urfeex

CMakeLists.txt

@@ -28,6 +28,7 @@ add_library(urcl
     src/primary/robot_message/version_message.cpp
     src/primary/robot_message/error_code_message.cpp
     src/primary/robot_state/kinematics_info.cpp
+    src/primary/robot_state/robot_mode_data.cpp
     src/rtde/control_package_pause.cpp
     src/rtde/control_package_setup_inputs.cpp
     src/rtde/control_package_setup_outputs.cpp

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

@@ -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,18 @@ static inline int sched_get_priority_max(int policy)
 namespace urcl
 {
 bool setFiFoScheduling(pthread_t& thread, const int priority);
-}
+
+/*!
+ * \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/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,60 @@ 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.
+   *
+   * \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.
+   */
+  void commandPowerOn(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30));
+
+  /*!
+   * \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.
+   *
+   * \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.
+   */
+  void commandPowerOff(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30));
+
+  /*!
+   * \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.
+   *
+   * \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.
+   */
+  void commandBrakeRelease(const bool validate = true,
+                           const std::chrono::milliseconds timeout = std::chrono::seconds(30));
+
+  /*!
+   * \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.

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

include/ur_client_library/primary/primary_parser.h

@@ -29,6 +29,7 @@
 #include "ur_client_library/primary/robot_state/kinematics_info.h"
 #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/robot_mode_data.h"
 
 namespace urcl
 {
@@ -150,12 +151,12 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
   {
     switch (type)
     {
-      /*case robot_state_type::ROBOT_MODE_DATA:
-        // SharedRobotModeData* rmd = new SharedRobotModeData();
+      case RobotStateType::ROBOT_MODE_DATA:
+        return new RobotModeData(type);
 
-        //return new rmd;
-      case robot_state_type::MASTERBOARD_DATA:
-        return new MBD;*/
+        // return new rmd;
+      // case robot_state_type::MASTERBOARD_DATA:
+      // return new MBD;*/
       case RobotStateType::KINEMATICS_INFO:
         return new KinematicsInfo(type);
       default:

include/ur_client_library/primary/robot_state/robot_mode_data.h

@@ -0,0 +1,112 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2025 Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED
+#define UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED
+
+#include <stdexcept>
+#include "ur_client_library/types.h"
+#include "ur_client_library/primary/robot_state.h"
+namespace urcl
+{
+namespace primary_interface
+{
+
+/*!
+ * \brief This messages contains data about the mode of the robot.
+ */
+class RobotModeData : public RobotState
+{
+public:
+  RobotModeData() = delete;
+  /*!
+   * \brief Creates a new RobotModeData object.
+   *
+   * \param type The type of RobotState message received
+   */
+  RobotModeData(const RobotStateType type) : RobotState(type)
+  {
+  }
+
+  /*!
+   * \brief Creates a copy of a RobotModeData object.
+   *
+   * \param pkg The RobotModeData object to be copied
+   */
+  RobotModeData(const RobotModeData& pkg);
+
+  virtual ~RobotModeData() = default;
+
+  /*!
+   * \brief Sets the attributes of the package by parsing a serialized representation of the
+   * package.
+   *
+   * \param bp A parser containing a serialized version of the package
+   *
+   * \returns True, if the package was parsed successfully, false otherwise
+   */
+  virtual bool parseWith(comm::BinParser& bp);
+
+  /*!
+   * \brief Consume this specific package with a specific consumer.
+   *
+   * \param consumer Placeholder for the consumer calling this
+   *
+   * \returns true on success
+   */
+  virtual bool consumeWith(AbstractPrimaryConsumer& consumer);
+
+  /*!
+   * \brief Produces a human readable representation of the package object.
+   *
+   * \returns A string representing the object
+   */
+  virtual std::string toString() const;
+
+  uint64_t timestamp_;
+  bool is_real_robot_connected_;
+  bool is_real_robot_enabled_;
+  bool is_robot_power_on_;
+  bool is_emergency_stopped_;
+  bool is_protective_stopped_;
+  bool is_program_running_;
+  bool is_program_paused_;
+  int8_t robot_mode_;
+  uint8_t control_mode_;
+  double target_speed_fraction_;
+  double speed_scaling_;
+  double target_speed_fraction_limit_;
+  std::string reserved_;
+};
+
+}  // namespace primary_interface
+}  // namespace urcl
+
+#endif  // ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED

src/helpers.cpp

@@ -26,12 +26,14 @@
  */
 //----------------------------------------------------------------------
 
+#include <ur_client_library/exceptions.h>
 #include <ur_client_library/helpers.h>
 #include <ur_client_library/log.h>
 
 #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 +101,20 @@ bool setFiFoScheduling(pthread_t& thread, const int priority)
   return true;
 #endif
 }
+
+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;
+    }
+    URCL_LOG_DEBUG("Waiting for condition to be met...");
+    std::this_thread::sleep_for(check_interval);
+  }
+  throw urcl::TimeoutException("Timeout while waiting for condition to be met", timeout);
+}
 }  // namespace urcl