This PR improves the primary interface support in the client library

CMakeLists.txt

@@ -25,10 +25,17 @@ add_library(urcl SHARED
     src/control/trajectory_point_interface.cpp
     src/control/script_command_interface.cpp
     src/primary/primary_package.cpp
+    src/primary/program_state_message.cpp
     src/primary/robot_message.cpp
     src/primary/robot_state.cpp
     src/primary/robot_message/version_message.cpp
+    src/primary/robot_message/key_message.cpp
+    src/primary/robot_message/error_code_message.cpp
+    src/primary/robot_message/runtime_exception_message.cpp
+    src/primary/robot_message/text_message.cpp
     src/primary/robot_state/kinematics_info.cpp
+    src/primary/robot_state/robot_mode_data.cpp
+    src/primary/primary_client.cpp
     src/rtde/control_package_pause.cpp
     src/rtde/control_package_setup_inputs.cpp
     src/rtde/control_package_setup_outputs.cpp

examples/primary_pipeline.cpp

@@ -25,10 +25,8 @@
  */
 //----------------------------------------------------------------------
 
-#include <ur_client_library/comm/pipeline.h>
-#include <ur_client_library/comm/producer.h>
-#include <ur_client_library/comm/shell_consumer.h>
-#include <ur_client_library/primary/primary_parser.h>
+#include <ur_client_library/primary/primary_client.h>
+#include <ur_client_library/ur/dashboard_client.h>
 
 using namespace urcl;
 
@@ -55,30 +53,67 @@ int main(int argc, char* argv[])
     second_to_run = std::stoi(argv[2]);
   }
 
-  // First of all, we need a stream that connects to the robot
-  comm::URStream<primary_interface::PrimaryPackage> primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT);
+  // The robot should be running in order to send script code to it
+  // Connect the the robot Dashboard
+  std::unique_ptr<DashboardClient> dashboard_client;
+  dashboard_client.reset(new DashboardClient(robot_ip));
+  if (!dashboard_client->connect())
+  {
+    URCL_LOG_ERROR("Could not connect to dashboard");
+    return 1;
+  }
+
+  // Stop program, if there is one running
+  if (!dashboard_client->commandStop())
+  {
+    URCL_LOG_ERROR("Could not send stop program command");
+    return 1;
+  }
+
+  // Release the brakes
+  if (!dashboard_client->commandBrakeRelease())
+  {
+    URCL_LOG_ERROR("Could not send BrakeRelease command");
+    return 1;
+  }
 
-  // This will parse the primary packages
-  primary_interface::PrimaryParser parser;
+  primary_interface::PrimaryClient primary_client(robot_ip);
 
-  // The producer needs both, the stream and the parser to fully work
-  comm::URProducer<primary_interface::PrimaryPackage> prod(primary_stream, parser);
-  prod.setupProducer();
+  // Check that the calibration checksum matches the one provided from the robot
+  const std::string calibration_check_sum = "";
+  bool check_calibration_result = primary_client.checkCalibration(calibration_check_sum);
+  std::string calibration_check_sum_matches = check_calibration_result ? "true" : "false";
+  URCL_LOG_INFO("calibration check sum matches: %s", calibration_check_sum_matches.c_str());
 
-  // The shell consumer will print the package contents to the shell
-  std::unique_ptr<comm::IConsumer<primary_interface::PrimaryPackage>> consumer;
-  consumer.reset(new comm::ShellConsumer<primary_interface::PrimaryPackage>());
+  // Send a script program to the robot
+  std::stringstream cmd;
+  cmd.imbue(std::locale::classic());  // Make sure, decimal divider is actually '.'
+  cmd << "def test():" << std::endl << "textmsg(\"Hello from script program\")" << std::endl << "end";
 
-  // The notifer will be called at some points during connection setup / loss. This isn't fully
-  // implemented atm.
-  comm::INotifier notifier;
+  if (primary_client.sendScript(cmd.str()))
+  {
+    URCL_LOG_INFO("Script program was successfully sent to the robot");
+  }
+  else
+  {
+    URCL_LOG_ERROR("Script program wasn't send successfully to the robot");
+    return 1;
+  }
 
-  // Now that we have all components, we can create and start the pipeline to run it all.
-  comm::Pipeline<primary_interface::PrimaryPackage> pipeline(prod, consumer.get(), "Pipeline", notifier);
-  pipeline.run();
+  // Send a secondary script program to the robot
+  cmd.str("");
+  cmd << "sec setup():" << std::endl << "textmsg(\"Hello from secondary program\")" << std::endl << "end";
+  if (primary_client.sendSecondaryScript(cmd.str()))
+  {
+    URCL_LOG_INFO("Secondary script program was successfully sent to the robot");
+  }
+  else
+  {
+    URCL_LOG_ERROR("Secondary script program wasn't send successfully to the robot");
+    return 1;
+  }
 
   // Package contents will be printed while not being interrupted
-  // Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed.
   do
   {
     std::this_thread::sleep_for(std::chrono::seconds(second_to_run));

examples/primary_pipeline_calibration.cpp

@@ -16,30 +16,62 @@
 // limitations under the License.
 // -- END LICENSE BLOCK ------------------------------------------------
 
-#include <ur_client_library/comm/pipeline.h>
-#include <ur_client_library/comm/producer.h>
-#include <ur_client_library/comm/shell_consumer.h>
-#include <ur_client_library/primary/primary_parser.h>
+#include <ur_client_library/primary/primary_client.h>
 
 using namespace urcl;
 
-class CalibrationConsumer : public urcl::comm::IConsumer<urcl::primary_interface::PrimaryPackage>
+// Create a primary consumer for logging calibration data
+class CalibrationConsumer : public primary_interface::AbstractPrimaryConsumer
 {
 public:
   CalibrationConsumer() : calibrated_(0), have_received_data(false)
   {
   }
   virtual ~CalibrationConsumer() = default;
 
-  virtual bool consume(std::shared_ptr<urcl::primary_interface::PrimaryPackage> product)
+  // We should consume all primary packages supported, in this example we just ignore the messages
+  virtual bool consume(primary_interface::RobotMessage& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::RobotState& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::ProgramStateMessage& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::VersionMessage& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::ErrorCodeMessage& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::RuntimeExceptionMessage& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::KeyMessage& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::RobotModeData& pkg) override
+  {
+    return true;
+  }
+  virtual bool consume(primary_interface::TextMessage& pkg) override
   {
-    auto kin_info = std::dynamic_pointer_cast<urcl::primary_interface::KinematicsInfo>(product);
-    if (kin_info != nullptr)
-    {
-      URCL_LOG_INFO("%s", product->toString().c_str());
-      calibrated_ = kin_info->calibration_status_;
-      have_received_data = true;
-    }
+    return true;
+  }
+
+  // The kinematics info stores the calibration data
+  virtual bool consume(primary_interface::KinematicsInfo& pkg) override
+  {
+    calibrated_ = pkg.calibration_status_;
+    have_received_data = true;
     return true;
   }
 
@@ -63,9 +95,11 @@ class CalibrationConsumer : public urcl::comm::IConsumer<urcl::primary_interface
 // system such as Boost.Program_options
 const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
 
+// The purpose of this example is to show how to add a primary consumer to the primary client. This consumer is used to
+// check that the robot is calibrated.
 int main(int argc, char* argv[])
 {
-  // Set the loglevel to info get print out the DH parameters
+  // Set the loglevel to info to print out the DH parameters
   urcl::setLogLevel(urcl::LogLevel::INFO);
 
   // Parse the ip arguments if given
@@ -75,35 +109,24 @@ int main(int argc, char* argv[])
     robot_ip = std::string(argv[1]);
   }
 
-  // First of all, we need a stream that connects to the robot
-  comm::URStream<primary_interface::PrimaryPackage> primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT);
+  // Create a primary client
+  primary_interface::PrimaryClient primary_client(robot_ip);
 
-  // This will parse the primary packages
-  primary_interface::PrimaryParser parser;
+  std::shared_ptr<CalibrationConsumer> calibration_consumer;
+  calibration_consumer.reset(new CalibrationConsumer());
 
-  // The producer needs both, the stream and the parser to fully work
-  comm::URProducer<primary_interface::PrimaryPackage> prod(primary_stream, parser);
-  prod.setupProducer();
+  // Add the calibration consumer to the primary consumers
+  primary_client.addPrimaryConsumer(calibration_consumer);
 
-  // The calibration consumer will print the package contents to the shell
-  CalibrationConsumer calib_consumer;
+  // Kinematics info is only send when you connect to the primary interface, so triggering a reconnect
+  primary_client.reconnect();
 
-  // The notifer will be called at some points during connection setup / loss. This isn't fully
-  // implemented atm.
-  comm::INotifier notifier;
-
-  // Now that we have all components, we can create and start the pipeline to run it all.
-  comm::Pipeline<primary_interface::PrimaryPackage> calib_pipeline(prod, &calib_consumer, "Pipeline", notifier);
-  calib_pipeline.run();
-
-  // Package contents will be printed while not being interrupted
-  // Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed.
-  while (!calib_consumer.calibrationStatusReceived())
+  while (!calibration_consumer->calibrationStatusReceived())
   {
     std::this_thread::sleep_for(std::chrono::seconds(1));
   }
 
-  if (calib_consumer.isCalibrated())
+  if (calibration_consumer->isCalibrated())
   {
     printf("The robot on IP: %s is calibrated\n", robot_ip.c_str());
   }
@@ -113,5 +136,8 @@ int main(int argc, char* argv[])
     printf("Remeber to turn on the robot to get calibration stored on the robot!\n");
   }
 
+  // We can remove the consumer again once we are done using it
+  primary_client.removePrimaryConsumer(calibration_consumer);
+
   return 0;
 }

include/ur_client_library/comm/pipeline.h

@@ -29,6 +29,8 @@
 #include <thread>
 #include <vector>
 #include <fstream>
+#include <mutex>
+#include <algorithm>
 
 namespace urcl
 {
@@ -89,18 +91,46 @@ 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.
    */
@@ -151,6 +181,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_)
     {
@@ -159,6 +190,9 @@ class MultiConsumer : public IConsumer<T>
     }
     return res;
   }
+
+private:
+  std::mutex consumer_list;
 };
 
 /*!

include/ur_client_library/primary/abstract_primary_consumer.h

@@ -31,8 +31,14 @@
 
 #include "ur_client_library/log.h"
 #include "ur_client_library/comm/pipeline.h"
+#include "ur_client_library/primary/program_state_message.h"
+#include "ur_client_library/primary/robot_message/key_message.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_message/runtime_exception_message.h"
+#include "ur_client_library/primary/robot_message/text_message.h"
 #include "ur_client_library/primary/robot_state/kinematics_info.h"
+#include "ur_client_library/primary/robot_state/robot_mode_data.h"
 
 namespace urcl
 {
@@ -51,7 +57,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).
    *
@@ -71,8 +77,14 @@ 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(ProgramStateMessage& pkg) = 0;
   virtual bool consume(VersionMessage& pkg) = 0;
   virtual bool consume(KinematicsInfo& pkg) = 0;
+  virtual bool consume(ErrorCodeMessage& pkg) = 0;
+  virtual bool consume(RuntimeExceptionMessage& pkg) = 0;
+  virtual bool consume(KeyMessage& pkg) = 0;
+  virtual bool consume(RobotModeData& pkg) = 0;
+  virtual bool consume(TextMessage& pkg) = 0;
 
 private:
   /* data */