Addressed PR comments

Author: jessica-chen-ocado

CMakeLists.txt

@@ -16,6 +16,7 @@ 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
@@ -34,7 +35,6 @@ add_library(urcl SHARED
     src/ur/ur_driver.cpp
     src/ur/calibration_checker.cpp
     src/ur/error_code_reader.cpp
-    src/ur/error_code_client.cpp
     src/ur/dashboard_client.cpp
     src/ur/tool_communication.cpp
     src/ur/robot_receive_timeout.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,59 @@
+#include <memory>
+
+#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>
+#include <ur_client_library/ur/error_code_reader.h>
+namespace urcl
+{
+namespace primary_interface
+{
+class PrimaryClient
+{
+public:
+  PrimaryClient() = delete;
+  PrimaryClient(comm::URStream<PrimaryPackage>& stream);
+  ~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<AbstractPrimaryConsumer> 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<AbstractPrimaryConsumer> primary_consumer);
+  void start();
+
+  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

include/ur_client_library/primary/primary_consumer.h

@@ -0,0 +1,172 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+//
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2020 FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Exner [email protected]
+ * \date    2020-04-30
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED
+#define UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED
+
+#include "ur_client_library/primary/abstract_primary_consumer.h"
+
+#include <functional>
+#include <mutex>
+#include <condition_variable>
+
+namespace urcl
+{
+namespace primary_interface
+{
+/*!
+ * \brief Primary consumer implementation
+ *
+ * This class implements an AbstractPrimaryConsumer such that it can consume all incoming primary
+ * messages. The actual work for each package will be done in this class.
+ */
+class PrimaryConsumer : public AbstractPrimaryConsumer
+{
+public:
+  PrimaryConsumer() {}
+  virtual ~PrimaryConsumer() = default;
+
+  /*!
+   * \brief Consume a RobotMessage
+   *
+   * \param msg Robot message
+   *
+   * \returns True
+   */
+  virtual bool consume(RobotMessage& msg) override
+  {
+    return true;
+  }
+
+  /*!
+   * \brief Consume a RobotState
+   *
+   * \param msg Robot state
+   *
+   * \returns True
+   */
+  virtual bool consume(RobotState& msg) override
+  {
+    return true;
+  }
+
+  /*!
+   * \brief Handle a VersionMessage
+   *
+   * \param pkg VersionMessage
+   *
+   * \returns True
+   */
+  virtual bool consume(VersionMessage& pkg) override
+  {
+    return true;
+  }
+
+  /*!
+   * \brief Handle a KinematicsInfo
+   *
+   * \param pkg KinematicsInfo
+   *
+   * \returns True
+   */
+  virtual bool consume(KinematicsInfo& pkg) override
+  {
+    return true;
+  }
+
+  /*!
+   * \brief Handle an ErrorCodeMessage
+   *
+   * \param pkg ErrorCodeMessage
+   *
+   * \returns True
+   */
+  virtual bool consume(ErrorCodeMessage& pkg) override
+  {
+    urcl::primary_interface::ErrorCode code;
+    code.message_code = pkg.message_code_;
+    code.message_argument = pkg.message_argument_;
+    code.report_level = pkg.report_level_;
+    code.data_type = pkg.data_type_;
+    code.data = pkg.data_;
+    code.text = pkg.text_;
+    code.timestamp = pkg.timestamp_;
+    code.to_string = pkg.toString();
+
+    const auto logContents =
+        "Logging an ErrorCodeMessage from the UR Controller Box: " + pkg.toString();
+
+    switch (code.report_level)
+    {
+      case urcl::primary_interface::ReportLevel::DEBUG:
+      case urcl::primary_interface::ReportLevel::DEVL_DEBUG:
+      case urcl::primary_interface::ReportLevel::DEVL_INFO:
+      case urcl::primary_interface::ReportLevel::DEVL_WARNING:
+      case urcl::primary_interface::ReportLevel::DEVL_VIOLATION:
+      case urcl::primary_interface::ReportLevel::DEVL_FAULT:
+        URCL_LOG_DEBUG(logContents.c_str());
+        break;
+      case urcl::primary_interface::ReportLevel::INFO:
+        URCL_LOG_INFO(logContents.c_str());
+        break;
+      case urcl::primary_interface::ReportLevel::WARNING:
+        URCL_LOG_WARN(logContents.c_str());
+        break;
+      default:
+        // urcl::primary_interface::ReportLevel::VIOLATION:
+        // urcl::primary_interface::ReportLevel::FAULT:
+        URCL_LOG_ERROR(logContents.c_str());
+        break;
+    }
+
+    if (error_code_message_callback_ != nullptr)
+    {
+      error_code_message_callback_(code);
+    }
+    return true;
+  }
+
+   /*!
+   * \brief Set callback function which will be triggered whenever error code messages are received
+   *
+   * \param callback_function Function handling the event information. The error code message received is passed to the
+   * function.
+   */
+  void setErrorCodeMessageCallback(std::function<void(ErrorCode&)> callback_function)
+  {
+    error_code_message_callback_ = callback_function;
+  }
+
+private:
+  std::function<void(ErrorCode&)> error_code_message_callback_;
+
+};
+
+}  // namespace primary_interface
+}  // namespace urcl
+
+#endif  // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED

include/ur_client_library/primary/robot_message/error_code_message.h

@@ -1,7 +1,3 @@
-// Below code is copied from a branch on the Universal_Robot_Client_Library
-// with some modification. Link to branch:
-// https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface
-//
 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
 
 // -- BEGIN LICENSE BLOCK ----------------------------------------------