Expose diagnostic error codes (#225)

This adds very basic support for a primary interface client. With this, it is possible to get a list of all recorded error codes sent from the robot.

The client is intended to be further extended in the future.

---------

Co-authored-by: Felix Exner <feex@universal-robots.com>

Author: jessica-chen-ocado

CMakeLists.txt

@@ -16,9 +16,11 @@ add_library(urcl SHARED
     src/control/trajectory_point_interface.cpp
     src/control/script_command_interface.cpp
     src/primary/primary_package.cpp
+    src/primary/primary_client.cpp
     src/primary/robot_message.cpp
     src/primary/robot_state.cpp
     src/primary/robot_message/version_message.cpp
+    src/primary/robot_message/error_code_message.cpp
     src/primary/robot_state/kinematics_info.cpp
     src/rtde/control_package_pause.cpp
     src/rtde/control_package_setup_inputs.cpp

include/ur_client_library/comm/pipeline.h

@@ -29,6 +29,8 @@
 #include <thread>
 #include <vector>
 #include <fstream>
+#include <mutex>
+#include <algorithm>
 
 namespace urcl
 {
@@ -83,26 +85,54 @@ class IConsumer
 
 /*!
  * \brief Consumer, that allows one product to be consumed by multiple arbitrary
- * conusmers.
+ * consumers.
  *
  * @tparam T Type of the consumed products
  */
 template <typename T>
 class MultiConsumer : public IConsumer<T>
 {
 private:
-  std::vector<IConsumer<T>*> consumers_;
+  std::vector<std::shared_ptr<IConsumer<T>>> consumers_;
 
 public:
   /*!
    * \brief Creates a new MultiConsumer object.
    *
    * \param consumers The list of consumers that should all consume given products
    */
-  MultiConsumer(std::vector<IConsumer<T>*> consumers) : consumers_(consumers)
+  MultiConsumer(std::vector<std::shared_ptr<IConsumer<T>>> consumers) : consumers_(consumers)
   {
   }
 
+  /*!
+   * \brief Adds a new consumer to the list of consumers
+   *
+   * \param consumer Consumer that should be added to the list
+   */
+  void addConsumer(std::shared_ptr<IConsumer<T>> consumer)
+  {
+    std::lock_guard<std::mutex> lk(consumer_list_);
+    consumers_.push_back(consumer);
+  }
+
+  /*!
+   * \brief Remove a consumer from the list of consumers
+   *
+   * \param consumer Consumer that should be removed from the list
+   */
+  void removeConsumer(std::shared_ptr<IConsumer<T>> consumer)
+  {
+    std::lock_guard<std::mutex> lk(consumer_list_);
+    auto it = std::find(consumers_.begin(), consumers_.end(), consumer);
+    if (it == consumers_.end())
+    {
+      URCL_LOG_ERROR("Unable to remove consumer as it is not part of the consumer list");
+      return;
+    }
+    consumers_.erase(it);
+  }
+
   /*!
    * \brief Sets up all registered consumers.
    */
@@ -153,6 +183,7 @@ class MultiConsumer : public IConsumer<T>
    */
   bool consume(std::shared_ptr<T> product)
   {
+    std::lock_guard<std::mutex> lk(consumer_list_);
     bool res = true;
     for (auto& con : consumers_)
     {
@@ -161,6 +192,9 @@ class MultiConsumer : public IConsumer<T>
     }
     return res;
   }
+
+private:
+  std::mutex consumer_list_;
 };
 
 /*!
@@ -234,7 +268,7 @@ class INotifier
 };
 
 /*!
- * \brief The Pipepline manages the production and optionally consumption of packages. Cyclically
+ * \brief The Pipeline manages the production and optionally consumption of packages. Cyclically
  * the producer is called and returned packages are saved in a queue. This queue is then either also
  * cyclically utilized by the registered consumer or can be externally used.
  *

include/ur_client_library/primary/abstract_primary_consumer.h

@@ -32,6 +32,7 @@
 #include "ur_client_library/log.h"
 #include "ur_client_library/comm/pipeline.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/kinematics_info.h"
 
 namespace urcl
@@ -51,7 +52,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual ~AbstractPrimaryConsumer() = default;
 
   /*!
-   * \brief This consume method is usally being called by the Pipeline structure. We don't
+   * \brief This consume method is usually being called by the Pipeline structure. We don't
    * necessarily need to know the specific package type here, as the packages themselves will take
    * care to be consumed with the correct function (visitor pattern).
    *
@@ -73,6 +74,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual bool consume(RobotState& pkg) = 0;
   virtual bool consume(VersionMessage& pkg) = 0;
   virtual bool consume(KinematicsInfo& pkg) = 0;
+  virtual bool consume(ErrorCodeMessage& pkg) = 0;
 
 private:
   /* data */

include/ur_client_library/primary/primary_client.h

@@ -0,0 +1,98 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright © 2024-2025 Ocado Group
+//
+// 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_PRIMARY_CLIENT_H_INCLUDED
+#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED
+
+#include <memory>
+#include <deque>
+
+#include <ur_client_library/comm/stream.h>
+#include <ur_client_library/comm/pipeline.h>
+#include <ur_client_library/comm/producer.h>
+#include <ur_client_library/primary/abstract_primary_consumer.h>
+#include <ur_client_library/primary/primary_consumer.h>
+#include <ur_client_library/primary/primary_package.h>
+#include <ur_client_library/primary/primary_parser.h>
+
+namespace urcl
+{
+namespace primary_interface
+{
+class PrimaryClient
+{
+public:
+  PrimaryClient() = delete;
+  PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier);
+  ~PrimaryClient();
+
+  /*!
+   * \brief Adds a primary consumer to the list of consumers
+   *
+   * \param primary_consumer Primary consumer that should be added to the list
+   */
+  void addPrimaryConsumer(std::shared_ptr<comm::IConsumer<PrimaryPackage>> primary_consumer);
+
+  /*!
+   * \brief Remove a primary consumer from the list of consumers
+   *
+   * \param primary_consumer Primary consumer that should be removed from the list
+   */
+  void removePrimaryConsumer(std::shared_ptr<comm::IConsumer<PrimaryPackage>> primary_consumer);
+  void start();
+
+  /*!
+   * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be
+   * deleted.
+   */
+  std::deque<ErrorCode> getErrorCodes();
+
+private:
+  // The function is called whenever an error code message is received
+  void errorMessageCallback(ErrorCode& code);
+
+  PrimaryParser parser_;
+  std::shared_ptr<PrimaryConsumer> consumer_;
+  std::unique_ptr<comm::MultiConsumer<PrimaryPackage>> multi_consumer_;
+
+  comm::INotifier notifier_;
+
+  comm::URStream<PrimaryPackage> stream_;
+  std::unique_ptr<comm::URProducer<PrimaryPackage>> prod_;
+  std::unique_ptr<comm::Pipeline<PrimaryPackage>> pipeline_;
+
+  std::mutex error_code_queue_mutex_;
+  std::deque<ErrorCode> error_code_queue_;
+};
+
+}  // namespace primary_interface
+}  // namespace urcl
+
+#endif  // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED