Improve primary interface

ur_robot_driver/CMakeLists.txt

@@ -83,15 +83,15 @@ include_directories(
 add_library(ur_robot_driver
     src/comm/tcp_socket.cpp
     src/comm/server.cpp
-    #src/ros/service_stopper.cpp
-    #src/ur/commander.cpp
-    #src/ur/master_board.cpp
-    #src/ur/messages.cpp
-    #src/ur/robot_mode.cpp
+    src/primary/primary_client.cpp
     src/primary/primary_package.cpp
     src/primary/robot_message.cpp
     src/primary/robot_state.cpp
+    src/primary/robot_message/error_code_message.cpp
+    src/primary/robot_message/runtime_exception_message.cpp
     src/primary/robot_message/version_message.cpp
+    src/primary/robot_message/text_message.cpp
+    src/primary/robot_message/key_message.cpp
     src/primary/robot_state/kinematics_info.cpp
     src/rtde/control_package_pause.cpp
     src/rtde/control_package_setup_inputs.cpp

ur_robot_driver/include/ur_robot_driver/comm/producer.h

@@ -57,6 +57,8 @@ class URProducer : public IProducer<T>
   {
   }
 
+  virtual ~URProducer() = default;
+
   /*!
    * \brief Triggers the stream to connect to the robot.
    */

ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h

@@ -30,6 +30,10 @@
 
 #include "ur_robot_driver/log.h"
 #include "ur_robot_driver/comm/pipeline.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"
+#include "ur_robot_driver/primary/robot_message/text_message.h"
 #include "ur_robot_driver/primary/robot_message/version_message.h"
 #include "ur_robot_driver/primary/robot_state/kinematics_info.h"
 
@@ -70,6 +74,10 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   // To be implemented in specific consumers
   virtual bool consume(RobotMessage& pkg) = 0;
   virtual bool consume(RobotState& pkg) = 0;
+  virtual bool consume(ErrorCodeMessage& pkg) = 0;
+  virtual bool consume(KeyMessage& pkg) = 0;
+  virtual bool consume(RuntimeExceptionMessage& pkg) = 0;
+  virtual bool consume(TextMessage& pkg) = 0;
   virtual bool consume(VersionMessage& pkg) = 0;
   virtual bool consume(KinematicsInfo& pkg) = 0;
 

ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h

@@ -0,0 +1,60 @@
+// 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_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED
+#define UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED
+
+#include <ur_robot_driver/log.h>
+#include <ur_robot_driver/primary/primary_package_handler.h>
+#include <ur_robot_driver/primary/robot_message/key_message.h>
+
+namespace ur_driver
+{
+namespace primary_interface
+{
+class KeyMessageHandler : public IPrimaryPackageHandler<KeyMessage>
+{
+public:
+  KeyMessageHandler() = default;
+  virtual ~KeyMessageHandler() = default;
+
+  /*!
+   * \brief Actual worker function
+   *
+   * \param pkg package that should be handled
+   */
+  virtual void handle(KeyMessage& pkg) override
+  {
+    LOG_INFO("---KeyMessage---\n%s", pkg.toString().c_str());
+  }
+
+private:
+  /* data */
+};
+}  // namespace primary_interface
+}  // namespace ur_driver
+#endif  // ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED

ur_robot_driver/include/ur_robot_driver/primary/primary_client.h

@@ -0,0 +1,79 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Text 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_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED
+#define UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED
+
+#include <ur_robot_driver/primary/primary_parser.h>
+#include <ur_robot_driver/comm/producer.h>
+#include <ur_robot_driver/comm/stream.h>
+#include <ur_robot_driver/comm/pipeline.h>
+#include <ur_robot_driver/ur/calibration_checker.h>
+#include <ur_robot_driver/primary/primary_consumer.h>
+
+namespace ur_driver
+{
+namespace primary_interface
+{
+class PrimaryClient
+{
+public:
+  PrimaryClient() = delete;
+  PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum);
+  virtual ~PrimaryClient() = default;
+
+  /*!
+   * \brief Sends a custom script program to the robot.
+   *
+   * The given code must be valid according the UR Scripting Manual.
+   *
+   * \param script_code URScript code that shall be executed by the robot.
+   *
+   * \returns true on successful upload, false otherwise.
+   */
+  bool sendScript(const std::string& script_code);
+
+  /*!
+   * \brief Checks if the kinematics information in the used model fits the actual robot.
+   *
+   * \param checksum Hash of the used kinematics information
+   */
+  void checkCalibration(const std::string& checksum);
+
+private:
+  std::string robot_ip_;
+  PrimaryParser parser_;
+  std::unique_ptr<PrimaryConsumer> consumer_;
+  comm::INotifier notifier_;
+  std::unique_ptr<comm::URProducer<PrimaryPackage>> producer_;
+  std::unique_ptr<comm::URStream<PrimaryPackage>> stream_;
+  std::unique_ptr<comm::Pipeline<PrimaryPackage>> pipeline_;
+};
+
+}  // namespace primary_interface
+}  // namespace ur_driver
+
+#endif  // ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED

ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h

@@ -0,0 +1,149 @@
+// 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_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED
+#define UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED
+
+#include "ur_robot_driver/log.h"
+#include "ur_robot_driver/comm/pipeline.h"
+#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"
+#include "ur_robot_driver/primary/robot_message/text_message.h"
+#include "ur_robot_driver/primary/robot_message/version_message.h"
+#include "ur_robot_driver/primary/robot_state/kinematics_info.h"
+
+namespace ur_driver
+{
+namespace primary_interface
+{
+/*!
+ * \brief Primary consumer implementation
+ *
+ * This class implements am AbstractPrimaryConsumer such that it can consume all incoming primary
+ * messages. However, actual work will be done by workers for each specific type.
+ */
+class PrimaryConsumer : public AbstractPrimaryConsumer
+{
+public:
+  PrimaryConsumer()
+  {
+    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;
+
+  virtual bool consume(RobotMessage& msg) override
+  {
+    LOG_INFO("---RobotMessage:---\n%s", msg.toString().c_str());
+    return true;
+  }
+  virtual bool consume(RobotState& msg) override
+  {
+    // LOG_INFO("---RobotState:---\n%s", msg.toString().c_str());
+    return true;
+  }
+  virtual bool consume(RuntimeExceptionMessage& msg) override
+  {
+    LOG_INFO("---RuntimeExceptionMessage---\n%s", msg.toString().c_str());
+    return true;
+  }
+  virtual bool consume(TextMessage& msg) override
+  {
+    LOG_INFO("---TextMessage---\n%s", msg.toString().c_str());
+    return true;
+  }
+  virtual bool consume(VersionMessage& msg) override
+  {
+    LOG_INFO("---VersionMessage---\n%s", msg.toString().c_str());
+    return true;
+  }
+
+  /*!
+   * \brief Handle a KinematicsInfo
+   *
+   * \returns True if there's a handler for this message type registered. False otherwise.
+   */
+  virtual bool consume(KinematicsInfo& pkg) override
+  {
+    if (kinematics_info_message_worker_ != nullptr)
+    {
+      kinematics_info_message_worker_->handle(pkg);
+      return true;
+    }
+    return false;
+  }
+
+  /*!
+   * \brief Handle a KeyMessage
+   *
+   * \returns True if there's a handler for this message type registered. False otherwise.
+   */
+  virtual bool consume(KeyMessage& pkg) override
+  {
+    if (key_message_worker_ != nullptr)
+    {
+      key_message_worker_->handle(pkg);
+      return true;
+    }
+    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
+}  // namespace ur_driver
+
+#endif  // ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED