Migrate Urscript interface to primary client (#1833)

* Switch to primary client and implement action server
* Documentation of action server
* Update ur_robot_driver/doc/usage/script_code.rst

Author: URJala

ur_robot_driver/CMakeLists.txt

@@ -151,7 +151,10 @@ add_executable(urscript_interface
 )
 target_link_libraries(urscript_interface PUBLIC
   rclcpp::rclcpp
+  rclcpp_action::rclcpp_action
   ${std_msgs_TARGETS}
+  ${ur_msgs_TARGETS}
+  rclcpp::rclcpp
   ur_client_library::urcl
 )
 

ur_robot_driver/doc/usage/script_code.rst

@@ -65,3 +65,51 @@ To prevent interrupting the main program, you can send certain commands as `seco
        textmsg(\"This is a log message\")
 
      end"}'
+
+Scripts with execution monitoring
+---------------------------------
+The action server at ``/urscript_interface/execute_script`` allows executing script programs and getting the information when execution is done and whether there was an error during execution.
+
+The ``SendScript`` action definition can be seen in the `ur_msgs action definitions <https://docs.ros.org/en/rolling/p/ur_msgs/__action_definitions.html/>`_
+
+This action server is a ROS wrapper around the `URCL primary client's <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/architecture/primary_client.rst/>`_
+SendScriptBlocking method, and the meaning of parameters can be seen there. 
+The action will be reported as successful if no errors occur during script execution. See :ref:`script-cancel` for additional info about when the action might be reported as successful.
+If an error is detected, a message explaining what went wrong will be made available in the action result.
+While the action server is waiting for a script to finish execution, it will reject any further action goals, and the ``/urscript_interface/script_command`` topic will also be ignored during this time.
+
+.. note::
+  The previous note about restarting the external control URCap also applies when using the action server.
+
+.. _script-cancel:
+
+Script cancellation
+^^^^^^^^^^^^^^^^^^^
+If script execution is canceled through the ROS2 action interface, the running script will be stopped, and the action will be reported as canceled. If script execution is stopped via the teach pendant or **any** other non-ROS means, the corresponding ROS action will be reported as successful, as the action server can not distinguish between successful programs and programs that were stopped outside of ROS.
+
+Command line example
+^^^^^^^^^^^^^^^^^^^^
+The action server can be called from the command line like this:
+
+.. code-block:: bash
+
+  ros2 action send_goal /urscript_interface/execute_script ur_msgs/action/SendScript '{
+    program: "popup(\"cmd line example\")",
+    script_name: "cmd_line_example",
+    start_timeout: {sec: 1.0, nanosec: 0},
+    fail_on_warnings: true}'
+    
+Parameters
+^^^^^^^^^^
+- ``robot_ip`` (string, **required**)
+
+  The IP address at which the robot is reachable.
+
+- ``retry_on_readonly_interface`` (boolean, default: ``true``)
+
+  When the client is currently connected to a read-only interface (because the robot is not in
+  remote_control mode / was put to local control mode since the client connected) script code will
+  not be accepted by the primary interface.
+  
+  When set to ``true`` the client will attempt to reconnect to the primary interface and send the 
+  script again once. This mechanism is only active when using the action interface.

ur_robot_driver/src/urscript_interface.cpp

@@ -35,53 +35,174 @@
  */
 //----------------------------------------------------------------------
 
-#include <ur_client_library/comm/stream.h>
-#include <ur_client_library/primary/primary_package.h>
+#include <ur_client_library/primary/primary_client.h>
 
-#include <memory>
+#include <chrono>
+#include <thread>
+
+#include <ur_msgs/action/send_script.hpp>
 
 #include <rclcpp/rclcpp.hpp>
+#include "rclcpp_action/rclcpp_action.hpp"
 #include <std_msgs/msg/string.hpp>
 
+using SendScript = ur_msgs::action::SendScript;
+
 class URScriptInterface : public rclcpp::Node
 {
 public:
-  URScriptInterface()
-    : Node("urscript_interface")
-    , m_script_sub(this->create_subscription<std_msgs::msg::String>(
-          "~/script_command", 1, [this](const std_msgs::msg::String::SharedPtr msg) {
-            auto program_with_newline = msg->data + '\n';
-
-            RCLCPP_INFO_STREAM(this->get_logger(), program_with_newline);
-
-            size_t len = program_with_newline.size();
-            const auto* data = reinterpret_cast<const uint8_t*>(program_with_newline.c_str());
-            size_t written;
-
-            if (m_secondary_stream->write(data, len, written)) {
-              URCL_LOG_INFO("Sent program to robot:\n%s", program_with_newline.c_str());
-              return true;
-            }
-            URCL_LOG_ERROR("Could not send program to robot");
-            return false;
-          }))
+  URScriptInterface() : Node("urscript_interface")
   {
     this->declare_parameter("robot_ip", rclcpp::PARAMETER_STRING);
-    m_secondary_stream = std::make_unique<urcl::comm::URStream<urcl::primary_interface::PrimaryPackage>>(
-        this->get_parameter("robot_ip").as_string(), urcl::primary_interface::UR_SECONDARY_PORT);
-    m_secondary_stream->connect();
-
-    auto program_with_newline = std::string("sec urscript_interface_initialization:\ntextmsg(\"urscript_interface "
-                                            "connected\")\nend\n");
-    size_t len = program_with_newline.size();
-    const auto* data = reinterpret_cast<const uint8_t*>(program_with_newline.c_str());
-    size_t written;
-    m_secondary_stream->write(data, len, written);
+    this->declare_parameter("retry_on_readonly_interface", true);
+
+    primary_client_ =
+        std::make_unique<urcl::primary_interface::PrimaryClient>(this->get_parameter("robot_ip").as_string(), notif_);
+
+    primary_client_->start(10, std::chrono::seconds(10));
+
+    script_sub_ = create_subscription<std_msgs::msg::String>(
+        "~/script_command", 1, std::bind(&URScriptInterface::script_callback, this, std::placeholders::_1));
+
+    send_script_server_ = rclcpp_action::create_server<SendScript>(
+        this, "~/execute_script",
+        std::bind(&URScriptInterface::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
+        std::bind(&URScriptInterface::handle_cancel, this, std::placeholders::_1),
+        std::bind(&URScriptInterface::handle_accepted, this, std::placeholders::_1));
+  }
+
+  ~URScriptInterface() override
+  {
+    if (execute_thread_.joinable()) {
+      execute_thread_.join();
+    }
   }
 
 private:
-  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr m_script_sub;
-  std::unique_ptr<urcl::comm::URStream<urcl::primary_interface::PrimaryPackage>> m_secondary_stream;
+  // Use non-blocking interface
+  bool script_callback(const std_msgs::msg::String::SharedPtr msg)
+  {
+    if (primary_client_ == nullptr) {
+      RCLCPP_ERROR(get_logger(), "Primary client not initialized yet");
+      return false;
+    }
+    if (busy) {
+      RCLCPP_ERROR(get_logger(), "Script interface is executing action, ignoring topic");
+      return false;
+    }
+    if (primary_client_->sendScript(msg->data)) {
+      URCL_LOG_INFO("Sent program to robot:\n%s", msg->data.c_str());
+      return true;
+    } else {
+      URCL_LOG_ERROR("Could not send program to robot");
+      return false;
+    }
+  }
+
+  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid,
+                                          std::shared_ptr<const SendScript::Goal> goal)
+  {
+    if (primary_client_ == nullptr) {
+      RCLCPP_ERROR(get_logger(), "Primary client not initialized yet");
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (busy) {
+      RCLCPP_ERROR(get_logger(), "Action server is busy");
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+    busy = true;
+
+    if (execute_thread_.joinable()) {
+      // Should return immediately, as we are not busy
+      execute_thread_.join();
+    }
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  rclcpp_action::CancelResponse
+  handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<SendScript>> goal_handle)
+  {
+    if (goal_handle->is_executing()) {
+      try {
+        primary_client_->commandStop();
+      } catch (const urcl::UrException& exc) {
+        RCLCPP_ERROR(get_logger(), "Caught UR exception while trying to cancel goal:");
+        RCLCPP_ERROR(get_logger(), exc.what());
+      } catch (const std::exception& exc) {
+        RCLCPP_ERROR(get_logger(), "Caught unexpected exception while trying to cancel goal:");
+        RCLCPP_ERROR(get_logger(), exc.what());
+      }
+      return rclcpp_action::CancelResponse::ACCEPT;
+    } else {
+      RCLCPP_ERROR(get_logger(), "Received cancel request for inactive goal, rejecting");
+      return rclcpp_action::CancelResponse::REJECT;
+    }
+  }
+
+  void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<SendScript>> goal_handle)
+  {
+    execute_thread_ = std::thread([this, goal_handle]() { this->execute(goal_handle); });
+  }
+
+  void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<SendScript>> goal_handle)
+  {
+    auto goal = goal_handle->get_goal();
+    auto timeout = goal->start_timeout;
+    auto chrono_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::seconds(timeout.sec) + std::chrono::nanoseconds(timeout.nanosec));
+    // A ROS duration can be negative, would not make sense here
+    if (chrono_timeout < std::chrono::milliseconds(0)) {
+      chrono_timeout = std::chrono::milliseconds(0);
+    }
+
+    try {
+      primary_client_->sendScriptBlocking(goal->program, goal->script_name, chrono_timeout, goal->fail_on_warnings,
+                                          this->get_parameter("retry_on_readonly_interface").as_bool());
+    }
+
+    catch (const urcl::UrException& exc) {
+      RCLCPP_ERROR(get_logger(), "Script did not execute successfully. Error message:");
+      RCLCPP_ERROR(get_logger(), exc.what());
+      auto res = std::make_shared<SendScript::Result>();
+      res->success = false;
+      res->message = exc.what();
+      goal_handle->abort(res);
+      busy = false;
+      return;
+    }
+
+    catch (const std::exception& exc) {
+      RCLCPP_ERROR(get_logger(), "Unexpected exception caught during script execution:");
+      RCLCPP_ERROR(get_logger(), exc.what());
+      auto res = std::make_shared<SendScript::Result>();
+      res->success = false;
+      res->message = exc.what();
+      goal_handle->abort(res);
+      busy = false;
+      return;
+    }
+
+    auto res = std::make_shared<SendScript::Result>();
+    if (goal_handle->is_canceling()) {
+      res->success = false;
+      res->message = "Script execution canceled";
+      goal_handle->canceled(res);
+    } else {
+      res->success = true;
+      res->message = "Script executed successfully";
+      goal_handle->succeed(res);
+    }
+    busy = false;
+    return;
+  }
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr script_sub_;
+  rclcpp_action::Server<SendScript>::SharedPtr send_script_server_;
+  std::unique_ptr<urcl::primary_interface::PrimaryClient> primary_client_;
+  urcl::comm::INotifier notif_;
+  std::atomic<bool> busy = false;
+  std::thread execute_thread_;
 };
 
 int main(int argc, char** argv)

ur_robot_driver/test/urscript_interface.py

@@ -34,8 +34,12 @@
 import pytest
 import rclpy
 import rclpy.node
+from rclpy.action import ActionClient
+from builtin_interfaces.msg import Duration
 from std_msgs.msg import String as StringMsg
 from ur_msgs.msg import IOStates
+from ur_msgs.action import SendScript
+from action_msgs.srv import CancelGoal_Response
 
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
@@ -75,6 +79,8 @@ def init_robot(self):
         self.urscript_pub = self.node.create_publisher(
             StringMsg, "/urscript_interface/script_command", 1
         )
+        self.client = ActionClient(self.node, SendScript, "/urscript_interface/execute_script")
+        self.client.wait_for_server()
 
     def setUp(self):
         self._dashboard_interface.start_robot()
@@ -141,3 +147,86 @@ def check_pin_states(self, pins, states):
                     pin_states[i] = self.io_msg.digital_out_states[pin_id].state
         self.assertIsNotNone(self.io_msg, "Did not receive an IO state in requested time.")
         self.assertEqual(pin_states, states)
+
+    def test_send_script_action(self):
+        goal_msg = SendScript.Goal()
+        goal_msg.program = 'textmsg("hello")'
+        goal_msg.script_name = "test_send_script_action"
+        goal_msg.start_timeout = Duration(sec=10, nanosec=0)
+        goal_msg.fail_on_warnings = False
+
+        send_goal_future = self.client.send_goal_async(goal_msg)
+        rclpy.spin_until_future_complete(self.node, send_goal_future, timeout_sec=15)
+        self.assertIsNotNone(send_goal_future.result(), "Failed to send SendScript goal")
+
+        goal_handle = send_goal_future.result()
+        self.assertTrue(goal_handle.accepted, "SendScript goal was rejected")
+
+        result_future = goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self.node, result_future, timeout_sec=30)
+        self.assertIsNotNone(result_future.result(), "No result from SendScript action")
+
+        result = result_future.result().result
+        self.assertTrue(result.success, f"SendScript action failed: {result.message}")
+
+    def test_reject_goals_when_busy(self):
+        goal_msg = SendScript.Goal()
+        goal_msg.program = "sleep(5.)"
+        goal_msg.script_name = "test_reject_goals"
+        goal_msg.start_timeout = Duration(sec=1, nanosec=0)
+        goal_msg.fail_on_warnings = False
+
+        send_goal_future = self.client.send_goal_async(goal_msg)
+        rclpy.spin_until_future_complete(self.node, send_goal_future, timeout_sec=15)
+        self.assertIsNotNone(send_goal_future.result(), "Failed to send SendScript goal")
+
+        goal_handle = send_goal_future.result()
+        self.assertTrue(goal_handle.accepted, "SendScript goal was rejected")
+        time.sleep(1)
+
+        goal_msg.program = 'textmsg("Should be rejected")'
+        send_goal_future = self.client.send_goal_async(goal_msg)
+        rclpy.spin_until_future_complete(self.node, send_goal_future, timeout_sec=15)
+        self.assertIsNotNone(send_goal_future.result(), "Failed to send SendScript goal")
+
+        goal_handle_reject = send_goal_future.result()
+        self.assertFalse(goal_handle_reject.accepted)
+
+        result_future = goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self.node, result_future, timeout_sec=30)
+        self.assertIsNotNone(result_future.result(), "No result from SendScript action")
+
+        result = result_future.result().result
+        self.assertTrue(result.success, f"SendScript action failed: {result.message}")
+
+    def test_cancel_goal(self):
+        goal_msg = SendScript.Goal()
+        goal_msg.program = "sleep(5.)"
+        goal_msg.script_name = "test_reject_cancel"
+        goal_msg.start_timeout = Duration(sec=1, nanosec=0)
+        goal_msg.fail_on_warnings = False
+
+        send_goal_future = self.client.send_goal_async(goal_msg)
+        rclpy.spin_until_future_complete(self.node, send_goal_future, timeout_sec=15)
+        self.assertIsNotNone(send_goal_future.result(), "Failed to send SendScript goal")
+
+        goal_handle = send_goal_future.result()
+        self.assertTrue(goal_handle.accepted, "SendScript goal was rejected")
+        time.sleep(1)
+
+        cancel_future = goal_handle.cancel_goal_async()
+        rclpy.spin_until_future_complete(self.node, cancel_future)
+        self.assertEqual(cancel_future.result().return_code, CancelGoal_Response.ERROR_NONE)
+
+        result_future = goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self.node, result_future, timeout_sec=30)
+        self.assertIsNotNone(result_future.result(), "No result from SendScript action")
+
+        result = result_future.result().result
+        self.assertFalse(result.success, f"SendScript action failed: {result.message}")
+
+    def test_ready_after_cancel(self):
+        # send goal and cancel
+        self.test_cancel_goal()
+        # Test if server is ready again
+        self.test_send_script_action()