Improve primary client

CMakeLists.txt

@@ -23,11 +23,24 @@ add_library(urcl SHARED
     src/control/script_sender.cpp
     src/control/trajectory_point_interface.cpp
     src/control/script_command_interface.cpp
+    src/primary/primary_client.cpp
     src/primary/primary_package.cpp
+    src/primary/program_state_message.cpp
     src/primary/robot_message.cpp
     src/primary/robot_state.cpp
+    src/primary/program_state_message/global_variables_update_message.cpp
+    src/primary/program_state_message/global_variables_setup_message.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/primary/robot_state/robot_mode_data.cpp
+    src/primary/robot_state/joint_data.cpp
+    src/primary/robot_state/cartesian_info.cpp
+    src/primary/robot_state/force_mode_data.cpp
+    src/primary/robot_state/additional_info.cpp
     src/rtde/control_package_pause.cpp
     src/rtde/control_package_setup_inputs.cpp
     src/rtde/control_package_setup_outputs.cpp

examples/full_driver.cpp

@@ -44,6 +44,8 @@ const std::string SCRIPT_FILE = "resources/external_control.urscript";
 const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
 const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
 const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
+const bool SIMULATED_ROBOT = true;
+bool g_program_running = false;
 
 std::unique_ptr<UrDriver> g_my_driver;
 std::unique_ptr<DashboardClient> g_my_dashboard;
@@ -54,6 +56,7 @@ void handleRobotProgramState(bool program_running)
 {
   // Print the text in green so we see it better
   std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
+  g_program_running = program_running;
 }
 
 int main(int argc, char* argv[])
@@ -109,12 +112,18 @@ int main(int argc, char* argv[])
   std::unique_ptr<ToolCommSetup> tool_comm_setup;
   const bool HEADLESS = true;
   g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
-                                 std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+                                 std::move(tool_comm_setup), CALIBRATION_CHECKSUM, SIMULATED_ROBOT));
 
   // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
-  // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
+  // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main
   // loop.
 
+  // Wait for the program to run on the robot
+  while (!g_program_running)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
   g_my_driver->startRTDECommunication();
 
   double increment = 0.01;
@@ -157,7 +166,7 @@ int main(int argc, char* argv[])
         URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
         return 1;
       }
-      URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString());
+      URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
     }
     else
     {

examples/primary_pipeline.cpp

@@ -27,18 +27,18 @@
 
 #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;
 
 // In a real-world example it would be better to get those values from command line parameters / a better configuration
 // system such as Boost.Program_options
 const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
+const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
 
 int main(int argc, char* argv[])
 {
-  // Set the loglevel to info get print out the DH parameters
   urcl::setLogLevel(urcl::LogLevel::INFO);
 
   // Parse the ip arguments if given
@@ -48,40 +48,41 @@ int main(int argc, char* argv[])
     robot_ip = std::string(argv[1]);
   }
 
-  // Parse how many seconds to run
-  int second_to_run = -1;
-  if (argc > 2)
-  {
-    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);
-
-  // This will parse the primary packages
-  primary_interface::PrimaryParser parser;
-
-  // The producer needs both, the stream and the parser to fully work
-  comm::URProducer<primary_interface::PrimaryPackage> prod(primary_stream, parser);
-  prod.setupProducer();
+  // First of all, we need to create a primary client that connects to the robot
+  primary_interface::PrimaryClient primary_client(robot_ip, CALIBRATION_CHECKSUM);
 
-  // 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>());
+  // Give time to get the client to connect
+  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
-  // 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> pipeline(prod, consumer.get(), "Pipeline", notifier);
-  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.
-  do
+  for (int i = 0; i < 10; ++i)
   {
-    std::this_thread::sleep_for(std::chrono::seconds(second_to_run));
-  } while (second_to_run < 0);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    // Create script program to send through client
+    std::stringstream cmd;
+    cmd.imbue(std::locale::classic());  // Make sure, decimal divider is actually '.'
+    cmd << "sec setup():" << std::endl
+        << " textmsg(\"Command through primary interface complete " << i << "\")" << std::endl
+        << "end";
+    std::string script_code = cmd.str();
+    auto program_with_newline = script_code + '\n';
+    // Send script
+    primary_client.sendScript(program_with_newline);
+
+    try
+    {
+      URCL_LOG_INFO("Cartesian Information:\n%s", primary_client.getCartesianInfo()->toString().c_str());
+      URCL_LOG_INFO("Calibration Hash:\n%s", primary_client.getCalibrationChecker()->getData()->toHash().c_str());
+      URCL_LOG_INFO("Build Date:\n%s", primary_client.getVersionMessage()->build_date_.c_str());
+      std::stringstream os;
+      os << primary_client.getJointData()->q_actual_;
+      URCL_LOG_INFO("Joint Angles:\n%s", os.str().c_str());
+      // getGlobalVariablesSetupMessage() will throw an exception if a program on the robot has not been started
+      URCL_LOG_INFO("Global Variables:\n%s", primary_client.getGlobalVariablesSetupMessage()->variable_names_.c_str());
+    }
+    catch (const UrException& e)
+    {
+      URCL_LOG_WARN(e.what());
+    }
+  }
   return 0;
 }

