Primary client script execution feedback (UniversalRobots#484)

Add a blocking function to send script code to the primary client. With this, it is possible to monitor script execution on the robot, know when it's done and if execution was successful.

Author: URJala

CMakeLists.txt

@@ -87,6 +87,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. Note: protective stops are reported as warnings by the robot.
+| 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.

doc/examples.rst

@@ -26,6 +26,7 @@ may be running forever until manually stopped.
    examples/external_fts_through_rtde
    examples/script_command_interface
    examples/script_sender
+   examples/send_script
    examples/spline_example
    examples/tool_contact_example
    examples/direct_torque_control

doc/examples/send_script.rst

@@ -0,0 +1,99 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/send_script.rst
+
+.. _send_script_example:
+
+Send script example
+===================
+
+This example shows how to send arbitrary URScript code to the robot using the
+:ref:`primary_client`. It demonstrates both the blocking variant (``sendScriptBlocking``), which
+waits for execution feedback, and the non-blocking variant (``sendScript``), which only confirms
+that the script has been forwarded to the robot.
+
+The full source code can be found in `send_script.cpp <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/send_script.cpp>`_.
+
+Setting up the primary client
+-----------------------------
+
+The example connects to the robot's primary interface by creating a ``PrimaryClient``. After
+starting the client, the robot's brakes are released so that motion scripts can actually run, and
+the safety state is checked before any script is sent:
+
+.. literalinclude:: ../../examples/send_script.cpp
+   :language: c++
+   :caption: examples/send_script.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: auto notif = comm::INotifier();
+   :end-at: }
+
+Sending scripts with execution feedback
+---------------------------------------
+
+The ``sendScriptBlocking`` function uploads URScript code to the robot and waits until the robot
+reports the result of the execution. The given code can be a fully defined script (with its own
+``def ... end`` block) or a snippet that will automatically be wrapped into a function on the
+client side. Comments and whitespace-only lines are stripped before the script is sent.
+
+.. literalinclude:: ../../examples/send_script.cpp
+   :language: c++
+   :caption: examples/send_script.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: const std::string fully_defined_script
+   :end-at: client.sendScriptBlocking(fully_defined_script)
+
+If you don't provide a function definition, the library wraps the snippet in one for you. You can
+optionally pass a ``script_name`` (used in log messages on both the client and the robot) and a
+``start_timeout`` that limits how long the call waits for the robot to confirm that the script has
+started. A timeout of ``0`` means "wait indefinitely":
+
+.. literalinclude:: ../../examples/send_script.cpp
+   :language: c++
+   :caption: examples/send_script.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: client.sendScriptBlocking(R"(textmsg("Successful program execution"))");
+   :end-at: client.sendScriptBlocking(R"(textmsg("hello"))", "cool_function_name", std::chrono::milliseconds(0));
+
+Secondary programs can also be uploaded through ``sendScriptBlocking``. Since the robot does not
+report execution feedback for secondary programs, the call returns as soon as the script has been
+accepted. Note that secondary programs must be *fully defined* by the user (``sec ... end``):
+
+.. literalinclude:: ../../examples/send_script.cpp
+   :language: c++
+   :caption: examples/send_script.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: std::string secondary_script
+   :end-at: client.sendScriptBlocking(secondary_script);
+
+Reporting bad script code
+-------------------------
+
+When a script contains errors (e.g. a typo or an undefined symbol), ``sendScriptBlocking`` will
+report this back to the caller. The example sends a script that uses an undefined variable
+``current_pos`` instead of ``current_pose``, and logs the result:
+
+.. literalinclude:: ../../examples/send_script.cpp
+   :language: c++
+   :caption: examples/send_script.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: const std::string bad_script_code
+   :end-before: // We can also send script code without any checks
+
+Sending scripts without feedback
+--------------------------------
+
+For situations where execution feedback is not needed, ``sendScript`` can be
+used. It returns ``true`` as soon as the script has been transferred to the robot. The library
+performs no further checks, so faulty script code will *not* be reported back here:
+
+.. literalinclude:: ../../examples/send_script.cpp
+   :language: c++
+   :caption: examples/send_script.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // We can also send script code without any checks
+   :end-at: }

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
+  send_script.cpp)
+target_link_libraries(send_script 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.cpp

@@ -0,0 +1,96 @@
+#include <ur_client_library/primary/primary_client.h>
+#include <chrono>
+
+using namespace urcl;
+
+std::string g_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 = g_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);
+
+  // --------------- INITIALIZATION END -------------------
+
+  // Make sure the robot is running
+  client.commandBrakeRelease();
+
+  if (!client.safetyModeAllowsExecution())
+  {
+    URCL_LOG_ERROR("Robot is not in a safety state where script execution is possible. Exiting.");
+    return 1;
+  }
+
+  // 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,-1.2,1.2,-0.1,1.57,0])
+  sleep(0.1)
+  current_pose = get_target_tcp_pose()
+  relative_move = p[0,-0.1,0,0,0,0]
+  movel(pose_trans(current_pose, relative_move), t=1)
+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);
+
+  // Sending wrong script code will result in a clear error
+  const std::string bad_script_code = R"""(
+def bad_code():
+  current_pose = get_target_tcp_pose()
+  movel(current_pos) # note pose vs pos
+end)""";
+  URCL_LOG_INFO("Sending bad script code...");
+  bool success = client.sendScriptBlocking(bad_script_code);
+  {
+    std::stringstream ss;
+    ss << "Execution of bad code successful? " << std::boolalpha << success;
+    URCL_LOG_INFO("%s", ss.str().c_str());
+  }
+
+  // We can also send script code without any checks
+  URCL_LOG_INFO("Executing motion without feedback");
+  client.sendScript("movej([0.1,-0.9,0.9,0,0,0])");
+  // But we won't know when that is done or even if our code was correct.
+  // E.g. sending the bad script here will not give us any information
+  // The return value will only tell us that the script code has been sent to the robot.
+  URCL_LOG_INFO("Sending bad script code without feedback...");
+  success = client.sendScript(bad_script_code);
+  {
+    std::stringstream ss;
+    ss << "Bad code sent to robot successfully? " << std::boolalpha << success;
+    URCL_LOG_INFO("%s", ss.str().c_str());
+  }
+}

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

@@ -38,6 +38,8 @@
 #include "ur_client_library/primary/robot_state/configuration_data.h"
 #include "ur_client_library/primary/robot_state/masterboard_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
 {
@@ -83,6 +85,8 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual bool consume(ConfigurationData& pkg) = 0;
   virtual bool consume(MasterboardData& pkg) = 0;
   virtual bool consume(SafetyModeMessage& pkg) = 0;
+  virtual bool consume(KeyMessage& pkg) = 0;
+  virtual bool consume(RuntimeExceptionMessage& pkg) = 0;
 
 private:
   /* data */