Added handler for ErrorCode messages

Author: fmauch

ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h

@@ -33,6 +33,7 @@
 #include "ur_robot_driver/primary/primary_package_handler.h"
 #include "ur_robot_driver/primary/abstract_primary_consumer.h"
 #include "ur_robot_driver/primary/key_message_handler.h"
+#include "ur_robot_driver/primary/error_code_message_handler.h"
 #include "ur_robot_driver/primary/robot_message/error_code_message.h"
 #include "ur_robot_driver/primary/robot_message/key_message.h"
 #include "ur_robot_driver/primary/robot_message/runtime_exception_message.h"
@@ -57,6 +58,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   {
     LOG_INFO("Constructing primary consumer");
     key_message_worker_.reset(new KeyMessageHandler());
+    error_code_message_worker_.reset(new ErrorCodeMessageHandler());
     LOG_INFO("Constructed primary consumer");
   }
   virtual ~PrimaryConsumer() = default;
@@ -71,11 +73,6 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     // LOG_INFO("---RobotState:---\n%s", msg.toString().c_str());
     return true;
   }
-  virtual bool consume(ErrorCodeMessage& msg) override
-  {
-    LOG_INFO("---ErrorCodeMessage---\n%s", msg.toString().c_str());
-    return true;
-  }
   virtual bool consume(RuntimeExceptionMessage& msg) override
   {
     LOG_INFO("---RuntimeExceptionMessage---\n%s", msg.toString().c_str());
@@ -122,13 +119,28 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return false;
   }
 
+  /*!
+   * \brief Handle a ErrorCodeMessage
+   *
+   * \returns True if there's a handler for this message type registered. False otherwise.
+   */
+  virtual bool consume(ErrorCodeMessage& pkg) override
+  {
+    if (error_code_message_worker_ != nullptr)
+    {
+      error_code_message_worker_->handle(pkg);
+      return true;
+    }
+    return false;
+  }
   void setKinematicsInfoHandler(const std::shared_ptr<IPrimaryPackageHandler<KinematicsInfo>>& handler)
   {
     kinematics_info_message_worker_ = handler;
   }
 
 private:
   std::shared_ptr<IPrimaryPackageHandler<KeyMessage>> key_message_worker_;
+  std::shared_ptr<IPrimaryPackageHandler<ErrorCodeMessage>> error_code_message_worker_;
   std::shared_ptr<IPrimaryPackageHandler<KinematicsInfo>> kinematics_info_message_worker_;
 };
 }  // namespace primary_interface

ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h

@@ -34,6 +34,20 @@ namespace ur_driver
 {
 namespace primary_interface
 {
+enum class ReportLevel : int32_t
+{
+  DEBUG = 0,
+  INFO = 1,
+  WARNING = 2,
+  VIOLATION = 3,
+  FAULT = 4,
+  DEVL_DEBUG = 128,
+  DEVL_INFO = 129,
+  DEVL_WARNING = 130,
+  DEVL_VIOLATION = 131,
+  DEVL_FAULT = 132
+};
+
 /*!
  * \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface.
  */
@@ -81,7 +95,7 @@ class ErrorCodeMessage : public RobotMessage
 
   int32_t message_code_;
   int32_t message_argument_;
-  int32_t report_level_;
+  ReportLevel report_level_;
   uint8_t data_type_;
   uint32_t data_;
   std::string text_;

ur_robot_driver/src/primary/robot_message/error_code_message.cpp

@@ -40,7 +40,9 @@ bool ErrorCodeMessage::parseWith(comm::BinParser& bp)
 {
   bp.parse(message_code_);
   bp.parse(message_argument_);
-  bp.parse(report_level_);
+  int32_t report_level;
+  bp.parse(report_level);
+  report_level_ = static_cast<ReportLevel>(report_level);
   bp.parse(data_type_);
   bp.parse(data_);
   bp.parseRemainder(text_);
@@ -58,7 +60,7 @@ std::string ErrorCodeMessage::toString() const
   std::stringstream ss;
   ss << "Message code: " << message_code_ << std::endl;
   ss << "Message argument: " << message_argument_ << std::endl;
-  ss << "Report level: " << report_level_ << std::endl;
+  ss << "Report level: " << static_cast<int>(report_level_) << std::endl;
   ss << "Datatype: " << static_cast<int>(data_type_) << std::endl;
   ss << "Data: " << data_ << std::endl;
   ss << "Text: " << text_;