Urscript interface (#721)

* Add a urscript interface node

* Add urscript_interface to standard launchfile

* Added documentation for urscript_interface

* Add a notice about incorrect script code

* Add test for urscript interface

* Move tests to one single tests

This should avoid that different tests run in parallel

* Wait for IO controller before checking IOs

* Write an initial textmessage when connecting the urscript_interface

* Wait for controller_manager services longer

* Make sure we have a clean robot state without any program running once we enter our test

similar to how we did it on the robot_driver test

* Remove unneeded Destructor definition

ur_robot_driver/CMakeLists.txt

@@ -103,8 +103,13 @@ add_executable(controller_stopper_node
 )
 ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+add_executable(urscript_interface
+  src/urscript_interface.cpp
+)
+ament_target_dependencies(urscript_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 install(
-  TARGETS dashboard_client ur_ros2_control_node controller_stopper_node
+  TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -182,5 +187,9 @@ if(BUILD_TESTING)
       TIMEOUT
         500
     )
+    add_launch_test(test/urscript_interface.py
+      TIMEOUT
+        500
+    )
   endif()
 endif()

ur_robot_driver/doc/usage.rst

@@ -214,3 +214,61 @@ are a couple of things to know:
   additional tool configured on the Teach pendant (TP), this should be equivalent to ``tool0`` given that
   the URDF uses the specific robot's :ref:`calibration <calibration_extraction>`. If a tool is
   configured on the TP, then the additional transformation will show in ``base`` -> ``tool0``.
+
+Custom URScript commands
+------------------------
+
+The driver's package contains a ``urscript_interface`` node that allows sending URScript snippets
+directly to the robot. It gets started in the driver's launchfiles by default. To use it, simply
+publish a message to its interface:
+
+.. code-block:: bash
+
+  # simple popup
+  ros2 topic pub /urscript_interface/script_command std_msgs/msg/String '{data: popup("hello")}' --once
+
+Be aware, that running a program on this interface (meaning publishing script code to that interface) stops any running program on the robot.
+Thus, the motion-interpreting program that is started by the driver gets stopped and has to be
+restarted again. Depending whether you use headless mode or not, you'll have to call the
+``resend_program`` service or press the ``play`` button on the teach panel to start the
+external_control program again.
+
+.. note::
+  Currently, there is no feedback on the code's correctness. If the code sent to the
+  robot is incorrect, it will silently not get executed. Make sure that you send valid URScript code!
+
+Multi-line programs
+^^^^^^^^^^^^^^^^^^^
+
+When you want to define multi-line programs, make sure to check that newlines are correctly
+interpreted from your message. For this purpose the driver prints the program as it is being sent to
+the robot. When sending a multi-line program from the command line, you can use an empty line
+between each statement:
+
+.. code-block:: bash
+
+   ros2 topic pub --once /urscript_interface/script_command std_msgs/msg/String '{data:
+   "def my_prog():
+
+     set_digital_out(1, True)
+
+     movej(p[0.2, 0.3, 0.8, 0, 0, 3.14], a=1.2, v=0.25, r=0)
+
+     textmsg(\"motion finished\")
+
+   end"}'
+
+Non-interrupting programs
+^^^^^^^^^^^^^^^^^^^^^^^^^
+
+To prevent interrupting the main program, you can send certain commands as `secondary programs
+<https://www.universal-robots.com/articles/ur/programming/secondary-program/>`_.
+
+.. code-block:: bash
+
+   ros2 topic pub --once /urscript_interface/script_command std_msgs/msg/String '{data:
+     "sec my_prog():
+
+       textmsg(\"This is a log message\")
+
+     end"}'

ur_robot_driver/launch/ur_control.launch.py

@@ -242,6 +242,13 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
+    urscript_interface = Node(
+        package="ur_robot_driver",
+        executable="urscript_interface",
+        parameters=[{"robot_ip": robot_ip}],
+        output="screen",
+    )
+
     controller_stopper_node = Node(
         package="ur_robot_driver",
         executable="controller_stopper_node",
@@ -340,6 +347,7 @@ def controller_spawner(name, active=True):
         dashboard_client_node,
         tool_communication_node,
         controller_stopper_node,
+        urscript_interface,
         robot_state_publisher_node,
         rviz_node,
         initial_joint_controller_spawner_stopped,

ur_robot_driver/src/urscript_interface.cpp

@@ -0,0 +1,92 @@
+// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Exner [email protected]
+ * \date    2023-06-20
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_client_library/comm/stream.h>
+#include <ur_client_library/primary/primary_package.h>
+
+#include <memory>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+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;
+          }))
+  {
+    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("textmsg(\"urscript_interface connected\")\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);
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr m_script_sub;
+  std::unique_ptr<urcl::comm::URStream<urcl::primary_interface::PrimaryPackage>> m_secondary_stream;
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_unique<URScriptInterface>());
+  rclcpp::shutdown();
+  return 0;
+}