Migrate Urscript interface to primary client (backport #1833) (#1865)

mergify[bot] · web-flow · commit bb6d99e865f4 · 2026-07-10T13:03:18.000+02:00
* Switch to primary client and implement action server
* Documentation of action server
* Update ur_robot_driver/doc/usage/script_code.rst
diff --git a/ur_robot_driver/src/urscript_interface.cpp b/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)
diff --git a/ur_robot_driver/test/urscript_interface.py b/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
 
 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()