include/ur_client_library/comm/bin_parser.h

@@ -207,6 +207,19 @@ class BinParser
     }
   }
 
+  /*!
+   * \brief Parses the next bytes as a vector of 6 floats.
+   *
+   * \param val Reference to write the parsed value to
+   */
+  void parse(vector6f_t& val)
+  {
+    for (size_t i = 0; i < val.size(); ++i)
+    {
+      parse(val[i]);
+    }
+  }
+
   /*!
    * \brief Parses the next bytes as a vector of 6 32 bit integers.
    *

include/ur_client_library/comm/pipeline.h

@@ -303,6 +303,8 @@ class Pipeline
       return;
 
     URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
+    URCL_LOG_DEBUG("Producer thread joinable?! <%i>", pThread_.joinable());
+    URCL_LOG_DEBUG("Consumer thread joinable?! <%i>", cThread_.joinable());
 
     running_ = false;
 
@@ -315,6 +317,7 @@ class Pipeline
     {
       cThread_.join();
     }
+    URCL_LOG_DEBUG("Joined pipeline threads");
     notifier_.stopped(name_);
   }
 
@@ -352,7 +355,7 @@ class Pipeline
 
   void runProducer()
   {
-    URCL_LOG_DEBUG("Starting up producer");
+    URCL_LOG_DEBUG("Starting up producer <%s>", name_.c_str());
     std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
     bool has_realtime;
     realtime_file >> has_realtime;

include/ur_client_library/comm/producer.h

@@ -42,7 +42,7 @@ class URProducer : public IProducer<T>
 private:
   URStream<T>& stream_;
   Parser<T>& parser_;
-  std::chrono::seconds timeout_;
+  std::chrono::milliseconds timeout_;
 
   bool running_;
 
@@ -57,6 +57,8 @@ class URProducer : public IProducer<T>
   {
   }
 
+  virtual ~URProducer() = default;
+
   /*!
    * \brief Triggers the stream to connect to the robot.
    */
@@ -83,6 +85,7 @@ class URProducer : public IProducer<T>
    */
   void stopProducer() override
   {
+    URCL_LOG_DEBUG("Stopping producer");
     running_ = false;
   }
 
@@ -105,24 +108,24 @@ class URProducer : public IProducer<T>
     // 4KB should be enough to hold any packet received from UR
     uint8_t buf[4096];
     size_t read = 0;
-    // expoential backoff reconnects
+    // exponential backoff reconnects
     while (true)
     {
+      if (!running_)
+        return true;
+
       if (stream_.read(buf, sizeof(buf), read))
       {
         // reset sleep amount
-        timeout_ = std::chrono::seconds(1);
+        timeout_ = std::chrono::milliseconds(100);
         BinParser bp(buf, read);
         return parser_.parse(bp, products);
       }
 
-      if (!running_)
-        return true;
-
       if (stream_.closed())
         return false;
 
-      URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
+      URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld milliseconds...", timeout_.count());
       std::this_thread::sleep_for(timeout_);
 
       if (stream_.connect())

include/ur_client_library/primary/abstract_primary_consumer.h

@@ -26,13 +26,24 @@
  */
 //----------------------------------------------------------------------
 
-#ifndef UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED
-#define UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED
+#ifndef UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED
+#define UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED
 
 #include "ur_client_library/log.h"
 #include "ur_client_library/comm/pipeline.h"
+#include "ur_client_library/primary/robot_message/error_code_message.h"
+#include "ur_client_library/primary/robot_message/key_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_message/version_message.h"
+#include "ur_client_library/primary/robot_state/robot_mode_data.h"
+#include "ur_client_library/primary/robot_state/joint_data.h"
+#include "ur_client_library/primary/robot_state/cartesian_info.h"
 #include "ur_client_library/primary/robot_state/kinematics_info.h"
+#include "ur_client_library/primary/robot_state/force_mode_data.h"
+#include "ur_client_library/primary/robot_state/additional_info.h"
+#include "ur_client_library/primary/program_state_message/global_variables_update_message.h"
+#include "ur_client_library/primary/program_state_message/global_variables_setup_message.h"
 
 namespace urcl
 {
@@ -51,7 +62,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,13 +82,25 @@ 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(RobotModeData& pkg) = 0;
+  virtual bool consume(JointData& pkg) = 0;
+  virtual bool consume(CartesianInfo& pkg) = 0;
   virtual bool consume(KinematicsInfo& pkg) = 0;
+  virtual bool consume(ForceModeData& pkg) = 0;
+  virtual bool consume(AdditionalInfo& pkg) = 0;
+  virtual bool consume(ProgramStateMessage& pkg) = 0;
+  virtual bool consume(GlobalVariablesUpdateMessage& pkg) = 0;
+  virtual bool consume(GlobalVariablesSetupMessage& pkg) = 0;
 
 private:
   /* data */
 };
 }  // namespace primary_interface
 }  // namespace urcl
 
-#endif  // ifndef UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED
+#endif  // ifndef UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED