Primary client script execution feedback

CMakeLists.txt

@@ -85,6 +85,7 @@ else()
       src/ur/dashboard_client.cpp
       src/ur/dashboard_client_implementation_g5.cpp
       src/ur/dashboard_client_implementation_x.cpp
+      src/primary/primary_client.cpp
       PROPERTIES COMPILE_OPTIONS "-Wno-maybe-uninitialized")
   endif()
 

doc/architecture.rst

@@ -10,6 +10,7 @@ well as a couple of standalone modules to directly use subsets of the library's
    :maxdepth: 1
 
    architecture/dashboard_client
+   architecture/primary_client
    architecture/reverse_interface
    architecture/rtde_client
    architecture/script_command_interface

doc/architecture/primary_client.rst

@@ -0,0 +1,42 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/architecture/primary_client.rst
+
+.. _primary_client:
+
+PrimaryClient
+=============
+
+The Primary Client serves as an interface to the robot's `primary interface <https://docs.universal-robots.com/tutorials/communication-protocol-tutorials/primary-secondary-guide.html>`_, present on port 30001.
+The ``PrimaryClient`` class supports, among other things, sending URScript code for execution on the robot through the primary interface. Currently it offers two methods of script execution: ``sendScript`` and ``sendScriptBlocking``.
+
+Script execution without feedback
+---------------------------------
+Method signature:
+
+.. code-block:: c++
+
+   bool sendScript(std::string program);
+
+The ``sendScript`` method will accept valid URScript code, and send it to the robot through the primary interface. This is a non-blocking method, as it will return as soon as the program has been transferred to the robot. It returns true when the program is successfully transferred to the robot, and false otherwise.
+There is no feedback on whether the program is actually executed on the robot.
+
+Script execution with feedback
+------------------------------
+Method signature:
+
+.. code-block:: c++
+
+   bool sendScriptBlocking(
+     std::string program,
+     std::string script_name = "",
+     std::chrono::milliseconds timeout = std::chrono::seconds(1),
+     bool fail_on_warnings = true
+   );
+
+| The ``sendScriptBlocking`` method will also accept valid URScript code, but blocks until the execution result of the given program is available.
+| Prior to transferring the program it will first check that the robot is in a state where it can execute programs, if not it returns false.
+| If the robot is ready, the program is then transferred, and the method will wait for the robot to report that the program has either started, finished or encountered an error.
+| If the program has not started within the given ``timeout``, the method returns false.
+| If the robot encounters an error or runtime exception during program execution the method also returns false.
+| If ``fail_on_warnings`` is true, it will also return false, if the robot reports a warning during program execution.
+| The method only returns true if the program is successfully executed on the robot.
+| This method also accepts secondary programs, but no feedback is available for those, so it will behave similarly to the ``sendScript`` method in those cases, except for the pre-transfer checks.

examples/CMakeLists.txt

@@ -11,6 +11,10 @@ add_executable(primary_pipeline_example
   primary_pipeline.cpp)
 target_link_libraries(primary_pipeline_example ur_client_library::urcl)
 
+add_executable(send_script_blocking
+  send_script_blocking.cpp)
+target_link_libraries(send_script_blocking ur_client_library::urcl)
+
 add_executable(primary_pipeline_calibration_example
   primary_pipeline_calibration.cpp)
 target_link_libraries(primary_pipeline_calibration_example ur_client_library::urcl)

examples/send_script_blocking.cpp

@@ -0,0 +1,68 @@
+#include <ur_client_library/primary/primary_client.h>
+#include <thread>
+#include <chrono>
+
+using namespace urcl;
+
+std::string DEFAULT_ROBOT_IP = "192.168.56.101";
+
+int main(int argc, char* argv[])
+{
+  // Set the loglevel to info to print info logs
+  urcl::setLogLevel(urcl::LogLevel::INFO);
+
+  // Parse the ip arguments if given
+  std::string robot_ip = DEFAULT_ROBOT_IP;
+  if (argc > 1)
+  {
+    robot_ip = std::string(argv[1]);
+  }
+  auto notif = comm::INotifier();
+  auto client = primary_interface::PrimaryClient(robot_ip, notif);
+  client.start(10);
+  std::cout << "Client connected" << std::endl;
+
+  // --------------- INITIALIZATION END -------------------
+
+  // Make sure the robot is running
+  client.commandBrakeRelease();
+
+  if (!client.safetyModeAllowsExecution())
+  {
+    std::cout << "Robot is not in a safety state where script execution is possible. Exiting." << std::endl;
+    return 0;
+  }
+
+  // The sendScriptBlocking accepts script code, and will return true or false,
+  // depending on whether the script is successfully executed
+  const std::string fully_defined_script = R"""(
+# This is a fully defined script, function definition and all
+# All comments in this script will be stripped before sending the script to the robot
+
+# Any whitespace-only lines will also be removed
+def example_fun():
+  movej([0,-0.75,0,0,0,0])
+  sleep(0.1)
+  movel([0,0,-1.5,0,0,0], t=5)
+end)""";
+
+  if (client.sendScriptBlocking(fully_defined_script))
+  {
+    // The function definition can also be omitted
+    // A function name will then be auto generated
+    client.sendScriptBlocking(R"(textmsg("Successful program execution"))");
+  }
+  // A script-function name can also be passed to the method
+  // A timeout can also be given to limit the wait for the passed function to start. If timeout = 0, it will
+  // wait indefinitely.
+  client.sendScriptBlocking(R"(textmsg("hello"))", "cool_function_name", std::chrono::milliseconds(0));
+  // There is no feedback on secondary programs, so it will return successful as soon as the script is sent to the
+  // robot (Behavior is the same the sendScript function, except that robot state is checked before script is sent)
+  // Note that secondary scripts have to be "fully defined" by the user.
+  std::string secondary_script = R"(
+sec sec_script():
+  textmsg("Named secondary program")
+end
+)";
+  client.sendScriptBlocking(secondary_script);
+}

include/ur_client_library/exceptions.h

@@ -319,5 +319,22 @@ class RTDEInputConflictException : public UrException
   std::string key_;
   std::string message_;
 };
+
+class ScriptCodeSyntaxException : public UrException
+{
+public:
+  explicit ScriptCodeSyntaxException() = delete;
+
+  explicit ScriptCodeSyntaxException(const std::string& text) : std::runtime_error(text)
+  {
+  }
+
+  virtual ~ScriptCodeSyntaxException() = default;
+
+  virtual const char* what() const noexcept override
+  {
+    return std::runtime_error::what();
+  }
+};
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED

include/ur_client_library/primary/abstract_primary_consumer.h

@@ -37,6 +37,8 @@
 #include "ur_client_library/primary/robot_state/robot_mode_data.h"
 #include "ur_client_library/primary/robot_state/configuration_data.h"
 #include "ur_client_library/primary/robot_message/safety_mode_message.h"
+#include "ur_client_library/primary/robot_message/key_message.h"
+#include "ur_client_library/primary/robot_message/runtime_exception_message.h"
 
 namespace urcl
 {
@@ -81,6 +83,8 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual bool consume(RobotModeData& pkg) = 0;
   virtual bool consume(ConfigurationData& pkg) = 0;
   virtual bool consume(SafetyModeMessage& pkg) = 0;
+  virtual bool consume(KeyMessage& pkg) = 0;
+  virtual bool consume(RuntimeExceptionMessage& pkg) = 0;
 
 private:
   /* data */

include/ur_client_library/primary/primary_client.h

@@ -47,6 +47,21 @@ namespace urcl
 {
 namespace primary_interface
 {
+
+enum ScriptTypes
+{
+  DEF = 0,
+  SEC = 1,
+};
+
+struct ScriptInfo
+{
+  std::string script_name;
+  std::string script_code;
+  ScriptTypes script_type;
+  ScriptInfo(std::string name, std::string code, ScriptTypes type)
+    : script_name(name), script_code(code), script_type(type) {};
+};
 class PrimaryClient
 {
 public:
@@ -88,6 +103,33 @@ class PrimaryClient
    */
   bool sendScript(const std::string& program);
 
+  /*!
+   * \brief Send a custom script program to the robot, and wait for the execution result.
+   *
+   * The given code must be valid according the UR Scripting Manual. The given script code will be automatically wrapped
+   * in a function definition, if it is not already. Secondary programs can also be passed to this function, but must be
+   * fully defined as a secondary program when calling. Secondary programs create no feedback, so this function will
+   * return true as soon as the program is uploaded successfully to the robot (same as the sendScript function).
+   *
+   * \param program URScript code that shall be executed by the robot.
+   *
+   * \param script_name Name of the script to be executed. This will be ignored, if the given script already defines a
+   * function name. The script name will be used in log messages in both the client library and in the robot logs. If no
+   * name is defined in any way, the script will be given a generic, but unique, name.
+   *
+   * \param timeout Amount of time to allow before the robot must have confirmed that the script has been started. If
+   * timeout is 0, it will be ignored. Default value: 1 second
+   *
+   * \param fail_on_warnings Whether or not the function should report a failure, if the robot reports a warning-level
+   * error during execution. Default true
+   *
+   * \throw urcl::ScriptCodeSyntaxException if the given script code has syntax errors, which are checked here.
+   *
+   * \returns true on successful execution of the script, false otherwise
+   */
+  bool sendScriptBlocking(const std::string& program, std::string script_name = "",
+                          std::chrono::milliseconds timeout = std::chrono::seconds(1), bool fail_on_warnings = true);
+
   bool checkCalibration(const std::string& checksum);
 
   /*!
@@ -272,6 +314,12 @@ class PrimaryClient
    */
   RobotSeries getRobotSeries();
 
+  /* \brief Check if the current safety mode allows for script execution
+   *
+   * Safety modes allowing for execution are: NORMAL, REDUCED, RECOVERY, UNDEFINED_SAFETY_MODE
+   */
+  bool safetyModeAllowsExecution();
+
 private:
   /*!
    * \brief Reconnects the primary stream used to send program to the robot.
@@ -284,6 +332,11 @@ class PrimaryClient
 
   // The function is called whenever an error code message is received
   void errorMessageCallback(ErrorCode& code);
+  void keyMessageCallback(KeyMessage& msg);
+  void runtimeExceptionCallback(RuntimeExceptionMessage& msg);
+
+  ScriptInfo prepare_script(std::string script, std::string script_name);
+  std::vector<std::string> strip_comments_and_whitespace(std::vector<std::string> script_lines);
 
   PrimaryParser parser_;
   std::shared_ptr<PrimaryConsumer> consumer_;
@@ -297,6 +350,12 @@ class PrimaryClient
 
   std::mutex error_code_queue_mutex_;
   std::deque<ErrorCode> error_code_queue_;
+
+  std::mutex key_message_queue_mutex_;
+  std::deque<KeyMessage> key_message_queue_;
+
+  std::mutex runtime_exception_mutex_;
+  std::shared_ptr<primary_interface::RuntimeExceptionMessage> latest_runtime_exception_;
 };
 
 }  // namespace primary_interface

include/ur_client_library/primary/primary_consumer.h

@@ -32,6 +32,7 @@
 #include "ur_client_library/primary/robot_state/robot_mode_data.h"
 #include "ur_client_library/ur/datatypes.h"
 #include "ur_client_library/ur/version_information.h"
+#include "ur_client_library/primary/robot_message/key_message.h"
 
 #include <functional>
 #include <mutex>
@@ -196,6 +197,34 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     error_code_message_callback_ = callback_function;
   }
 
+  virtual bool consume(KeyMessage& pkg) override
+  {
+    if (key_message_callback_ != nullptr)
+    {
+      key_message_callback_(pkg);
+    }
+    return true;
+  }
+
+  void setKeyMessageCallback(std::function<void(KeyMessage&)> callback_function)
+  {
+    key_message_callback_ = callback_function;
+  }
+
+  virtual bool consume(RuntimeExceptionMessage& pkg) override
+  {
+    if (runtime_exception_callback_ != nullptr)
+    {
+      runtime_exception_callback_(pkg);
+    }
+    return true;
+  }
+
+  void setRuntimeExceptionCallback(std::function<void(RuntimeExceptionMessage&)> callback_function)
+  {
+    runtime_exception_callback_ = callback_function;
+  }
+
   /*!
    * \brief Get the kinematics info
    *
@@ -258,6 +287,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
 
 private:
   std::function<void(ErrorCode&)> error_code_message_callback_;
+  std::function<void(KeyMessage&)> key_message_callback_;
+  std::function<void(RuntimeExceptionMessage&)> runtime_exception_callback_;
   std::shared_ptr<KinematicsInfo> kinematics_info_;
   std::mutex robot_mode_mutex_;
   std::shared_ptr<RobotModeData> robot_mode_;