Add parsing of safety mode messages to the primary interface (UniversalRobots#480)

Author: URJala

CMakeLists.txt

@@ -33,6 +33,7 @@ add_library(urcl
     src/primary/robot_message/runtime_exception_message.cpp
     src/primary/robot_message/text_message.cpp
     src/primary/robot_message/version_message.cpp
+    src/primary/robot_message/safety_mode_message.cpp
     src/primary/robot_state.cpp
     src/primary/robot_state/kinematics_info.cpp
     src/primary/robot_state/robot_mode_data.cpp

include/ur_client_library/comm/bin_parser.h

@@ -32,6 +32,7 @@
 #include "ur_client_library/log.h"
 #include "ur_client_library/types.h"
 #include "ur_client_library/exceptions.h"
+#include <variant>
 
 namespace urcl
 {
@@ -142,6 +143,20 @@ class BinParser
     buf_pos_ += sizeof(T);
   }
 
+  /*!
+   * \brief Parses the next bytes as the given type, the given type must match one of the types in the variant
+   *
+   * @tparam T Type to parse as
+   * \param val Reference to variant to write the parsed value to
+   */
+  template <typename T>
+  void parse(std::variant<uint32_t, int32_t, float>& val)
+  {
+    T return_val;
+    parse<T>(return_val);
+    val = return_val;
+  }
+
   /*!
    * \brief Parses the next bytes as a double.
    *

include/ur_client_library/primary/abstract_primary_consumer.h

@@ -36,6 +36,7 @@
 #include "ur_client_library/primary/robot_state/kinematics_info.h"
 #include "ur_client_library/primary/robot_state/robot_mode_data.h"
 #include "ur_client_library/primary/robot_state/configuration_data.h"
+#include "ur_client_library/primary/robot_message/safety_mode_message.h"
 
 namespace urcl
 {
@@ -79,6 +80,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual bool consume(ErrorCodeMessage& pkg) = 0;
   virtual bool consume(RobotModeData& pkg) = 0;
   virtual bool consume(ConfigurationData& pkg) = 0;
+  virtual bool consume(SafetyModeMessage& pkg) = 0;
 
 private:
   /* data */

include/ur_client_library/primary/primary_client.h

@@ -200,6 +200,34 @@ class PrimaryClient
     return consumer_->getRobotModeData();
   }
 
+  /*!
+   * \brief Get the latest safety mode message.
+   *
+   * The safety mode will be updated in the background. This will always show the latest received
+   * state independent of the time that has passed since receiving it. The return value of this
+   * will be a nullptr if no data has been received yet.
+   */
+  std::shared_ptr<SafetyModeMessage> getSafetyModeMessage()
+  {
+    return consumer_->getSafetyModeMessage();
+  }
+
+  /*!
+   * \brief Get the latest safety mode.
+   *
+   * The safety 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.
+   */
+  SafetyMode getSafetyMode()
+  {
+    auto safety_mode = consumer_->getSafetyModeMessage();
+    if (safety_mode == nullptr)
+    {
+      return SafetyMode::UNDEFINED_SAFETY_MODE;
+    }
+    return safety_mode->safety_mode_type_;
+  }
+
   /*!
    * \brief Query if the robot is protective stopped.
    *

include/ur_client_library/primary/primary_consumer.h

@@ -170,6 +170,14 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return true;
   }
 
+  virtual bool consume(SafetyModeMessage& pkg) override
+  {
+    URCL_LOG_DEBUG("Robot safety mode is now %s", safetyModeString(pkg.safety_mode_type_).c_str());
+    std::scoped_lock lock(safety_mode_mutex_);
+    safety_mode_ = std::make_shared<SafetyModeMessage>(pkg);
+    return true;
+  }
+
   virtual bool consume(ConfigurationData& pkg) override
   {
     std::scoped_lock lock(configuration_data_mutex_);
@@ -210,6 +218,18 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return robot_mode_;
   }
 
+  /*!
+   * \brief Get the latest safety mode message.
+   *
+   * The safety mode will be updated in the background. This will always show the latest received
+   * safety mode independent of the time that has passed since receiving it.
+   */
+  std::shared_ptr<SafetyModeMessage> getSafetyModeMessage()
+  {
+    std::scoped_lock lock(safety_mode_mutex_);
+    return safety_mode_;
+  }
+
   /*!
    * \brief Get the latest version information.
    *
@@ -245,6 +265,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   std::shared_ptr<VersionInformation> version_information_;
   std::shared_ptr<ConfigurationData> configuration_data_;
   std::mutex configuration_data_mutex_;
+  std::mutex safety_mode_mutex_;
+  std::shared_ptr<SafetyModeMessage> safety_mode_;
 };
 
 }  // namespace primary_interface

include/ur_client_library/primary/primary_parser.h

@@ -34,6 +34,7 @@
 #include "ur_client_library/primary/robot_message/error_code_message.h"
 #include "ur_client_library/primary/robot_state/robot_mode_data.h"
 #include "ur_client_library/primary/robot_state/configuration_data.h"
+#include "ur_client_library/primary/robot_message/safety_mode_message.h"
 
 namespace urcl
 {
@@ -241,6 +242,8 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
         return new KeyMessage(timestamp, source);
       case RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION:
         return new RuntimeExceptionMessage(timestamp, source);
+      case RobotMessagePackageType::ROBOT_MESSAGE_SAFETY_MODE:
+        return new SafetyModeMessage(timestamp, source);
       default:
         return new RobotMessage(timestamp, source, type);
     }

include/ur_client_library/primary/robot_message/safety_mode_message.h

@@ -0,0 +1,116 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2026 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 ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author      Jacob Larsen jala@universal-robots.com
+ * \maintainer  Felix Exner feex@universal-robots.com
+ * \date        2026-03-30
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CLIENT_LIBRARY_PRIMARY_SAFETY_MODE_MESSAGE_H_INCLUDED
+#define UR_CLIENT_LIBRARY_PRIMARY_SAFETY_MODE_MESSAGE_H_INCLUDED
+
+#include "ur_client_library/primary/robot_message.h"
+#include "ur_client_library/ur/datatypes.h"
+#include <variant>
+
+namespace urcl
+{
+namespace primary_interface
+{
+/*!
+ *  \brief Representation of the primary interface's safety mode message
+ */
+class SafetyModeMessage : public RobotMessage
+{
+public:
+  SafetyModeMessage() = delete;
+
+  /*!
+   * \brief Creates a new SafetyModeMessage object to be filled from a package.
+   *
+   * \param timestamp Timestamp of the package
+   * \param source The package's source
+   */
+  SafetyModeMessage(uint64_t timestamp, int8_t source)
+    : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_SAFETY_MODE)
+  {
+  }
+  SafetyModeMessage(const SafetyModeMessage& pkg);
+
+  virtual ~SafetyModeMessage() = default;
+
+  /*!
+   * \brief Sets the attributes of the package by parsing a serialized representation of the
+   * package.
+   *
+   * \param bp A parser containing a serialized text of the package
+   *
+   * \returns True, if the package was parsed successfully, false otherwise
+   */
+  virtual bool parseWith(comm::BinParser& bp);
+
+  /*!
+   * \brief Consume this 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;
+
+  int32_t message_code_;
+  int32_t message_argument_;
+  SafetyMode safety_mode_type_;
+  uint32_t report_data_type_;
+
+  /* Content data type varies depending on report_data_type_
+  Type 0 and 1: uint32_t
+  Type 2: int32_t
+  Type 3: float
+  Type 4 : uint32_t (but should be displayed as hex, when printed) */
+  std::variant<uint32_t, int32_t, float> report_data_;
+};
+}  // namespace primary_interface
+
+}  // namespace urcl
+
+#endif  // ifndef UR_CLIENT_LIBRARY_PRIMARY_SAFETY_MODE_MESSAGE_H_INCLUDED

include/ur_client_library/ur/datatypes.h

@@ -49,7 +49,7 @@ enum class RobotMode : int8_t
   UPDATING_FIRMWARE = 8
 };
 
-enum class SafetyMode : int8_t
+enum class SafetyMode : uint8_t
 {
   NORMAL = 1,
   REDUCED = 2,
@@ -61,7 +61,17 @@ enum class SafetyMode : int8_t
   VIOLATION = 8,
   FAULT = 9,
   VALIDATE_JOINT_ID = 10,
-  UNDEFINED_SAFETY_MODE = 11
+  UNDEFINED_SAFETY_MODE = 11,
+  AUTOMATIC_MODE_SAFEGUARD_STOP = 12,
+  SYSTEM_THREE_POSITION_ENABLING_STOP = 13,
+  TP_THREE_POSITION_ENABLING_STOP = 14,
+  IMMI_EMERGENCY_STOP = 15,
+  IMMI_SAFEGUARD_STOP = 16,
+  PROFISAFE_WAITING_FOR_PARAMETERS = 17,
+  PROFISAFE_AUTOMATIC_MODE_SAFEGUARD_STOP = 18,
+  PROFISAFE_SAFEGUARD_STOP = 19,
+  PROFISAFE_EMERGENCY_STOP = 20,
+  SAFETY_API_SAFEGUARD_STOP = 22
 };
 
 enum class SafetyStatus : int8_t  // Only available on 3.10/5.4
@@ -159,6 +169,26 @@ inline std::string safetyModeString(const SafetyMode& mode)
       return "VALIDATE_JOINT_ID";
     case SafetyMode::UNDEFINED_SAFETY_MODE:
       return "UNDEFINED_SAFETY_MODE";
+    case SafetyMode::AUTOMATIC_MODE_SAFEGUARD_STOP:
+      return "AUTOMATIC_MODE_SAFEGUARD_STOP";
+    case SafetyMode::SYSTEM_THREE_POSITION_ENABLING_STOP:
+      return "SYSTEM_THREE_POSITION_ENABLING_STOP";
+    case SafetyMode::TP_THREE_POSITION_ENABLING_STOP:
+      return "TP_THREE_POSITION_ENABLING_STOP";
+    case SafetyMode::IMMI_EMERGENCY_STOP:
+      return "IMMI_EMERGENCY_STOP";
+    case SafetyMode::IMMI_SAFEGUARD_STOP:
+      return "IMMI_SAFEGUARD_STOP";
+    case SafetyMode::PROFISAFE_WAITING_FOR_PARAMETERS:
+      return "PROFISAFE_WAITING_FOR_PARAMETERS";
+    case SafetyMode::PROFISAFE_AUTOMATIC_MODE_SAFEGUARD_STOP:
+      return "PROFISAFE_AUTOMATIC_MODE_SAFEGUARD_STOP";
+    case SafetyMode::PROFISAFE_SAFEGUARD_STOP:
+      return "PROFISAFE_SAFEGUARD_STOP";
+    case SafetyMode::PROFISAFE_EMERGENCY_STOP:
+      return "PROFISAFE_EMERGENCY_STOP";
+    case SafetyMode::SAFETY_API_SAFEGUARD_STOP:
+      return "SAFETY_API_SAFEGUARD_STOP";
     default:
       std::stringstream ss;
       ss << "Unknown safety mode: " << static_cast<int>(mode);