first commit

CHANGELOG.rst

@@ -0,0 +1,300 @@
+2.6.0 (2025-03-17)
+------------------
+* Update transformForceTorque to handle whether it is a cb3 or an e-Series robot (backport of `#1287 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1287>`_) (`#1299 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1299>`_)
+* Port robot_state_helper to ROS2 (backport of  `#933 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/933>`_) (`#1286 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1286>`_)
+* Fix crashes on shutting down (`#1270 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1270>`_) (`#1271 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1271>`_)
+* Remove build warnings for Humble (`#1234 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1234>`_)
+* Auto-update pre-commit hooks (backport `#1260 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1260>`_) (`#1261 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1261>`_)
+* Fix doc links (`#1247 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1247>`_)
+* Remove urdf folder (`#1257 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1257>`_)
+* Contributors: Felix Exner, mergify[bot]
+
+2.5.2 (2025-01-21)
+------------------
+* Check quaternions for equal dot_product instead of comparing their components individually (backport `#1238 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1238>`_) (`#1243 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1243>`_)
+* fix sphinx doc link in ur_robot_driver (`#1240 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1240>`_) (`#1242 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1242>`_)
+* Update pre-commit the same as on the main branch (`#1232 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1232>`_)
+* Disable pose broadcaster on mock hardware (backport of `#1229 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1229>`_) (`#1230 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1230>`_)
+* Remove unused include (backport of `#1220 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1220>`_)
+  Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
+* Contributors: Felix Exner, mergify[bot]
+
+2.5.1 (2024-12-21)
+------------------
+
+2.5.0 (2024-12-18)
+------------------
+* Freedrive Controller (`#1114 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1114>`_) (`#1211 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1211>`_)
+* Add force mode controller (`#1049 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1049>`_) (`#1193 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1193>`_)
+* Update package maintainers (backport of `#1203 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1203>`_)
+* Forward trajectory controller (backport of `#944 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/944>`_)
+* Use pose_broadcaster to publish the TCP pose (backport of `#1108 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1108>`_)
+* Contributors: mergify[bot]
+
+2.2.16 (2024-10-28)
+-------------------
+* Allow setting the analog output domain when setting an analog output (backport of `#1123 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1123>`_)
+* Fix component lifecycle (backport of `#1098 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1098>`_)
+* [moveit] Disable execution_duration_monitoring by default (`#1133 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1133>`_)
+* Service to get software version of robot (backport of `#964 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/964>`_)
+* Assure the description is loaded as string (backport of `#1107 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1107>`_)
+* Contributors: Felix Exner (fexner), mergify[bot], Jacob Larsen
+
+2.2.15 (2024-07-26)
+-------------------
+* Fix passing launch_dashobard_client launch argument (backport of `#1057 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1057>`_)
+* Updated the UR family photo on the readme (backport of `#1064 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1064>`_)
+* Contributors: Rune Søoe-Knudsen, Felix Exner
+
+2.2.14 (2024-07-01)
+-------------------
+* Add sleep between controller stopper's controller queries (backport of `#1038 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1038>`_)
+* Contributors: Felix Exner
+
+2.2.13 (2024-06-17)
+-------------------
+* Use robot_receive_timeout instead of keepalive_count (`#1009 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1009>`_)
+* Remove extra spaces from start_ursim statement in tests (backport of `#1010 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1010>`_)
+* Add calibration file to launch arguments (`#1001 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1001>`_)
+* Contributors: Vincenzo Di Pentima, Felix Exner
+
+2.2.12 (2024-05-16)
+-------------------
+* Remove dependency to docker.io (backport `#985 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/985>`_)
+* Simplify tests (backport of `#849 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/849>`_)
+* Update installation instructions for source build (backport `#967 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/967>`_)
+* Move installation instructions to subpage (backport `#870 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/870>`_)
+* Reduce number of controller_spawners to 3 (`#928 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/928>`_)
+* Fix multi-line strings in DeclareLaunchArgument (backport `#948 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/948>`_)
+* "use_fake_hardware" for UR20 (`#950 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/950>`_)
+* Contributors: Vincenzo Di Pentima, Felix Exner, Robert Wilbrandt, Matthijs van der Burgh
+
+2.2.11 (2024-04-08)
+-------------------
+* Add UR30 support (`#930 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/930>`_)
+* Move communication setup to on_configure instead of on_activate (`#936 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/936>`_)
+* Contributors: Felix Exner, Vincenzo Di Pentima
+
+2.2.10 (2024-01-03)
+-------------------
+* Add backward_ros to driver (`#872 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/872>`_) (`#878 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/878>`_)
+* Port configuration  (`#835 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/835>`_) (`#847 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/847>`_)
+* Update link to MoveIt! documentation (`#845 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/845>`_)
+* Contributors: mergify[bot]
+
+2.2.9 (2023-09-22)
+------------------
+* Added a test that sjtc correctly aborts on violation of constraints
+* Added support for UR20 (`#805 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/805>`_)
+* Introduced tf_prefix into log handler (`#713 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/713>`_)
+* Start ursim from lib (`#733 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/733>`_)
+* Run robot driver test also with tf_prefix (`#729 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/729>`_)
+* Urscript interface (`#721 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/721>`_) (`#742 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/742>`_)
+* Contributors: Felix Exner, Lennart Nachtigall, mergify[bot]
+
+2.2.8 (2023-06-26)
+------------------
+* Use tf prefix properly (backport `#688 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/688>`_) (`#725 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/725>`_)
+* Use SCHED_FIFO for controller_manager's main thread (`#719 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/719>`_) (`#722 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/722>`_)
+* Contributors: mergify[bot]
+
+2.2.7 (2023-06-02)
+------------------
+* Calling on_deactivate in dtr (`#679 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/679>`_) (`#704 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/704>`_)
+* Adds full nonblocking readout support (Multiarm part 4)  - v2 (`#673 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/673>`_) (`#703 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/703>`_)
+* Correct calibration correction launch file in doc (`#590 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/590>`_)
+* Introduce hand back control service (`#528 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/528>`_) (`#670 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/670>`_)
+* Update definition of test goals to new version. (backport `#637 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/637>`_) (`#668 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/668>`_)
+* Default path to ur_client_library urscript (`#316 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/316>`_) (`#553 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/553>`_)
+  * Change default path for urscript for headless mode.
+  * Replace urscript path also in newer ur_robot_driver launchfile
+* Wait longer for controllers to load and activate
+* Fix flaky tests (`#641 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/641>`_)
+* Added services to set tool voltage and zero force torque sensor (`#466 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/466>`_) (`#582 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/582>`_)
+* Controller spawner timeout (backport `#608 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/608>`_) (`#609 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/609>`_)
+* Fix cmake dependency on controller_manager (backport `#598 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/598>`_) (`#599 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/599>`_)
+* Increase timeout for first test service call to driver (Backport of `#605 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/605>`_) (`#607 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/607>`_)
+* Update linters & checkers (backport `#426 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/426>`_) (`#556 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/556>`_)
+* Clean up & improve execution tests (Backport of `#512 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/512>`_) (`#552 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/552>`_)
+* Contributors: Felix Exner (fexner), Lennart Nachtigall, Robert Wilbrandt, mergify[bot], Denis Stogl, livanov93, Mads Holm Peters
+
+2.2.6 (2022-11-28)
+------------------
+* Cleanup humble branch (`#545 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/545>`_)
+* Contributors: Felix Exner (fexner)
+
+2.2.5 (2022-11-19)
+------------------
+* ur_robot_driver: Controller_stopper fix deprecation warning
+* Fix tool voltage setup (`#526 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/526>`_)
+  * Move BEGIN_REPLACE inside of header
+  * Change default value of tool_voltage
+  Keeping this at 0 requires users to explicitly set it to non-zero. This way
+  we won't accitentally destroy hardware that cannot handle 24V.
+* Added dependency to socat (`#527 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/527>`_)
+  This is needed for the tool forwarding.
+* Add a note in the tool_comm doc about a URCap conflict (`#524 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/524>`_)
+  * Add a note in the tool_comm doc about a URCap conflict
+  * Update ur_robot_driver/doc/setup_tool_communication.rst
+  Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com>
+  * Fix formatting and one spelling mistake
+  Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com>
+* Contributors: Felix Exner, Felix Exner (fexner)
+
+2.2.4 (2022-10-07)
+------------------
+* Remove the custom ursim docker files (`#478 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/478>`_)
+  This has been migrated inside the docs and is not needed anymore.
+* Remove duplicated update_rate parameter (`#479 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/479>`_)
+* Contributors: Felix Exner
+
+2.2.3 (2022-07-27)
+------------------
+* Adapt ros control api (`#448 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/448>`_)
+  * scaled jtc: Use get_interface_name instead of get_name
+  * Migrate from stopped controllers to inactive controllers
+  stopped controllers has been deprecated upstream
+* Contributors: Felix Exner
+
+2.2.2 (2022-07-19)
+------------------
+* Made sure all past maintainers are listed as authors (`#429 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/429>`_)
+* Silence a compilation warning (`#425 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/425>`_)
+  Since setting the receive timeout takes the time_buffer as an argument
+  this raises a "may be used uninitialized" warning. Setting this to 0
+  explicitly should prevent that.
+* Doc: Fix IP address in usage->ursim section (`#422 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/422>`_)
+* Contributors: Felix Exner
+
+2.2.1 (2022-06-27)
+------------------
+* Fixed controller name for force_torque_sensor_broadcaster (`#411 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/411>`_)
+* Contributors: Felix Exner
+
+2.2.0 (2022-06-20)
+------------------
+* Updated package maintainers
+* Rework bringup (`#403 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/403>`_)
+* Prepare for humble (`#394 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/394>`_)
+* Update dependencies on all packages (`#391 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/391>`_)
+* Update HW-interface API for humble. (`#377 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/377>`_)
+* Use types in hardware interface from ros2_control in local namespace (`#339 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/339>`_)
+* Update header extension to remove compile warning. (`#285 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/285>`_)
+* Add resource files from ROS World. (`#226 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/226>`_)
+* Add sphinx documentation (`#340 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/340>`_)
+* Update license to BSD-3-Clause (`#277 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/277>`_)
+* Update ROS_INTERFACE.md to current driver (`#335 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/335>`_)
+* Fix hardware interface names in error output (`#329 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/329>`_)
+* Added controller stopper node (`#309 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/309>`_)
+* Correct link to calibration extraction (`#310 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/310>`_)
+* Start the tool communication script if the flag is set (`#267 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/267>`_)
+* Change driver constructor and change calibration check (`#282 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/282>`_)
+* Use GPIO tag from URDF in driver. (`#224 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/224>`_)
+* Separate control node (`#281 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/281>`_)
+* Add missing dependency on angles and update formatting for linters. (`#283 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/283>`_)
+* Do not print an error output if writing is not possible (`#266 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/266>`_)
+* Update features.md (`#250 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/250>`_)
+* Tool communication (`#218 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/218>`_)
+* Payload service (`#238 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/238>`_)
+* Import transformation of force-torque into tcp frame from ROS1 driver (https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/src/hardware_interface.cpp). (`#237 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/237>`_)
+* Make reading and writing work when hardware is disconnected (`#233 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/233>`_)
+* Add missing command and state interfaces to get everything working with the fake hardware and add some comment into xacro file to be clearer. (`#221 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/221>`_)
+* Decrease the rate of async tasks. (`#223 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/223>`_)
+* Change robot type. (`#220 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/220>`_)
+* Driver to headless. (`#217 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/217>`_)
+* Test execution tests (`#216 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/216>`_)
+* Integration tests improvement (`#206 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/206>`_)
+* Set start modes to empty. Avoid position ctrl loop on start. (`#211 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/211>`_)
+* Add resend program service and enable headless mode (`#198 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/198>`_)
+* Implement "choices" for robot_type param (`#204 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/204>`_)
+* Calibration extraction package (`#186 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/186>`_)
+* Add breaking api changes from ros2_control to hardware_interface (`#189 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/189>`_)
+* Fix prepare and perform switch operation (`#191 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/191>`_)
+* Update CI configuration to support galactic and rolling (`#142 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/142>`_)
+* Dockerize ursim with driver in docker compose (`#144 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/144>`_)
+* Enabling velocity mode (`#146 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/146>`_)
+* Moved registering publisher and service to on_active (`#151 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/151>`_)
+* Converted io_test and switch_on_test to ROS2 (`#124 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/124>`_)
+* Added loghandler to handle log messages from the Client Library with … (`#126 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/126>`_)
+* Removed dashboard client from hardware interface
+* [WIP] Updated feature list (`#102 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/102>`_)
+* Moved Async check out of script running check (`#112 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/112>`_)
+* Fix gpio controller (`#103 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/103>`_)
+* Fixed speed slider service call (`#100 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/100>`_)
+* Adding missing backslash and only setting workdir once (`#108 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/108>`_)
+* Added dockerfile for the driver (`#105 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/105>`_)
+* Using official Universal Robot Client Library (`#101 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/101>`_)
+* Reintegrating missing ur_client_library dependency since the break the building process (`#97 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/97>`_)
+* Fix readme hardware setup (`#91 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/91>`_)
+* Fix move to home bug (`#92 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/92>`_)
+* Using modern python
+* Some intermediate commit
+* Remove obsolete and unused files and packages. (`#80 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/80>`_)
+* Review CI by correcting the configurations (`#71 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/71>`_)
+* Add support for gpios, update MoveIt and ros2_control launching (`#66 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/66>`_)
+* Quickfix against move home bug
+* Added missing initialization
+* Use GitHub Actions, use pre-commit formatting (`#56 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/56>`_)
+* Put dashboard services into corresponding namespace
+* Start dashboard client from within the hardware interface
+* Added try catch blocks for service calls
+* Removed repeated declaration of timeout parameter which lead to connection crash
+* Removed static service name in which all auto generated services where mapped
+* Removed unused variable
+* Fixed clang-format issue
+* Removed all robot status stuff
+* Exchanged hardcoded value for RobotState msgs enum
+* Removed currently unused controller state variables
+* Added placeholder for industrial_robot_status_interface
+* Fixed clang issues
+* Added checks for internal robot state machine
+* Only load speed scaling interface
+* Changed state interface to combined speed scaling factor
+* Added missing formatting in hardware interface
+* Initial version of the speed_scaling_state_controller
+* Fix clang tidy in multiple pkgs.
+* Clang tidy fix.
+* Update force torque state controller.
+* Prepare for testing.
+* Fix decision breaker for position control. Make decision effect instantaneous.
+* Use only position interface.
+* Update hardware interface for ROS2 (`#8 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/8>`_)
+* Update the dashboard client for ROS2 (`#5 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/5>`_)
+* Hardware interface framework (`#3 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/3>`_)
+* Add XML schema to all ``package.xml`` files
+* Silence ``ament_lint_cmake`` errors
+* Update packaging for ROS2
+* Update package.xml files so ``ros2 pkg list`` shows all pkgs
+* Clean out ur_robot_driver for initial ROS2 compilation
+* Compile ur_dashboard_msgs for ROS2
+* Delete all launch/config files with no UR5 relation
+* Initial work toward compiling ur_robot_driver
+* Update CMakeLists and package.xml for:
+  - ur5_moveit_config
+  - ur_bringup
+  - ur_description
+* Change pkg versions to 0.0.0
+* Contributors: AndyZe, Denis Stogl, Denis Štogl, Felix Exner, John Morris, Lovro, Mads Holm Peters, Marvin Große Besselmann, Rune Søe-Knudsen, livanov93, Robert Wilbrandt
+
+0.0.3 (2019-08-09)
+------------------
+* Added a service to end ROS control from ROS side
+* Publish IO state on ROS topics
+* Added write channel through RTDE with speed slider and IO services
+* Added subscriber to send arbitrary URScript commands to the robot
+
+0.0.2 (2019-07-03)
+------------------
+* Fixed dependencies and installation
+* Updated README
+* Fixed passing parameters through launch files
+* Added support for correctly switching controllers during runtime and using the standard
+  joint_trajectory_controller
+* Updated externalcontrol URCap to version 1.0.2
+  + Fixed Script timeout when running the URCap inside of a looping tree
+  + Fixed a couple of typos
+* Increased minimal required UR software version to 3.7/5.1
+
+0.0.1 (2019-06-28)
+------------------
+Initial release

CMakeLists.txt

@@ -0,0 +1,227 @@
+cmake_minimum_required(VERSION 3.5)
+project(ur_robot_driver)
+
+# Default to off as this only works in environments where a new docker container can be started with the appropriate networking, which
+# might not be possible e.g. inside the buildfarm
+option(
+  UR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS
+  "Build integration tests using the start_ursim script"
+  OFF
+)
+
+add_compile_options(-Wall)
+add_compile_options(-Wextra)
+add_compile_options(-Wno-unused-parameter)
+
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.")
+  set(CMAKE_BUILD_TYPE RelWithDebInfo)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+find_package(controller_manager REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(ur_client_library REQUIRED)
+find_package(ur_dashboard_msgs REQUIRED)
+find_package(ur_msgs REQUIRED)
+
+include_directories(include)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_manager
+  controller_manager_msgs
+  geometry_msgs
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  std_msgs
+  std_srvs
+  tf2_geometry_msgs
+  ur_client_library
+  ur_dashboard_msgs
+  ur_msgs
+)
+
+add_library(ur_robot_driver_plugin
+  SHARED
+  src/dashboard_client_ros.cpp
+  src/hardware_interface.cpp
+  src/urcl_log_handler.cpp
+  src/robot_state_helper.cpp
+)
+target_link_libraries(
+  ur_robot_driver_plugin
+  ur_client_library::urcl
+)
+target_include_directories(
+  ur_robot_driver_plugin
+  PRIVATE
+  include
+)
+ament_target_dependencies(
+  ur_robot_driver_plugin
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml)
+
+#
+# dashboard_client
+#
+add_executable(dashboard_client
+  src/dashboard_client_ros.cpp
+  src/dashboard_client_node.cpp
+  src/urcl_log_handler.cpp
+)
+target_link_libraries(dashboard_client ur_client_library::urcl)
+ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+#
+# ur_ros2_control_node
+#
+add_executable(ur_ros2_control_node
+  src/ur_ros2_control_node.cpp
+)
+ament_target_dependencies(ur_ros2_control_node
+  controller_manager
+  rclcpp
+)
+
+#
+# controller_stopper_node
+#
+add_executable(controller_stopper_node
+  src/controller_stopper.cpp
+  src/controller_stopper_node.cpp
+)
+ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+#
+# robot_state_helper
+#
+add_executable(robot_state_helper
+  src/robot_state_helper.cpp
+  src/robot_state_helper_node.cpp
+)
+target_link_libraries(robot_state_helper ur_client_library::urcl)
+ament_target_dependencies(robot_state_helper ${${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 urscript_interface robot_state_helper
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# INSTALL
+install(
+  TARGETS ur_robot_driver_plugin
+  DESTINATION lib
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ur_robot_driver_plugin
+)
+
+install(DIRECTORY resources
+  DESTINATION share/${PROJECT_NAME}
+  REGEX "/ursim/" EXCLUDE
+)
+
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+# Install Python execs
+ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables
+install(PROGRAMS
+  scripts/tool_communication.py
+  scripts/ur3e_actions.py
+  scripts/start_ursim.sh
+  scripts/hi.py
+  scripts/ur3e_topic.py
+  examples/examples.py
+  examples/force_mode.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(PROGRAMS scripts/wait_dashboard_server.sh
+  DESTINATION bin
+)
+
+install(PROGRAMS scripts/start_ursim.sh
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
+
+if(BUILD_TESTING)
+  find_package(ur_controllers REQUIRED)
+  find_package(ur_description REQUIRED)
+  find_package(ur_msgs REQUIRED)
+  find_package(launch_testing_ament_cmake)
+
+  if(${UR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS})
+    add_launch_test(test/launch_args.py
+      TIMEOUT
+        180
+    )
+    add_launch_test(test/dashboard_client.py
+      TIMEOUT
+        180
+    )
+    add_launch_test(test/robot_driver.py
+      TIMEOUT
+        800
+    )
+    add_launch_test(test/integration_test_controller_switch.py
+      TIMEOUT
+        800
+    )
+    add_launch_test(test/integration_test_force_mode.py
+      TIMEOUT
+        800
+    )
+    add_launch_test(test/urscript_interface.py
+      TIMEOUT
+        500
+    )
+  endif()
+endif()

README.md

@@ -0,0 +1,96 @@
+# ur_robot_driver
+
+This package contains the actual driver for UR robots. It is part of the *universal_robots_driver*
+repository and requires other packages from that repository. Also, see the [main repository's
+README](../README.md) for information on how to install and startup this driver.
+
+## ROS-API
+The ROS API is documented in a [standalone document](doc/ROS_INTERFACE.md).
+
+## Technical details
+The following image shows a very coarse overview of the driver's architecture.
+
+![Architecture overview](doc/architecture_coarse.svg "Architecture overview")
+
+Upon connection to the primary interface the robot sends version and calibration information which
+is consumed by the *calibration_check*. If the calibration reported by the robot doesn't match the
+one configured (See [calibration guide](../ur_calibration/README.md)) an error will be printed to Roslog.
+
+Real-time data from the robot is read through the RTDE interface. This is done automatically as soon
+as a connection to the robot could be established. Thus joint states and IO data will be immediately
+available.
+
+To actually control the robot, a program node from the **External Control** URCap must be running on
+the robot interpreting commands sent from an external source. When this program is not running, no
+controllers moving the robot around will be available. Please see the [initial setup
+guide](doc/installation/robot_setup.rst) on how to install and start this on the robot.
+
+The URScript that will be running on the robot is requested by the **External Control** program node
+from the remote ROS PC. The robot *ur_control.launch* file has a parameter called `urscript_file` to
+select a different program than the default one that will be sent as a response to a program
+request.
+
+**Custom script snippets** can be sent to the robot on a topic basis. By default, they will
+interrupt other programs (such as the one controlling the robot). For a certain subset of functions,
+it is however possible to send them as secondary programs. See [UR
+documentation](https://www.universal-robots.com/articles/ur/programming/secondary-program/)
+on details.
+<br/>
+**Note to e-Series users:**
+The robot won't accept script code from a remote source unless the robot is put into
+*remote_control-mode*. However, if put into *remote_control-mode*, the program containing the
+**External Control** program node can't be started from the panel.
+For this purpose, please use the **dashboard** services to load, start and stop the main program
+running on the robot. See the [ROS-API documentation](doc/ROS_INTERFACE.md) for details on the
+dashboard services.
+
+For using the **tool communication interface** on e-Series robots, a `socat` script is prepared to
+forward the robot's tool communication interface to a local device on the ROS PC. See [the tool
+communication setup guide](doc/setup_tool_communication.rst) for details.
+
+This driver is using [ROS-Control](https://wiki.ros.org/ros_control) for any control statements.
+Therefore, it can be used with all position-based controllers available in ROS-Control. However, we
+recommend using the controllers from the `ur_controllers` package. See it's
+[documentation](../ur_controllers/README.md) for details. **Note: Speed scaling support will only be
+available using the controllers from `ur_controllers`**
+
+## A note about modes
+The term **mode** is used in different meanings inside this driver.
+
+### Remote control mode
+On the e-series the robot itself can operate in different command modes: It can be either in **local control
+mode** where the teach pendant is the single point of command or in **remote control mode**, where
+motions from the TP, starting & loading programs from the TP activating the freedrive mode are
+blocked. Note that the **remote control mode** has to be explicitly enabled in the robot's settings
+under **Settings** -> **System** -> **Remote Control**. See the robot's manual for details.
+
+The **remote control mode** is needed for many aspects of this driver such as
+ * headless mode (see below)
+ * sending script code to the robot
+ * many dashboard functionalities such as
+   * restarting the robot after protective / EM-Stop
+   * powering on the robot and do brake release
+   * loading and starting programs
+ * the `set_mode` action, as it uses the dashboard calls mentioned above
+
+### Headless mode
+Inside this driver, there's the **headless** mode, which can be either enabled or not. When the
+[headless mode](./doc/ROS_INTERFACE.md#headless_mode-default-false) is activated, required script
+code for external control will be sent to the robot directly when the driver starts. As soon as
+other script code is sent to the robot either by sending it directly through this driver or by
+pressing any motion-related button on the teach pendant, the script will be overwritten by this
+action and has to be restarted by using the
+[resend_robot_program](./doc/ROS_INTERFACE.md#resend_robot_program-std_srvstrigger) service. If this
+is necessary, you will see the output `Connection to robot dropped, waiting for new connection.`
+from the driver. Note that pressing "play" on the TP won't start the external control again, but
+whatever program is currently loaded on the controller. This mode doesn't require the "External
+Control" URCap being installed on the robot as the program is sent to the robot directly. However,
+we recommend to use the non-headless mode and leverage the `set_mode` action to start program
+execution without the teach pendant. The **headless** mode might be removed in future releases.
+
+**Note for the e-Series:** In order to leverage the **headless** mode on the e-Series the robot must
+be in **remote_control_mode** as explained above.
+
+## controller_stopper
+A small helper node that stops and restarts ROS controllers based on a boolean status topic. When the status goes to `false`, all running controllers except a set of predefined *consistent_controllers* gets stopped. If status returns to `true` the stopped controllers are restarted.
+This is done by Subscribing to a robot's running state topic. Ideally this topic is latched and only publishes on changes. However, this node only reacts on state changes, so a state published each cycle would also be fine.

config/test_goal_publishers_config.yaml

@@ -0,0 +1,65 @@
+publisher_scaled_joint_trajectory_controller:
+  ros__parameters:
+
+    controller_name: "scaled_joint_trajectory_controller"
+    wait_sec_between_publish: 6
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1:
+      positions: [0.785, -1.57, 0.785, 0.785, 0.785, 0.785]
+    pos2:
+      positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]
+    pos3:
+      positions: [0.0, -1.57, 0.0, 0.0, -0.785, 0.0]
+    pos4:
+      positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]
+
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
+    check_starting_point: true
+    starting_point_limits:
+      shoulder_pan_joint: [-0.1,0.1]
+      shoulder_lift_joint: [-1.6,-1.5]
+      elbow_joint: [-0.1,0.1]
+      wrist_1_joint: [-1.6,-1.5]
+      wrist_2_joint: [-0.1,0.1]
+      wrist_3_joint: [-0.1,0.1]
+
+publisher_joint_trajectory_controller:
+  ros__parameters:
+
+    controller_name: "joint_trajectory_controller"
+    wait_sec_between_publish: 6
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1:
+      positions: [0.785, -1.57, 0.785, 0.785, 0.785, 0.785]
+    pos2:
+      positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]
+    pos3:
+      positions: [0.0, -1.57, 0.0, 0.0, -0.785, 0.0]
+    pos4:
+      positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]
+
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
+    check_starting_point: true
+    starting_point_limits:
+      shoulder_pan_joint: [-0.1,0.1]
+      shoulder_lift_joint: [-1.6,-1.5]
+      elbow_joint: [-0.1,0.1]
+      wrist_1_joint: [-1.6,-1.5]
+      wrist_2_joint: [-0.1,0.1]
+      wrist_3_joint: [-0.1,0.1]

config/test_velocity_goal_publishers_config.yaml

@@ -0,0 +1,10 @@
+publisher_forward_velocity_controller:
+  ros__parameters:
+
+    controller_name: "forward_velocity_controller"
+    wait_sec_between_publish: 5
+
+    goal_names: ["pos1", "pos2", "pos3"]
+    pos1: [0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
+    pos2: [0.0, 0.0, 0.0, 0.0, 0.0, -0.05]
+    pos3: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

config/ur10_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 125  # Hz

config/ur10e_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz

config/ur16e_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz

config/ur20_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz

config/ur30_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz

config/ur3_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 125  # Hz

config/ur3e_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz

config/ur5_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 125  # Hz

config/ur5e_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz

config/ur_controllers.yaml

@@ -0,0 +1,177 @@
+controller_manager:
+  ros__parameters:
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    io_and_status_controller:
+      type: ur_controllers/GPIOController
+
+    speed_scaling_state_broadcaster:
+      type: ur_controllers/SpeedScalingStateBroadcaster
+
+    force_torque_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    scaled_joint_trajectory_controller:
+      type: ur_controllers/ScaledJointTrajectoryController
+
+    forward_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    forward_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    force_mode_controller:
+      type: ur_controllers/ForceModeController
+
+    freedrive_mode_controller:
+      type: ur_controllers/FreedriveModeController
+
+    passthrough_trajectory_controller:
+      type: ur_controllers/PassthroughTrajectoryController
+
+    tcp_pose_broadcaster:
+      type: pose_broadcaster/PoseBroadcaster
+
+    passthrough_trajectory_controller:
+      type: ur_controllers/PassthroughTrajectoryController
+
+    ur_configuration_controller:
+      type: ur_controllers/URConfigurationController
+
+speed_scaling_state_broadcaster:
+  ros__parameters:
+    state_publish_rate: 100.0
+    tf_prefix: "$(var tf_prefix)"
+
+io_and_status_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+
+ur_configuration_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+
+force_torque_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: $(var tf_prefix)tcp_fts_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    frame_id: $(var tf_prefix)tool0
+    topic_name: ft_data
+
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 10.0
+    allow_partial_joints_goal: false 
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 5.0
+      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)elbow_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)wrist_1_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)wrist_2_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)wrist_3_joint: { trajectory: 0.5, goal: 0.5 }
+
+
+scaled_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 10.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 5.0
+      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)elbow_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)wrist_1_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)wrist_2_joint: { trajectory: 0.5, goal: 0.5 }
+      $(var tf_prefix)wrist_3_joint: { trajectory: 0.5, goal: 0.5 }
+    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor
+
+passthrough_trajectory_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+    state_interfaces:
+      - position
+      - velocity
+    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor
+
+forward_velocity_controller:
+  ros__parameters:
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+    interface_name: velocity
+
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+
+force_mode_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+
+freedrive_mode_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+
+tcp_pose_broadcaster:
+  ros__parameters:
+    frame_id: $(var tf_prefix)base
+    pose_name: $(var tf_prefix)tcp_pose
+    tf:
+      child_frame_id: $(var tf_prefix)tool0_controller

doc/ROS_INTERFACE.md

@@ -0,0 +1,213 @@
+**NOTE**: This documentation is obsolete and does not cover in full the current state of driver.
+
+# ROS interface
+
+The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series.
+
+## Nodes
+
+### ur_ros2_control_node
+
+This is the actual driver node containing the ROS2-Control stack. Interfaces documented here refer to the robot's hardware interface. Controller-specific API elements might be present for the individual controllers outside of this package.
+
+#### Parameters
+Note that parameters are passed through the ros2_control xacro definition.
+
+##### headless_mode
+
+Start robot in headless mode. This does not require the 'External Control' URCap to be running on the robot, but this will send the URScript to the robot directly. On e-Series robots this requires the robot to run in 'remote-control' mode.
+
+##### input_recipe_filename (Required)
+
+Path to the file containing the recipe used for requesting RTDE inputs.
+
+##### kinematics/hash (Required)
+
+Hash of the calibration reported by the robot. This is used for validating the robot description is using the correct calibration. If the robot's calibration doesn't match this hash, an error will be printed. You can use the robot as usual, however Cartesian poses of the endeffector might be inaccurate. See the "ur_calibration" package on help how to generate your own hash matching your actual robot.
+
+##### non_blocking_read (default: "false")
+
+Enables non_blocking_read mode. Disables error generated when read returns without any data, sets
+the read timeout to zero, and synchronizes read/write operations. This is a leftover from the ROS1
+driver and we currently see no real application where it would make sense to enable this.
+
+##### output_recipe_filename (Required)
+
+Path to the file containing the recipe used for requesting RTDE outputs.
+
+##### reverse_port (Required)
+
+Port that will be opened to communicate between the driver and the robot controller.
+
+##### robot_ip (Required)
+
+The robot's IP address.
+
+##### script_filename (Required)
+
+Path to the urscript code that will be sent to the robot.
+
+##### script_sender_port (Required)
+
+The driver will offer an interface to receive the program's URScript on this port.
+
+##### servoj_gain (Required)
+
+Specify the gain for the underlying servoj command. This will be used whenever position control is
+active. A higher value will lead to sharper motions, but might also introduce
+higher jerks and vibrations.
+
+Range: [100 - 3000]
+
+##### servoj_lookahead_time (Required)
+
+Specify lookahead_time parameter of underlying servoj command. This will be used whenever position
+control is active. A higher value will result in smoother trajectories, but will also introduce a
+higher delay between the commands sent from ROS and the motion being executed on the robot.
+
+Unit: seconds, range: [0.03 - 0.2]
+
+
+
+##### tool_baud_rate (Required)
+
+Baud rate used for tool communication. Will be set as soon as the UR-Program on the robot is started. See UR documentation for valid baud rates.  Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE.  Then, this parameter is required.
+
+##### tool_parity (Required)
+
+Parity used for tool communication. Will be set as soon as the UR-Program on the robot is started. Can be 0 (None), 1 (odd) and 2 (even).  Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE.  Then, this parameter is required.
+
+##### tool_rx_idle_chars (Required)
+
+Number of idle chars for the RX unit used for tool communication. Will be set as soon as the UR-Program on the robot is started. Valid values: min=1.0, max=40.0  Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE.  Then, this parameter is required.
+
+##### tool_stop_bits (Required)
+
+Number of stop bits used for tool communication. Will be set as soon as the UR-Program on the robot is started. Can be 1 or 2.  Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE.  Then, this parameter is required.
+
+##### tool_tx_idle_chars (Required)
+
+Number of idle chars for the TX unit used for tool communication. Will be set as soon as the UR-Program on the robot is started. Valid values: min=0.0, max=40.0  Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE.  Then, this parameter is required.
+
+##### tool_voltage (Required)
+
+Tool voltage that will be set as soon as the UR-Program on the robot is started. Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE. Then, this parameter is required.
+
+##### use_tool_communication (Required)
+
+Should the tool's RS485 interface be forwarded to the ROS machine? This is only available on e-Series models. Setting this parameter to TRUE requires multiple other parameters to be set,as well.
+
+
+### dashboard_client
+
+#### Advertised Services
+
+##### add_to_log ([ur_dashboard_msgs/AddToLog](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/AddToLog.html))
+
+Service to add a message to the robot's log
+
+##### brake_release ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Service to release the brakes. If the robot is currently powered off, it will get powered on on the fly.
+
+##### clear_operational_mode ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+If this service is called the operational mode can again be changed from PolyScope, and the user password is enabled.
+
+##### close_popup ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Close a (non-safety) popup on the teach pendant.
+
+##### close_safety_popup ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Close a safety popup on the teach pendant.
+
+##### connect ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Service to reconnect to the dashboard server
+
+##### get_loaded_program ([ur_dashboard_msgs/GetLoadedProgram](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetLoadedProgram.html))
+
+Load a robot installation from a file
+
+##### get_robot_mode ([ur_dashboard_msgs/GetRobotMode](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetRobotMode.html))
+
+Service to query the current robot mode
+
+##### get_safety_mode ([ur_dashboard_msgs/GetSafetyMode](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetSafetyMode.html))
+
+Service to query the current safety mode
+
+##### load_installation ([ur_dashboard_msgs/Load](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/Load.html))
+
+Load a robot installation from a file
+
+##### load_program ([ur_dashboard_msgs/Load](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/Load.html))
+
+Load a robot program from a file
+
+##### pause ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Pause a running program.
+
+##### play ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Start execution of a previously loaded program
+
+##### popup ([ur_dashboard_msgs/Popup](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/Popup.html))
+
+Service to show a popup on the UR Teach pendant.
+
+##### power_off ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Power off the robot motors
+
+##### power_on ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Power on the robot motors. To fully start the robot, call 'brake_release' afterwards.
+
+##### program_running ([ur_dashboard_msgs/IsProgramRunning](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/IsProgramRunning.html))
+
+Query whether there is currently a program running
+
+##### program_saved ([ur_dashboard_msgs/IsProgramSaved](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/IsProgramSaved.html))
+
+Query whether the current program is saved
+
+##### program_state ([ur_dashboard_msgs/GetProgramState](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetProgramState.html))
+
+Service to query the current program state
+
+##### quit ([ur_dashboard_msgs/GetLoadedProgram](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetLoadedProgram.html))
+
+Disconnect from the dashboard service.
+
+##### raw_request ([ur_dashboard_msgs/RawRequest](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/RawRequest.html))
+
+General purpose service to send arbitrary messages to the dashboard server
+
+##### restart_safety ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Used when robot gets a safety fault or violation to restart the safety. After safety has been rebooted the robot will be in Power Off. NOTE: You should always ensure it is okay to restart the system. It is highly recommended to check the error log before using this command (either via PolyScope or e.g. ssh connection).
+
+##### shutdown ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Shutdown the robot controller
+
+##### stop ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Stop program execution on the robot
+
+##### unlock_protective_stop ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+Dismiss a protective stop to continue robot movements. NOTE: It is the responsibility of the user to ensure the cause of the protective stop is resolved before calling this service.
+
+#### Parameters
+
+##### receive_timeout (Required)
+
+Timeout after which a call to the dashboard server will be considered failure if no answer has been received.
+
+##### robot_ip (Required)
+
+The IP address under which the robot is reachable.

doc/conf.py

@@ -0,0 +1,175 @@
+#
+# Configuration file for the Sphinx documentation builder.
+#
+# This file does only contain a selection of the most common options. For a
+# full list see the documentation:
+# http://www.sphinx-doc.org/en/master/config
+
+# -- Path setup --------------------------------------------------------------
+
+# If extensions (or modules to document with autodoc) are in another directory,
+# add these directories to sys.path here. If the directory is relative to the
+# documentation root, use os.path.abspath to make it absolute, like shown here.
+#
+# import os
+# import sys
+# sys.path.insert(0, os.path.abspath('.'))
+
+
+# -- Project information -----------------------------------------------------
+
+project = "ur_robot_driver"
+copyright = "2022, Universal Robots A/S"
+author = "Felix Exner"
+
+# The short X.Y version
+version = ""
+# The full version, including alpha/beta/rc tags
+release = ""
+
+
+# -- General configuration ---------------------------------------------------
+
+# If your documentation needs a minimal Sphinx version, state it here.
+#
+# needs_sphinx = '1.0'
+
+# Add any Sphinx extension module names here, as strings. They can be
+# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
+# ones.
+extensions = []
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ["_templates"]
+
+# The suffix(es) of source filenames.
+# You can specify multiple suffix as a list of string:
+#
+source_suffix = [".rst", ".md"]
+
+# The master toctree document.
+master_doc = "index"
+
+# The language for content autogenerated by Sphinx. Refer to documentation
+# for a list of supported languages.
+#
+# This is also used if you do content translation via gettext catalogs.
+# Usually you set "language" from the command line for these cases.
+language = None
+
+# List of patterns, relative to source directory, that match files and
+# directories to ignore when looking for source files.
+# This pattern also affects html_static_path and html_extra_path.
+exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"]
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = None
+
+
+# -- Options for HTML output -------------------------------------------------
+
+# The theme to use for HTML and HTML Help pages.  See the documentation for
+# a list of builtin themes.
+#
+html_theme = "alabaster"
+
+# Theme options are theme-specific and customize the look and feel of a theme
+# further.  For a list of options available for each theme, see the
+# documentation.
+#
+# html_theme_options = {}
+
+# Add any paths that contain custom static files (such as style sheets) here,
+# relative to this directory. They are copied after the builtin static files,
+# so a file named "default.css" will overwrite the builtin "default.css".
+html_static_path = ["_static"]
+
+# Custom sidebar templates, must be a dictionary that maps document names
+# to template names.
+#
+# The default sidebars (for documents that don't match any pattern) are
+# defined by theme itself.  Builtin themes are using these templates by
+# default: ``['localtoc.html', 'relations.html', 'sourcelink.html',
+# 'searchbox.html']``.
+#
+# html_sidebars = {}
+
+
+# -- Options for HTMLHelp output ---------------------------------------------
+
+# Output file base name for HTML help builder.
+htmlhelp_basename = "ur_robot_driverdoc"
+
+
+# -- Options for LaTeX output ------------------------------------------------
+
+latex_elements = {
+    # The paper size ('letterpaper' or 'a4paper').
+    #
+    # 'papersize': 'letterpaper',
+    # The font size ('10pt', '11pt' or '12pt').
+    #
+    # 'pointsize': '10pt',
+    # Additional stuff for the LaTeX preamble.
+    #
+    # 'preamble': '',
+    # Latex figure (float) alignment
+    #
+    # 'figure_align': 'htbp',
+}
+
+# Grouping the document tree into LaTeX files. List of tuples
+# (source start file, target name, title,
+#  author, documentclass [howto, manual, or own class]).
+latex_documents = [
+    (
+        master_doc,
+        "ur_robot_driver.tex",
+        "ur\\_robot\\_driver Documentation",
+        "Felix Exner",
+        "manual",
+    ),
+]
+
+
+# -- Options for manual page output ------------------------------------------
+
+# One entry per manual page. List of tuples
+# (source start file, name, description, authors, manual section).
+man_pages = [(master_doc, "ur_robot_driver", "ur_robot_driver Documentation", [author], 1)]
+
+
+# -- Options for Texinfo output ----------------------------------------------
+
+# Grouping the document tree into Texinfo files. List of tuples
+# (source start file, target name, title, author,
+#  dir menu entry, description, category)
+texinfo_documents = [
+    (
+        master_doc,
+        "ur_robot_driver",
+        "ur_robot_driver Documentation",
+        author,
+        "ur_robot_driver",
+        "One line description of project.",
+        "Miscellaneous",
+    ),
+]
+
+
+# -- Options for Epub output -------------------------------------------------
+
+# Bibliographic Dublin Core info.
+epub_title = project
+
+# The unique identifier of the text. This can be a ISBN number
+# or the project homepage.
+#
+# epub_identifier = ''
+
+# A unique identification for the text.
+#
+# epub_uid = ''
+
+# A list of files that should not be packed into the epub file.
+epub_exclude_files = ["search.html"]

doc/controller_stopper.rst

@@ -0,0 +1,29 @@
+.. _controller_stopper:
+
+Controller stopper
+==================
+
+As explained in the section :ref:`robot_startup_program`, the robot needs to run a program in order
+to receive motion commands from the ROS driver. When the program is not running, commands sent to
+the robot will have no effect.
+
+To make that transparent, the ``controller_stopper`` node mirrors that state in the ROS
+controller's state. It listens to the ``/io_and_status_controller/robot_program_running`` topic and
+deactivates all motion controllers  (or any controller not explicitly marked as "consistent", see
+below)when the program is not running.
+
+Once the program is running again, any previously active motion controller will be activated again.
+
+This way, when sending commands to an inactive controller the caller should be transparently
+informed, that the controller cannot accept commands at the moment.
+
+In the same way, any running action on the ROS controller will be aborted, as the controller gets
+deactivated by the controller_stopper.
+
+Parameters
+----------
+
+- ``~consistent_controllers`` (list of strings, default: ``[]``)
+
+  A list of controller names that should not be stopped when the program is not running. Any
+  controller that doesn't require the robot program to be running should be in that list.

doc/features.rst

@@ -0,0 +1,57 @@
+.. role:: raw-html-m2r(raw)
+   :format: html
+
+
+Feature list and roadmap
+------------------------
+
+.. list-table::
+   :header-rows: 1
+
+   * - Feature
+     - ROS2 Driver
+   * - joint-position-based control
+     - yes
+   * - scaled joint-position-based control
+     - yes
+   * - joint-velocity-based control
+     - yes\ :raw-html-m2r:`<sup>1</sup>`
+   * - Cartesian position-based control
+     - no
+   * - Cartesian twist-based control
+     - no
+   * - Trajectory forwarding for execution on robot
+     - no
+   * - reporting of tcp wrench
+     - yes
+   * - pausing of programs
+     - yes
+   * - continue trajectories after EM-Stop resume
+     - yes
+   * - continue trajectories after protective stop
+     - yes
+   * - panel interaction in between possible
+     - yes
+   * - get and set IO states
+     - yes
+   * - use `tool communication forwarder <https://github.com/UniversalRobots/Universal_Robots_ToolComm_Forwarder_URCap>`_ on e-series
+     - yes
+   * - use the driver without a teach pendant necessary
+     - yes
+   * - support of CB1 and CB2 robots
+     - no
+   * - trajectory extrapolation on robot on missing packages
+     - yes
+   * - use ROS as drop-in for TP-programs
+     - yes
+   * - headless mode
+     - yes
+   * - extract calibration from robot
+     - yes
+   * - send custom script commands to robot
+     - no
+   * - Reconnect on a disconnected robot
+     - yes
+
+
+:raw-html-m2r:`<sup>1</sup>` Velocity-based joint control is implemented in the driver, but the current version of ros2_control do not yet support Velocity-based joint trajectory control

doc/index.rst

@@ -0,0 +1,26 @@
+.. ur_robot_driver documentation master file, created by
+   sphinx-quickstart on Fri Apr  8 13:58:02 2022.
+   You can adapt this file completely to your liking, but it should at least
+   contain the root ``toctree`` directive.
+
+Welcome to ur_robot_driver's documentation!
+===========================================
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Contents:
+
+   overview
+   installation/toc
+   usage
+   setup_tool_communication
+   ROS_INTERFACE
+   generated/index
+   robot_state_helper
+   controller_stopper
+
+
+Indices and tables
+==================
+
+* :ref:`genindex`

doc/installation/install_urcap_cb3.rst

@@ -0,0 +1,67 @@
+.. _install-urcap-cb3:
+
+Installing a URCap on a CB3 robot
+=================================
+
+For using the *ur_robot_driver* with a real robot you need to install the
+**externalcontrol-1.0.5.urcap** which can be found inside the **resources** folder of this driver.
+
+**Note**\ : For installing this URCap a minimal PolyScope version of 3.7 is necessary.
+
+To install it you first have to copy it to the robot's **programs** folder which can be done either
+via scp or using a USB stick.
+
+On the welcome screen select *Setup Robot* and then *URCaps* to enter the URCaps installation
+screen.
+
+
+.. image:: initial_setup_images/cb3_01_welcome.png
+   :target: initial_setup_images/cb3_01_welcome.png
+   :alt: Welcome screen of a CB3 robot
+
+
+There, click the little plus sign at the bottom to open the file selector. There you should see
+all urcap files stored inside the robot's programs folder or a plugged USB drive.  Select and open
+the **externalcontrol-1.0.5.urcap** file and click *open*. Your URCaps view should now show the
+**External Control** in the list of active URCaps and a notification to restart the robot. Do that
+now.
+
+
+.. image:: initial_setup_images/cb3_05_urcaps_installed.png
+   :target: initial_setup_images/cb3_05_urcaps_installed.png
+   :alt: URCaps screen with installed urcaps
+
+
+After the reboot you should find the **External Control** URCaps inside the *Installation* section.
+For this select *Program Robot* on the welcome screen, select the *Installation* tab and select
+**External Control** from the list.
+
+
+.. image:: initial_setup_images/cb3_07_installation_excontrol.png
+   :target: initial_setup_images/cb3_07_installation_excontrol.png
+   :alt: Installation screen of URCaps
+
+
+Here you'll have to setup the IP address of the external PC which will be running the ROS driver.
+Note that the robot and the external PC have to be in the same network, ideally in a direct
+connection with each other to minimize network disturbances. The custom port should be left
+untouched for now.
+
+
+.. image:: initial_setup_images/cb3_10_prog_structure_urcaps.png
+   :target: initial_setup_images/cb3_10_prog_structure_urcaps.png
+   :alt: Insert the external control node
+
+
+To use the new URCaps, create a new program and insert the **External Control** program node into
+the program tree
+
+
+.. image:: initial_setup_images/cb3_11_program_view_excontrol.png
+   :target: initial_setup_images/cb3_11_program_view_excontrol.png
+   :alt: Program view of external control
+
+
+If you click on the *command* tab again, you'll see the settings entered inside the *Installation*.
+Check that they are correct, then save the program. Your robot is now ready to be used together with
+this driver.

doc/installation/install_urcap_e_series.rst

@@ -0,0 +1,66 @@
+.. _install-urcap-e-series:
+
+Installing a URCap on a e-Series robot
+======================================
+
+For using the *ur_robot_driver* with a real robot you need to install the
+**externalcontrol-1.0.5.urcap** which can be found inside the **resources** folder of this driver.
+
+**Note**\ : For installing this URCap a minimal PolyScope version of 5.1 is necessary.
+
+To install it you first have to copy it to the robot's **programs** folder which can be done either
+via scp or using a USB stick.
+
+On the welcome screen click on the hamburger menu in the top-right corner and select *Settings* to enter the robot's setup.  There select *System* and then *URCaps* to enter the URCaps installation screen.
+
+
+.. image:: initial_setup_images/es_01_welcome.png
+   :target: initial_setup_images/es_01_welcome.png
+   :alt: Welcome screen of an e-Series robot
+
+
+There, click the little plus sign at the bottom to open the file selector. There you should see
+all urcap files stored inside the robot's programs folder or a plugged USB drive.  Select and open
+the **externalcontrol-1.0.5.urcap** file and click *open*. Your URCaps view should now show the
+**External Control** in the list of active URCaps and a notification to restart the robot. Do that
+now.
+
+
+.. image:: initial_setup_images/es_05_urcaps_installed.png
+   :target: initial_setup_images/es_05_urcaps_installed.png
+   :alt: URCaps screen with installed urcaps
+
+
+After the reboot you should find the **External Control** URCaps inside the *Installation* section.
+For this select *Program Robot* on the welcome screen, select the *Installation* tab and select
+**External Control** from the list.
+
+
+.. image:: initial_setup_images/es_07_installation_excontrol.png
+   :target: initial_setup_images/es_07_installation_excontrol.png
+   :alt: Installation screen of URCaps
+
+
+Here you'll have to setup the IP address of the external PC which will be running the ROS driver.
+Note that the robot and the external PC have to be in the same network, ideally in a direct
+connection with each other to minimize network disturbances. The custom port should be left
+untouched for now.
+
+
+.. image:: initial_setup_images/es_10_prog_structure_urcaps.png
+   :target: initial_setup_images/es_10_prog_structure_urcaps.png
+   :alt: Insert the external control node
+
+
+To use the new URCaps, create a new program and insert the **External Control** program node into
+the program tree
+
+
+.. image:: initial_setup_images/es_11_program_view_excontrol.png
+   :target: initial_setup_images/es_11_program_view_excontrol.png
+   :alt: Program view of external control
+
+
+If you click on the *command* tab again, you'll see the settings entered inside the *Installation*.
+Check that they are correct, then save the program. Your robot is now ready to be used together with
+this driver.

doc/installation/installation.rst

@@ -0,0 +1,71 @@
+Installation of the ur_robot_driver
+===================================
+
+You can either install this driver from binary packages as shown above or build it from source. We
+recommend a binary package installation unless you want to join development and submit changes.
+
+Install from binary packages
+----------------------------
+
+1. `Install ROS2 <https://docs.ros.org/en/rolling/Installation/Ubuntu-Install-Debians.html>`_. This
+   branch supports only ROS2 Rolling. For other ROS2 versions, please see the respective branches.
+2. Install the driver using
+
+   .. code-block:: bash
+
+     sudo apt-get install ros-${ROS_DISTRO}-ur
+
+
+Build from source
+-----------------
+
+Before building from source please make sure that you actually need to do that. Building from source
+might require some special treatment, especially when it comes to dependency management.
+Dependencies might change from time to time. Upstream packages (such as the library) might change
+their features / API which require changes in this repo. Therefore, this repo's source builds might
+require upstream repositories to be present in a certain version as otherwise builds might fail.
+Starting from scratch following exactly the steps below should always work, but simply pulling and
+building might fail occasionally.
+
+1. `Install ROS2 <https://docs.ros.org/en/rolling/Installation/Ubuntu-Install-Debians.html>`_. This
+   branch supports only ROS2 Rolling. For other ROS2 versions, please see the respective branches.
+
+   Once installed, please make sure to actually `source ROS2 <https://docs.ros.org/en/rolling/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html#source-the-setup-files>`_ before proceeding.
+
+3. Make sure that ``colcon``, its extensions and ``vcs`` are installed:
+
+   .. code-block:: bash
+
+     sudo apt install python3-colcon-common-extensions python3-vcstool
+
+
+4. Create a new ROS2 workspace:
+
+   .. code-block:: bash
+
+     export COLCON_WS=~/workspace/ros_ur_driver
+     mkdir -p $COLCON_WS/src
+
+5. Clone relevant packages (replace ``<branch>`` with ``humble``, ``iron`` or ``main`` for rolling), install dependencies, compile, and source the workspace by using:
+
+   .. code-block:: bash
+
+     cd $COLCON_WS
+     git clone -b <branch> https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git src/Universal_Robots_ROS2_Driver
+     vcs import src --skip-existing --input src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver-not-released.${ROS_DISTRO}.repos
+     rosdep update
+     rosdep install --ignore-src --from-paths src -y
+     colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
+     source install/setup.bash
+
+6. When consecutive pulls lead to build errors it is possible that you'll have to build an upstream
+   package from source, as well. See the [detailed build status](ci_status.md). When the binary builds are red, but
+   the semi-binary builds are green, you need to build the upstream dependencies from source. The
+   easiest way to achieve this, is using the repos file:
+
+   .. code-block:: bash
+
+     cd $COLCON_WS
+     vcs import src --skip-existing --input src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver.${ROS_DISTRO}.repos
+     rosdep update
+     rosdep install --ignore-src --from-paths src -y

doc/installation/real_time.rst

@@ -0,0 +1,334 @@
+.. _real time setup:
+
+Setup for real-time scheduling
+==============================
+
+In order to run the ``universal_robot_driver``, we highly recommend to setup a ubuntu system with
+real-time capabilities. Especially with a robot from the e-Series the higher control frequency
+might lead to non-smooth trajectory execution if not run using a real-time-enabled system.
+
+You might still be able to control the robot using a non-real-time system. This is, however, not recommended.
+
+While the best-performing strategy would be to use a real-time enabled kernel, using a lowlatency
+kernel has shown to be sufficient in many situations which is why this is also shown as an option
+here.
+
+Installing a lowlatency-kernel
+------------------------------
+
+Installing a lowlatency kernel is pretty straightforward:
+
+.. code-block:: console
+
+   $ sudo apt install linux-lowlatency
+
+Setting up Ubuntu with a PREEMPT_RT kernel
+------------------------------------------
+
+To get real-time support into a ubuntu system, the following steps have to be performed:
+
+#. Get the sources of a real-time kernel
+#. Compile the real-time kernel
+#. Setup user privileges to execute real-time tasks
+
+This guide will help you setup your system with a real-time kernel.
+
+Preparing
+^^^^^^^^^
+
+To build the kernel, you will need a couple of tools available on your system. You can install them
+using
+
+.. code-block:: console
+
+   $ sudo apt-get install build-essential bc ca-certificates gnupg2 libssl-dev wget gawk flex bison libelf-dev dwarves
+
+
+.. note::
+
+   For different kernel versions the dependencies might be different than that. If you experience
+   problems such as ``fatal error: liXYZ.h: No such file or directory`` during compilation, try to
+   install the library's corresponding ``dev``-package.
+
+Before you download the sources of a real-time-enabled kernel, check the kernel version that is currently installed:
+
+.. code-block:: console
+
+   $ uname -r
+   5.15.0-107-generic
+
+To continue with this tutorial, please create a temporary folder and navigate into it. You should
+have sufficient space (around 25GB) there, as the extracted kernel sources take much space. After
+the new kernel is installed, you can delete this folder again.
+
+In this example we will use a temporary folder inside our home folder:
+
+.. code-block:: console
+
+   $ mkdir -p ${HOME}/rt_kernel_build
+   $ cd ${HOME}/rt_kernel_build
+
+All future commands are expected to be run inside this folder. If the folder is different, the ``$``
+sign will be prefixed with a path relative to the above folder.
+
+Getting the sources for a real-time kernel
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+To build a real-time kernel, we first need to get the kernel sources and the real-time patch.
+
+First, we must decide on the kernel version that we want to use. Above, we
+determined that our system has a 5.15 kernel installed. However, real-time
+patches exist only for selected kernel versions. Those can be found on the
+`linuxfoundation wiki <https://wiki.linuxfoundation.org/realtime/preempt_rt_versions>`_.
+
+In this example, we will select a 5.15.158 kernel with RT patch version 76. Select a kernel version close  to the
+one installed on your system. For easier reference later on we will export version information to
+our shell environment. Make sure to execute all following commands in this shell.
+
+.. code-block:: console
+
+   $ export KERNEL_MAJOR_VERSION=5
+   $ export KERNEL_MINOR_VERSION=15
+   $ export KERNEL_PATCH_VERSION=158
+   $ export RT_PATCH_VERSION=76
+   $ export KERNEL_VERSION="$KERNEL_MAJOR_VERSION.$KERNEL_MINOR_VERSION.$KERNEL_PATCH_VERSION"
+
+Go ahead and download the kernel sources, patch sources and their signature files:
+
+.. code-block:: console
+
+   $ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/$KERNEL_MAJOR_VERSION.$KERNEL_MINOR_VERSION/patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.xz
+   $ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/$KERNEL_MAJOR_VERSION.$KERNEL_MINOR_VERSION/patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.sign
+   $ wget https://www.kernel.org/pub/linux/kernel/v$KERNEL_MAJOR_VERSION.x/linux-$KERNEL_VERSION.tar.xz
+   $ wget https://www.kernel.org/pub/linux/kernel/v$KERNEL_MAJOR_VERSION.x/linux-$KERNEL_VERSION.tar.sign
+
+To unzip the downloaded files do
+
+.. code-block:: console
+
+   $ xz -dk patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.xz
+   $ xz -d linux-$KERNEL_VERSION.tar.xz
+
+Verification
+~~~~~~~~~~~~
+
+Technically, you can skip this section, it is however highly recommended to verify the file
+integrity of such a core component of your system!
+
+To verify file integrity, you must first import public keys by the kernel developers and the patch
+author. For the kernel sources use (as suggested on
+`kernel.org <https://www.kernel.org/signature.html>`_\ )
+
+.. code-block:: console
+
+   $ gpg2 --locate-keys torvalds@kernel.org gregkh@kernel.org
+
+and for the patch view the gpg information
+
+.. code-block:: console
+
+   $ gpg2 --verify patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.sign
+   gpg: assuming signed data in 'patch-5.15.158-rt76.patch'
+   gpg: Signature made Fri May  3 17:12:45 2024 UTC
+   gpg:                using RSA key AD85102A6BE1CDFE9BCA84F36CEF3D27CA5B141E
+   gpg: Can't check signature: No public key
+
+So, we need to import the key using
+
+.. code-block:: console
+
+   gpg2 --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys AD85102A6BE1CDFE9BCA84F36CEF3D27CA5B141E
+
+
+Now we can verify the downloaded sources:
+
+.. code-block:: console
+
+   $ gpg2 --verify linux-$KERNEL_VERSION.tar.sign
+   gpg: assuming signed data in 'linux-5.15.158.tar'
+   gpg: Signature made Thu May  2 14:28:07 2024 UTC
+   gpg:                using RSA key 647F28654894E3BD457199BE38DBBDC86092693E
+   gpg: Good signature from "Greg Kroah-Hartman <gregkh@kernel.org>" [unknown]
+   gpg: WARNING: This key is not certified with a trusted signature!
+   gpg:          There is no indication that the signature belongs to the owner.
+   Primary key fingerprint: 647F 2865 4894 E3BD 4571  99BE 38DB BDC8 6092 693E
+
+and
+
+.. code-block:: console
+
+   $ gpg2 --verify patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.sign
+   gpg: assuming signed data in 'patch-5.15.158-rt76.patch'
+   gpg: Signature made Fri May  3 17:12:45 2024 UTC
+   gpg:                using RSA key AD85102A6BE1CDFE9BCA84F36CEF3D27CA5B141E
+   gpg: Good signature from "Joseph Salisbury <joseph.salisbury@canonical.com>" [unknown]
+   gpg:                 aka "Joseph Salisbury <josephtsalisbury@gmail.com>" [unknown]
+   gpg:                 aka "Joseph Salisbury <joseph.salisbury@ubuntu.com>" [unknown]
+   gpg: WARNING: This key is not certified with a trusted signature!
+   gpg:          There is no indication that the signature belongs to the owner.
+   Primary key fingerprint: AD85 102A 6BE1 CDFE 9BCA  84F3 6CEF 3D27 CA5B 141E
+
+
+Compilation
+^^^^^^^^^^^
+
+Before we can compile the sources, we have to extract the tar archive and apply the patch
+
+.. code-block:: console
+
+   $ tar xf linux-$KERNEL_VERSION.tar
+   $ cd linux-$KERNEL_VERSION
+   $ xzcat ../patch-$KERNEL_VERSION-rt$RT_PATCH_VERSION.patch.xz | patch -p1
+
+Now to configure your kernel, just type
+
+.. code-block:: console
+
+   $ make oldconfig
+
+This will ask for kernel options. For everything else then the ``Preemption Model`` use the default
+value (just press Enter) or adapt to your preferences. For the preemption model select ``Fully Preemptible Kernel``\ :
+
+.. code-block:: console
+
+   Preemption Model
+     1. No Forced Preemption (Server) (PREEMPT_NONE)
+   > 2. Voluntary Kernel Preemption (Desktop) (PREEMPT_VOLUNTARY)
+     3. Preemptible Kernel (Low-Latency Desktop) (PREEMPT)
+     4. Fully Preemptible Kernel (Real-Time) (PREEMPT_RT) (NEW)
+   choice[1-4?]: 4
+
+On newer kernels you need to disable some key checking:
+
+.. code-block:: console
+
+   $ scripts/config --disable SYSTEM_TRUSTED_KEYS
+   $ scripts/config --disable SYSTEM_REVOCATION_KEYS
+
+Now you can build the kernel. This will take some time...
+
+.. code-block:: console
+
+   $ make -j $(getconf _NPROCESSORS_ONLN) deb-pkg
+
+After building, install the ``linux-headers`` and ``linux-image`` packages in the parent folder (only
+the ones without the ``-dbg`` in the name)
+
+.. code-block:: console
+
+   $ sudo apt install ../linux-headers-$KERNEL_VERSION-rt$RT_PATCH_VERSION*.deb \
+                      ../linux-image-$KERNEL_VERSION-rt$RT_PATCH_VERSION*.deb
+
+Setup user privileges to use real-time scheduling
+-------------------------------------------------
+
+To be able to schedule threads with user privileges (what the driver will do) you'll have to change
+the user's limits by changing ``/etc/security/limits.conf`` (See `the manpage <https://manpages.ubuntu.com/manpages/bionic/man5/limits.conf.5.html>`_ for details)
+
+We recommend to setup a group for real-time users instead of writing a fixed username into the config
+file:
+
+.. code-block:: console
+
+   $ sudo groupadd realtime
+   $ sudo usermod -aG realtime $(whoami)
+
+Then, make sure ``/etc/security/limits.conf`` contains
+
+.. code-block:: linuxconfig
+
+   @realtime soft rtprio 99
+   @realtime soft priority 99
+   @realtime soft memlock 102400
+   @realtime hard rtprio 99
+   @realtime hard priority 99
+   @realtime hard memlock 102400
+
+Note: You will have to log out and log back in (Not only close your terminal window) for these
+changes to take effect. No need to do this now, as we will reboot later on, anyway.
+
+Setup GRUB to always boot the lowlatency / real-time kernel
+-----------------------------------------------------------
+
+To make the new kernel the default kernel that the system will boot into every time, you'll have to
+change the grub config file inside ``/etc/default/grub``.
+
+Note: This works for ubuntu, but might not be working for other linux systems. It might be necessary
+to use another menuentry name there.
+
+But first, let's find out the name of the entry that we will want to make the default. You can list
+all available kernels using
+
+.. code-block:: console
+
+   $ awk -F\' '/menuentry |submenu / {print $1 $2}' /boot/grub/grub.cfg
+   menuentry Ubuntu
+   submenu Advanced options for Ubuntu
+           menuentry Ubuntu, with Linux 5.15.158-rt76
+           menuentry Ubuntu, with Linux 5.15.158-rt76 (recovery mode)
+           menuentry Ubuntu, with Linux 5.15.0-107-lowlatency
+           menuentry Ubuntu, with Linux 5.15.0-107-lowlatency (recovery mode)
+           menuentry Ubuntu, with Linux 5.15.0-107-generic
+           menuentry Ubuntu, with Linux 5.15.0-107-generic (recovery mode)
+
+From the output above, we'll need to generate a string with the pattern ``"submenu_name>entry_name"``. In our case this would be
+
+.. code-block:: text
+
+   "Advanced options for Ubuntu>Ubuntu, with Linux 5.15.158-rt76"
+
+**The double quotes and no spaces around the** ``>`` **are important!**
+
+With this, we can setup the default grub entry and then update the grub menu entries. Don't forget this last step!
+
+.. code-block:: console
+
+   $ sudo sed -i "s/^GRUB_DEFAULT=.*/GRUB_DEFAULT=\"Advanced options for Ubuntu>Ubuntu, with Linux ${KERNEL_VERSION}-rt${RT_PATCH_VERSION}\"/" /etc/default/grub
+   $ sudo update-grub
+
+Reboot the PC
+-------------
+
+After having performed the above mentioned steps, reboot the PC. It should boot into the correct
+kernel automatically.
+
+Check for preemption capabilities
+---------------------------------
+
+Make sure that the kernel does indeed support real-time scheduling:
+
+.. code-block:: console
+
+   $ uname -v | cut -d" " -f1-4
+   #1 SMP PREEMPT_RT Tue
+
+Optional: Disable CPU speed scaling
+-----------------------------------
+
+Many modern CPUs support changing their clock frequency dynamically depending on the currently
+requested computation resources. In some cases this can lead to small interruptions in execution.
+While the real-time scheduled controller thread should be unaffected by this, any external
+components such as a visual servoing system might be interrupted for a short period on scaling
+changes.
+
+To check and modify the power saving mode, install cpufrequtils:
+
+.. code-block:: console
+
+   $ sudo apt install cpufrequtils
+
+Run ``cpufreq-info`` to check available "governors" and the current CPU Frequency (\ ``current CPU
+frequency is XXX MHZ``\ ). In the following we will set the governor to "performance".
+
+.. code-block:: console
+
+   $ sudo systemctl disable ondemand
+   $ sudo systemctl enable cpufrequtils
+   $ sudo sh -c 'echo "GOVERNOR=performance" > /etc/default/cpufrequtils'
+   $ sudo systemctl daemon-reload && sudo systemctl restart cpufrequtils
+
+This disables the ``ondemand`` CPU scaling daemon, creates a ``cpufrequtils`` config file and restarts
+the ``cpufrequtils`` service. Check with ``cpufreq-info``.
+
+For further information about governors, please see the `kernel
+documentation <https://www.kernel.org/doc/Documentation/cpu-freq/governors.txt>`_.

doc/installation/robot_setup.rst

@@ -0,0 +1,88 @@
+
+Setting up a UR robot for ur_robot_driver
+=========================================
+
+Network setup
+-------------
+
+There are many possible ways to connect a UR robot. This section describes a good example using static IP addresses and a direct connection from the PC to the Robot to minimize latency introduced by network hardware. Though a good network switch usually works fine, as well.
+
+
+#.
+   Connect the UR control box directly to the remote PC with an ethernet cable.
+
+#.
+   Open the network settings from the UR teach pendant (Setup Robot -> Network) and enter these settings:
+
+.. code-block::
+
+   IP address: 192.168.1.102
+   Subnet mask: 255.255.255.0
+   Default gateway: 192.168.1.1
+   Preferred DNS server: 192.168.1.1
+   Alternative DNS server: 0.0.0.0
+
+
+#.
+   On the remote PC, turn off all network devices except the "wired connection", e.g. turn off wifi.
+
+#.
+   Open Network Settings and create a new Wired connection with these settings. You may want to name this new connection ``UR`` or something similar:
+
+.. code-block::
+
+   IPv4
+   Manual
+   Address: 192.168.1.101
+   Netmask: 255.255.255.0
+   Gateway: 192.168.1.1
+
+
+#. Verify the connection from the PC with e.g. ping.
+
+.. code-block::
+
+   ping 192.168.1.102
+
+Prepare the robot
+-----------------
+
+This section describes installation and launching of the URCap program from the pendant. It allows ROS to control the robot externally. Generally, you will launch the driver via ROS then start URCap from the pendant.
+
+For using the *ur_robot_driver* with a real robot you need to install the
+**externalcontrol urcap**. The latest release can be downloaded from `its own repository <https://github.com/UniversalRobots/Universal_Robots_ExternalControl_URCap/releases>`_.
+
+**Note**: For installing this URCap a minimal PolyScope version of 3.7 or 5.1 (in case of e-Series) is
+necessary.
+
+For installing the necessary URCap and creating a program, please see the individual tutorials on
+how to :ref:`setup a cb3 robot <install-urcap-cb3>` or how to
+:ref:`setup an e-Series robot <install-urcap-e-series>`.
+
+To setup the tool communication on an e-Series robot, please consider the :ref:`tool communication setup
+guide <setup-tool-communication>`.
+
+Prepare the ROS PC
+------------------
+
+For using the driver make sure it is installed (either by the debian package or built from source
+inside a colcon workspace).
+
+Extract calibration information
+-------------------------------
+
+Each UR robot is calibrated inside the factory giving exact forward and inverse kinematics. To also
+make use of this in ROS, you first have to extract the calibration information from the robot.
+
+Though this step is not necessary to control the robot using this driver, it is highly recommended
+to do so, as otherwise endeffector positions might be off in the magnitude of centimeters.
+
+For this, there exists a helper script:
+
+.. code:: bash
+
+   $ ros2 launch ur_calibration calibration_correction.launch.py \
+   robot_ip:=<robot_ip> target_filename:="${HOME}/my_robot_calibration.yaml"
+
+For the parameter ``robot_ip`` insert the IP address on which the ROS pc can reach the robot. As
+``target_filename`` provide an absolute path where the result will be saved to.

doc/installation/toc.rst

@@ -0,0 +1,17 @@
+############
+Installation
+############
+
+This chapter explains how to install the ``ur_robot_driver``
+
+
+.. toctree::
+   :maxdepth: 4
+   :caption: Contents:
+
+   installation
+   real_time
+   robot_setup
+   install_urcap_cb3
+   install_urcap_e_series
+   ursim_docker

doc/installation/ursim_docker.rst

@@ -0,0 +1,99 @@
+.. _ursim_docker:
+
+Setup URSim with Docker
+=======================
+URSim is the offline simulator by Universal Robots. Packed into a remote or virtual machine it acts almost
+identically to a real robot connected over the network. While it is possible to get URSim running
+locally on a Linux system or inside a VirtualBox virtual machine, we will focus on getting things
+setup using Docker. Using Docker for your simulated robot allows you to very quickly spin up a robot
+testing instance with very little computational overload.
+
+This guide will assume that you have Docker already installed and setup such that you can startup
+Docker containers using your current user.
+
+Start a URSim docker container
+------------------------------
+
+To startup a simulated robot run the following command. This will start a Docker container named
+``ursim`` and startup a simulated UR5e robot. It exposes ports 5900 and 6080 for the browser-based
+polyscope access. Note that this will expose the simulated robot to your local area network if you
+don't have any further level of security such as a firewall active. To prevent this, you can either
+skip the port forwarding instructions (skip the two ``-p port:port`` statements) in which case
+you'll have to use the container's IP address to access the polyscope gui rather than ``localhost`` or
+you can restrict the port forwarding to a certain network interface (such as the looppack interface)
+see Docker's upstream documentation on port exposure for further information.
+
+.. code-block:: bash
+
+   docker run --rm -it -p 5900:5900 -p 6080:6080 --name ursim universalrobots/ursim_e-series
+
+External Control
+----------------
+
+To use the external control functionality, we will need the ``external_control`` URCap installed on
+the robot and a program containing its *ExternalControl* program node. Both can be prepared on the
+host machine either by creating an own Dockerfile containing those or by mounting two folders
+containing installed URCaps and programs. See the Dockerfile's upstream `documentation <https://hub.docker.com/r/universalrobots/ursim_e-series>`_.
+
+In this example, we will bind-mount a folder for the programs and URCaps. First, let's create a
+local folder where we can store things inside:
+
+.. code-block:: bash
+
+   mkdir -p ${HOME}/.ursim/programs
+   mkdir -p ${HOME}/.ursim/urcaps
+
+Then, we can "install" the URCap by placing its ``.jar`` file inside the urcaps folder
+
+.. code-block:: bash
+
+   URCAP_VERSION=1.0.5 # latest version as if writing this
+   curl -L -o ${HOME}/.ursim/urcaps/externalcontrol-${URCAP_VERSION}.jar \
+     https://github.com/UniversalRobots/Universal_Robots_ExternalControl_URCap/releases/download/v${URCAP_VERSION}/externalcontrol-${URCAP_VERSION}.jar
+
+With this, start your URSim containers with the following command:
+
+.. code-block:: bash
+
+   docker run --rm -it -p 5900:5900 -p 6080:6080 -v ${HOME}/.ursim/urcaps:/urcaps -v ${HOME}/.ursim/programs:/ursim/programs --name ursim universalrobots/ursim_e-series
+
+With this, you should be able to setup the ``external_control`` URCap and create a program as
+described in :ref:`URCap setup guide <install-urcap-e-series>`.
+
+Network setup
+-------------
+
+As described above, you can always start the URSim container using the default network setup. As long
+as you don't have any other docker containers running, it will most probably always get the same IP
+address assigned every time. However, to make things a bit more explicit, we can setup our own
+docker network where we can assign a static IP address to our URSim container.
+
+.. code-block:: bash
+
+   docker network create --subnet=192.168.56.0/24 ursim_net
+   docker run --rm -it -p 5900:5900 -p 6080:6080 --net ursim_net --ip 192.168.56.101 universalrobots/ursim_e-series
+
+The above commands first create a network for docker and then create a container with the URSim
+image attaching to this network.
+
+As we now have a fixed IP address we can also skip the port exposure as we know the robot's IP
+address. The VNC web server will be available at `<http://192.168.56.101:6080/vnc.html>`_
+
+Script startup
+--------------
+
+All of the above is put together in a script in the ``ur_client_library`` package.
+
+.. code-block:: bash
+
+   ros2 run ur_client_library start_ursim.sh
+
+This will start a URSim docker container running on ``192.168.56.101`` with the ``external_control``
+URCap preinstalled. Created programs and installation changes will be stored persistently inside
+``${HOME}/.ursim/programs``.
+
+With this, you can run
+
+.. code-block:: bash
+
+   ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101

doc/make.bat

@@ -0,0 +1,35 @@
+@ECHO OFF
+
+pushd %~dp0
+
+REM Command file for Sphinx documentation
+
+if "%SPHINXBUILD%" == "" (
+	set SPHINXBUILD=sphinx-build
+)
+set SOURCEDIR=.
+set BUILDDIR=_build
+
+if "%1" == "" goto help
+
+%SPHINXBUILD% >NUL 2>NUL
+if errorlevel 9009 (
+	echo.
+	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+	echo.installed, then set the SPHINXBUILD environment variable to point
+	echo.to the full path of the 'sphinx-build' executable. Alternatively you
+	echo.may add the Sphinx directory to PATH.
+	echo.
+	echo.If you don't have Sphinx installed, grab it from
+	echo.https://sphinx-doc.org/
+	exit /b 1
+)
+
+%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS%
+goto end
+
+:help
+%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS%
+
+:end
+popd

doc/overview.rst

@@ -0,0 +1,22 @@
+Overview
+========
+
+This driver collaborates closely with other ROS packages:
+
+``ur_bringup``
+  Package to launch the driver, simulated robot and test scripts.
+``ur_calibration``
+  Package containing the calibration extraction program that will extract parameters for correctly
+  parametrizing the URDF with calibration data from the specific robot.
+``ur_controllers``
+  Controllers specifically made for UR manipulators.
+``ur_dashboard_msgs``
+  Message packages used for the `dashboard <https://www.universal-robots.com/articles/ur/dashboard-server-e-series-port-29999/>`_ communication.
+``ur_moveit_config``
+  MoveIt! configuration for a plain robot. This is good as a starting point, but for a real
+  application you would rather create your own MoveIt! configuration package containing your actual
+  robot environment.
+``ur_description`` (`separate repository <https://github.com/UniversalRobots/Universal_Robots_ROS2_Description>`_)
+  URDF description for UR manipulators
+
+.. include:: features.rst

doc/resources/README.md

@@ -0,0 +1,46 @@
+# Resources about the ROS 2 Universal Robot driver
+
+The files in this folder are available for use under BSD-3-Clause license.
+
+
+## Diagrams and images
+
+- [ros2_control diagrams](ros2_control_ur_driver.drawio) - `.drawio` file, Author: [Dr.-Ing. Denis Stogl](mailto:denis@stoglrobotics.de) (PickNik Robotics)
+
+
+## 2021-10 ROS World presentation
+
+[Presentation: Making a robot ROS 2 powered - a case study using the UR manipulators](2021-10_ROS_World_2021_Making_a_robot_ROS2_powered.pdf)
+
+**Summary:**
+
+With the release of ros2_control and MoveIt 2, ROS 2 Foxy finally has all the “ingredients” needed to power a robot with similar features as in ROS 1. We present the driver for Universal Robot’s manipulators as a real-world example of how robots can be run using ROS 2. We show how to realize multi-interface support for position and velocity commands in the driver and how to support scaling controllers while respecting factors set on the teach pendant. Finally, we show how this real-world example influences development of ros2_control to support non-joint related inputs and outputs in its real-time control loop.
+
+**Presenter:** *Dr.-Ing. Denis Štogl*
+
+**Authors:**
+  - Dr.-Ing. Denis Štogl (PickNik Inc.)
+  - Dr. Nathan Brooks (PickNik Inc.)
+  - Lovro Ivanov (PickNik Inc.)
+  - Dr. Andy Zelenak (PickNik Inc.)
+  - Rune Søe-Knudsen (Universal Robots A/S)
+
+#### [Video] Presentation recording
+[![Recording](https://i.vimeocdn.com/video/1309381824-05663ce76ceac80f043bc50addcc7c4874da323145c0df0df_640)](https://vimeo.com/649651707)
+
+
+#### [Video] MoveIt2 Demo
+[![Video: MoveIt2 Demo](https://img.youtube.com/vi/d_cVXoZZ52w/0.jpg)](https://www.youtube.com/watch?v=d_cVXoZZ52w)
+
+This video shows free-space trajectory planning around a modeled collision scene object using the MoveIt2 MotionPlanning widget for Rviz2.
+
+#### [Video] Scaled Joint Trajectory Controller Demo
+[![Video: Scaled Joint Trajectory Controller Demo](https://img.youtube.com/vi/dHaxBpMGbw0/0.jpg)](https://www.youtube.com/watch?v=dHaxBpMGbw0)
+
+This video demonstrates the following features:
+- [[0:00](https://www.youtube.com/watch?v=dHaxBpMGbw0&t=0s)] - Previewing a plan with MoveIt2 MotionPlanning widget for Rviz2
+- [[0:05](https://www.youtube.com/watch?v=dHaxBpMGbw0&t=5s)] - Executing a plan with MoveIt2 MotionPlanning widget for Rviz2
+- [[0:09](https://www.youtube.com/watch?v=dHaxBpMGbw0&t=9s)] - Pendant speed scaling of a simulated MoveIt trajectory
+- [[0:18](https://www.youtube.com/watch?v=dHaxBpMGbw0&t=18s)] - Pendant speed scaling of an executed MoveIt trajectory
+- [[0:22](https://www.youtube.com/watch?v=dHaxBpMGbw0&t=22s)] - Online pendant speed scaling during trajectory execution
+- [[0:26](https://www.youtube.com/watch?v=dHaxBpMGbw0&t=26s)] - Resuming trajectory execution from E-stop

doc/robot_state_helper.rst

@@ -0,0 +1,59 @@
+.. _robot_state_helper:
+
+Robot state helper
+==================
+After switching on the robot, it has to be manually started, the brakes have to be released and a
+program has to be started in order to make the robot ready to use. This is usually done using the
+robot's teach pendant.
+
+Whenever the robot encounters an error, manual intervention is required to resolve the issue. For
+example, if the robot goes into a protective stop, the error has to be acknowledged and the robot
+program has to be unpaused.
+
+When the robot is in :ref:`remote_control_mode <operation_modes>`, most interaction with the robot can be done
+without using the teach pendant, many of that through the :ref:`dashboard client
+<dashboard_client_ros2>`.
+
+The ROS driver provides a helper node that can be used to automate some of these tasks. The
+``robot_state_helper`` node can be used to start the robot, release the brakes, and (re-)start the
+program through an action call. It is started by default and provides a
+`dashboard_msgs/action/SetMode
+<https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_dashboard_msgs/action/SetMode.action>`_ action.
+
+For example, to make the robot ready to be used by the ROS driver, call
+
+.. code-block:: console
+
+   $ ros2 action send_goal /ur_robot_state_helper/set_mode ur_dashboard_msgs/action/SetMode "{ target_robot_mode: 7, stop_program: true, play_program: true}"
+
+The ``target_robot_mode`` can be one of the following:
+
+.. table:: target_robot_mode
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   3      POWER_OFF -- Robot is powered off
+   5      IDLE -- Robot is powered on, but brakes are engaged
+   7      RUNNING -- Robot is powered on, brakes are released, ready to run a program
+   =====  =====
+
+.. note::
+
+   When the ROBOT_STATE is in ``RUNNING``, that is equivalent to the robot showing the green dot in
+   the lower left corner of the teach pendant (On PolyScope 5). The program state is independent of
+   that and shows with the text next to that button.
+
+The ``stop_program`` flag is used to stop the currently running program before changing the robot
+state. In combination with the :ref:`controller_stopper`, this will deactivate any motion
+controller and therefore stop any ROS action being active on those controllers.
+
+.. warning::
+   A robot's protective stop or emergency stop is only pausing the running program. If the program
+   is resumed after the P-Stop or EM-Stop is released, the robot will continue executing what it
+   has been doing. Therefore, it is advised to stop and re-start the program when recovering from a
+   fault.
+
+The ``play_program`` flag is used to start the program after the robot state has been set. This has
+the same effects as explained in :ref:`continuation_after_interruptions`.

doc/setup_tool_communication.rst

@@ -0,0 +1,76 @@
+.. _setup-tool-communication:
+
+Setting up the tool communication on an e-Series robot
+======================================================
+
+.. note::
+   Currently, there seems to be issues with having the tool communication setup from this guide and
+   the "Robotiq Grippers" URCap running in parallel. If you are planning to use the "Robotiq
+   Grippers" URCap then currently please refrain from installing the rs485 URCap.
+
+   See
+   `this issue <https://github.com/UniversalRobots/Universal_Robots_ToolComm_Forwarder_URCap/issues/9>`_
+   for details and the current state.
+
+The Universal Robots e-Series provides an rs485 based interface at the tool flange that can be used
+to attach an rs485-based device to the robot's tcp without the need to wire a separate cable along
+the robot.
+
+This driver enables forwarding this tool communication interface to an external machine for example
+to start a device's ROS driver on a remote PC.
+
+This document will guide you through installing the URCap needed for this and setting up your ROS
+launch files to utilize the robot's tool communication.
+
+Robot setup
+-----------
+
+For setting up the robot, please install the **rs485-1.0.urcap** found in the **resources** folder.
+Installing a URCap is explained in the :ref:`setup guide <install-urcap-e-series>` for the **external-control** URCap.
+
+After installing the URCap the robot will expose its tool communication device to the network.
+
+Setup the ROS side
+------------------
+
+In order to use the tool communication in ROS, simply pass the correct parameters to the bringup
+launch files:
+
+.. code-block:: bash
+
+   $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=yyy.yyy.yyy.yyy use_tool_communication:=true use_fake_hardware:=false launch_rviz:=false
+     # remember that your user needs to have the rights to write that file handle to /tmp/ttyUR
+
+Following parameters can be set `ur.ros2_control.xacro <https://github.com/UniversalRobots/Universal_Robots_ROS2_Description/blob/ros2/urdf/ur.ros2_control.xacro>`_\ :
+
+
+* ``tool_voltage``
+* ``tool_parity``
+* ``tool_baud_rate``
+* ``tool_stop_bits``
+* ``tool_rx_idle_chars``
+* ``tool_tx_idle_chars``
+* ``tool_device_name``
+* ``tool_tcp_port``
+
+The ``tool_device_name`` is an arbitrary name for the device file at which the device will be
+accessible in the local file system. Most ROS drivers for rs485 devices accept an argument to
+specify the device file path. With the example above you could run the ``rs485_node`` from the package
+``imaginary_drivers`` using the following command:
+
+.. code-block:: bash
+
+   $ ros2 run imaginary_drivers rs485_node --ros-args -p device:=/tmp/ttyUR
+
+You can basically choose any device name, but your user has to have the correct rights to actually
+create a new file handle inside this directory. Therefore, we didn't use the ``/dev`` folder in the
+example, as users usually don't have the access rights to create new files there.
+
+For all the other tool parameters seen above, please refer to the Universal Robots user manual.
+
+More information can be found at `Universal_Robots_ToolComm_Forwarder_URCap <https://github.com/UniversalRobots/Universal_Robots_ToolComm_Forwarder_URCap>`_.
+The convenience script for ``socat`` call is `here <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/scripts/tool_communication.py>`_ for ROS2 driver and can be run by:
+
+.. code-block:: bash
+
+   $ ros2 run ur_robot_driver tool_communication.py --ros-args -p robot_ip:=192.168.56.101 -p device_name:=/tmp/ttyUR

doc/usage.rst

@@ -0,0 +1,320 @@
+.. role:: raw-html-m2r(raw)
+   :format: html
+
+
+Usage
+=====
+
+Launch files
+------------
+
+For starting the driver there are two main launch files in the ``ur_robot_driver`` package.
+
+
+* ``ur_control.launch.py`` - starts ros2_control node including hardware interface, joint state broadcaster and a controller. This launch file also starts ``dashboard_client`` if real robot is used.
+* ``ur_dashboard_client.launch.py`` - start the dashboard client for UR robots.
+
+Also, there are predefined launch files for all supported types of UR robots.
+
+The arguments for launch files can be listed using ``ros2 launch ur_robot_driver <launch_file_name>.launch.py --show-args``.
+The most relevant arguments are the following:
+
+
+* ``ur_type`` (\ *mandatory* ) - a type of used UR robot (\ *ur3*\ , *ur3e*\ , *ur5*\ , *ur5e*\ , *ur10*\ , *ur10e*\ , or *ur16e*\ , *ur20*\ , *ur30*\ ).
+* ``robot_ip`` (\ *mandatory* ) - IP address by which the root can be reached.
+* ``use_fake_hardware`` (default: *false* ) - use simple hardware emulator from ros2_control.
+  Useful for testing launch files, descriptions, etc. See explanation below.
+* ``initial_positions`` (default: dictionary with all joint values set to 0) - Allows passing a dictionary to set the initial joint values for the fake hardware from `ros2_control <http://control.ros.org/>`_.  It can also be set from a yaml file with the ``load_yaml`` commands as follows:
+
+  .. code-block::
+
+     <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)}" / >
+
+  In this example, the **initial_positions_file** is a xacro argument that contains the absolute path to a yaml file. An example of the initial positions yaml file is as follows:
+
+  .. code-block::
+
+     elbow_joint: 1.158
+     shoulder_lift_joint: -0.953
+     shoulder_pan_joint: 1.906
+     wrist_1_joint: -1.912
+     wrist_2_joint: -1.765
+     wrist_3_joint: 0.0
+
+* ``fake_sensor_commands`` (default: *false* ) - enables setting sensor values for the hardware emulators.
+  Useful for offline testing of controllers.
+
+* ``robot_controller`` (default: *joint_trajectory_controller* ) - controller for robot joints to be started.
+  Available controllers:
+
+
+  * joint_trajectory_controller
+  * scaled_joint_trajectory_controller
+
+  Note: *joint_state_broadcaster*\ , *speed_scaling_state_broadcaster*\ , *force_torque_sensor_broadcaster*\ , and *io_and_status_controller* will always start.
+
+  *HINT* : list all loaded controllers using ``ros2 control list_controllers`` command.
+
+**NOTE**\ : The package can simulate hardware with the ros2_control ``FakeSystem``. This emulator enables an environment for testing of "piping" of hardware and controllers, as well as testing robot's descriptions. For more details see `ros2_control documentation <https://ros-controls.github.io/control.ros.org/>`_ for more details.
+
+Modes of operation
+------------------
+
+As mentioned in the last section the driver has two basic modes of operation: Using fake hardware or
+using real hardware(Or the URSim simulator, which is equivalent from the driver's perspective).
+Additionally, the robot can be simulated using
+`Gazebo <https://github.com/UniversalRobots/Universal_Robots_ROS2_Gazebo_Simulation>`_ or
+`ignition <https://github.com/UniversalRobots/Universal_Robots_ROS2_Ignition_Simulation>`_ but that's
+outside of this driver's scope.
+
+.. list-table::
+   :header-rows: 1
+
+   * - mode
+     - available controllers
+   * - fake_hardware
+     - :raw-html-m2r:`<ul><li>joint_trajectory_controller</li><li>forward_velocity_controller</li><li>forward_position_controller</li></ul>`
+   * - real hardware / URSim
+     - :raw-html-m2r:`<ul><li>joint_trajectory_controller</li><li>scaled_joint_trajectory_controller </li><li>forward_velocity_controller</li><li>forward_position_controller</li></ul>`
+
+
+Usage with official UR simulator
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The easiest way to use URSim is the `Docker
+image <https://hub.docker.com/r/universalrobots/ursim_e-series>`_ provided by Universal Robots (See
+`this link <https://hub.docker.com/r/universalrobots/ursim_cb3>`_ for a CB3-series image).
+
+To start it, we've prepared a script:
+
+.. code-block:: bash
+
+   ros2 run ur_client_library start_ursim.sh -m <ur_type>
+
+With this, we can spin up a driver using
+
+.. code-block:: bash
+
+   ros2 launch ur_robot_driver ur_control.launch.py ur_type:=<ur_type> robot_ip:=192.168.56.101 launch_rviz:=true
+
+You can view the polyscope GUI by opening `<http://192.168.56.101:6080/vnc.html>`_.
+
+When we now move the robot in Polyscope, the robot's RViz visualization should move accordingly.
+
+For details on the Docker image, please see the more detailed guide :ref:`here <ursim_docker>`.
+
+Example Commands for Testing the Driver
+---------------------------------------
+
+Allowed UR - Type strings: ``ur3``\ , ``ur3e``\ , ``ur5``\ , ``ur5e``\ , ``ur10``\ , ``ur10e``\ , ``ur16e``\ , ``ur20``, ``ur30``.
+
+1. Start hardware, simulator or mockup
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+
+* To do test with hardware, use:
+
+  .. code-block::
+
+     ros2 launch ur_robot_driver ur_control.launch.py ur_type:=<UR_TYPE> robot_ip:=<IP_OF_THE_ROBOT> launch_rviz:=true
+
+  For more details check the argument documentation with ``ros2 launch ur_robot_driver ur_control.launch.py --show-arguments``
+
+  After starting the launch file start the external_control URCap program from the pendant, as described above.
+
+* To do an offline test with URSim check details about it in `this section <#usage-with-official-ur-simulator>`_
+
+* To use mocked hardware(capability of ros2_control), use ``use_fake_hardware`` argument, like:
+
+  .. code-block::
+
+     ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=yyy.yyy.yyy.yyy use_fake_hardware:=true launch_rviz:=true
+
+  **NOTE**\ : Instead of using the global launch file for control stack, there are also prepeared launch files for each type of UR robots named. They accept the same arguments are the global one and are used by:
+
+  .. code-block::
+
+     ros2 launch ur_robot_driver <ur_type>.launch.py
+
+2. Sending commands to controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Before running any commands, first check the controllers' state using ``ros2 control list_controllers``.
+
+
+* Send some goal to the Joint Trajectory Controller by using a demo node from `ros2_control_demos <https://github.com/ros-controls/ros2_control_demos>`_ package by starting  the following command in another terminal:
+
+  .. code-block::
+
+     ros2 launch ur_robot_driver test_scaled_joint_trajectory_controller.launch.py
+
+  After a few seconds the robot should move.
+
+* To test another controller, simply define it using ``initial_joint_controller`` argument, for example when using fake hardware:
+
+  .. code-block::
+
+     ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=yyy.yyy.yyy.yyy initial_joint_controller:=joint_trajectory_controller use_fake_hardware:=true launch_rviz:=true
+
+  And send the command using demo node:
+
+  .. code-block::
+
+     ros2 launch ur_robot_driver test_joint_trajectory_controller.launch.py
+
+  After a few seconds the robot should move(or jump when using emulation).
+
+3. Using only robot description
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+If you just want to test description of the UR robots, e.g., after changes you can use the following command:
+
+.. code-block::
+
+   ros2 launch ur_description view_ur.launch.py ur_type:=ur5e
+
+Using MoveIt
+------------
+
+`MoveIt! <https://moveit.ros.org>`_ support is built-in into this driver already.
+
+Real robot / URSim
+^^^^^^^^^^^^^^^^^^
+
+To test the driver with the example MoveIt-setup, first start the driver as described
+`above <#start-hardware-simulator-or-mockup>`_.
+
+.. code-block::
+
+   ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e launch_rviz:=true
+
+Now you should be able to use the MoveIt Plugin in rviz2 to plan and execute trajectories with the
+robot as explained `here <https://moveit.picknik.ai/main/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_.
+
+.. note::
+   The MoveIt configuration provided here has Trajectory Execution Monitoring (TEM) *disabled*, as the
+   Scaled Joint Trajectory Controller may cause trajectories to be executed at a lower velocity
+   than they were originally planned by MoveIt. MoveIt's TEM however is not aware of this
+   deliberate slow-down due to scaling and will in most cases unnecessarily (and unexpectedly)
+   abort goals.
+
+   Until this incompatibility is resolved, the default value for ``execution_duration_monitoring``
+   is set to ``false``. Users who wish to temporarily (re)enable TEM at runtime (for use with
+   other, non-scaling controllers) can do so using the ROS 2 parameter services supported by
+   MoveIt.
+
+   .. literalinclude:: ../../ur_moveit_config/launch/ur_moveit.launch.py
+      :language: python
+      :start-at: trajectory_execution =
+      :end-at: execution_duration_monitoring": False
+      :append: }
+      :dedent: 4
+      :caption: ur_moveit_config/launch/ur_moveit.launch.py
+
+Fake hardware on ROS2 Galactic
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Currently, the ``scaled_joint_trajectory_controller`` does not work on ROS2 Galactic. There is an
+`upstream Merge-Request <https://github.com/ros-controls/ros2_control/pull/635>`_ pending to fix that. Until this is merged and released, please change the
+default controller in the `controllers.yaml <ur_moveit_config/config/controllers.yaml>`_ file. Make
+sure that the ``default`` field is assigned ``true`` for the ``joint_trajectory_controller`` and ``false``
+for the
+``scaled_joint_trajectory_controller``.
+
+.. code-block::
+
+   controller_names:
+     - scaled_joint_trajectory_controller
+     - joint_trajectory_controller
+
+
+   scaled_joint_trajectory_controller:
+     action_ns: follow_joint_trajectory
+     type: FollowJointTrajectory
+     default: false
+     joints:
+       - shoulder_pan_joint
+       - shoulder_lift_joint
+       - elbow_joint
+       - wrist_1_joint
+       - wrist_2_joint
+       - wrist_3_joint
+
+
+   joint_trajectory_controller:
+     action_ns: follow_joint_trajectory
+     type: FollowJointTrajectory
+     default: true
+     joints:
+       - shoulder_pan_joint
+       - shoulder_lift_joint
+       - elbow_joint
+       - wrist_1_joint
+       - wrist_2_joint
+       - wrist_3_joint
+
+Then start
+
+.. code-block::
+
+   ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=yyy.yyy.yyy.yyy use_fake_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller
+   # and in another shell
+   ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e launch_rviz:=true
+
+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"}'

doc/usage/controllers.rst

@@ -0,0 +1,127 @@
+Controllers
+===========
+
+This help page describes the different controllers available for the ``ur_robot_driver``. This
+should help users finding the right controller for their specific use case.
+
+Where are controllers defined?
+------------------------------
+
+Controllers are defined in the ``config/ur_controllers.yaml`` file.
+
+How do controllers get loaded and started?
+------------------------------------------
+
+As this driver uses `ros2_control <https://control.ros.org>`_ all controllers are managed by the
+controller_manager. During startup, a default set of running controllers is loaded and started,
+another set is loaded in stopped mode. Stopped controllers won't be usable right away, but they
+need to be started individually.
+
+Controllers that are actually writing to some command interfaces (e.g. joint positions) will claim
+those interfaces. Only one controller claiming a certain interface can be active at one point.
+
+Controllers can be switched either through the controller_manager's service calls, through the
+`rqt_controller_manager <https://control.ros.org/rolling/doc/ros2_control/controller_manager/doc/userdoc.html#rqt-controller-manager>`_ gui or through the ``ros2 control`` verb from the package ``ros-${ROS_DISTRO}-ros2controlcli`` package.
+
+For example, to switch from the default ``scaled_joint_trajectory_controller`` to the
+``forward_position_controller`` you can call
+
+.. code-block:: console
+
+   $ ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller \
+     --activate forward_position_controller
+   [INFO 2024-09-23 20:32:04.373] [_ros2cli_1207798]: waiting for service /controller_manager/switch_controller to become available...
+   Successfully switched controllers
+
+Read-only broadcasters
+----------------------
+
+These broadcasters are read-only. They read states from the robot and publish them on a ROS topic.
+As they are read-only, they don't claim any resources and can be combined freely. By default, they
+are all started and running. Those controllers do not require the robot to have the
+external_control script running.
+
+joint_state_broadcaster
+^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: `joint_state_broadcaster/JointStateBroadcaster <https://control.ros.org/rolling/doc/ros2_controllers/joint_state_broadcaster/doc/userdoc.html>`_
+
+Publishes all joints' positions, velocities, and motor currents as ``sensor_msgs/JointState`` on the ``joint_states`` topic.
+
+.. note::
+
+   The effort field contains the currents reported by the joints and not the actual efforts in a
+   physical sense.
+
+speed_scaling_state_broadcaster
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: :ref:`ur_controllers/SpeedScalingStateBroadcaster <speed_scaling_state_broadcaster>`
+
+This broadcaster publishes the current actual execution speed as reported by the robot. Values are
+floating points between 0 and 1.
+
+force_torque_sensor_broadcaster
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: `force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster <https://control.ros.org/rolling/doc/ros2_controllers/force_torque_sensor_broadcaster/doc/userdoc.html>`_
+
+Publishes the robot's wrench as reported from the controller.
+
+Commanding controllers
+----------------------
+
+The commanding controllers control the robot's motions. Those controllers can't be combined
+arbitrarily, as they will claim hardware resources. Only one controller can claim one hardware
+resource at a time.
+
+scaled_joint_trajectory_controller (Default motion controller)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: :ref:`ur_controllers/ScaledJointTrajectoryController <scaled_jtc>`
+
+Scaled version of the
+`joint_trajectory_controller
+<https://control.ros.org/master/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html>`_.
+It uses the robot's speed scaling information and thereby the safety compliance features, like pause on safeguard stop. In addition, it also makes it possible to adjust execution speed using the speed slider on the teach pendant or set the program in pause and restart it again.
+See it's linked documentation for details.
+
+io_and_status_controller
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: :ref:`ur_controllers/GPIOController <io_and_status_controller>`
+
+Allows setting I/O ports, controlling some UR-specific functionality and publishes status information about the robot.
+
+forward_velocity_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: `velocity_controllers/JointGroupVelocityController <https://control.ros.org/rolling/doc/ros2_controllers/position_controllers/doc/userdoc.html#position-controllers-jointgrouppositioncontroller>`_
+
+Allows setting target joint positions directly. The robot tries to reach the target position as
+fast as possible. The user is therefore responsible for sending commands that are achievable. This
+controller is particularly useful when doing servoing such as ``moveit_servo``.
+
+forward_position_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: `position_controllers/JointGroupPositionController <https://control.ros.org/rolling/doc/ros2_controllers/velocity_controllers/doc/userdoc.html#velocity-controllers-jointgroupvelocitycontroller>`_
+
+Allows setting target joint velocities directly. The user is responsible for sending commands that
+are achievable. This controller is particularly useful when doing servoing such as
+``moveit_servo``.
+
+force_mode_controller
+^^^^^^^^^^^^^^^^^^^^^
+
+Type: :ref:`ur_controllers/ForceModeController <force_mode_controller>`
+
+Allows utilizing the robot's builtin *Force Mode*.
+
+freedrive_mode_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Type: :ref:`ur_controllers/FreedriveModeController <freedrive_mode_controller>`
+
+Allows utilizing the robot's *Freedrive mode*, making it possible to manually move the robot's
+joints.

doc/usage/startup.rst

@@ -0,0 +1,142 @@
+.. _ur_robot_driver_startup:
+
+Startup the driver
+==================
+
+Prepare the robot
+-----------------
+
+If you want to use a real robot, or a URSim simulator, with this driver, you need to prepare it,
+first. Make sure that you complete all steps from the :ref:`setup instructions<robot_setup>`,
+installed the External Control URCap and created a program as explained
+:ref:`here<install_urcap>`.
+
+Launch files
+------------
+
+For starting the driver it is recommended to start the ``ur_control.launch.py`` launchfile from the
+``ur_robot_driver`` package. It starts the driver, a set of controllers and a couple of helper
+nodes for UR robots. The only required arguments are the ``ur_type`` and ``robot_ip`` parameters.
+
+.. code-block:: console
+
+   $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101
+
+Allowed ``ur_type`` strings: ``ur3``, ``ur3e``, ``ur5``, ``ur5e``, ``ur10``, ``ur10e``, ``ur16e``,
+``ur20``, ``ur30``.
+
+Other important arguments are:
+
+
+* ``kinematics_params_file`` (default: *None*) - Path to the calibration file extracted from the robot, as described in :ref:`calibration_extraction`.
+* ``use_mock_hardware`` (default: *false* ) - Use simple hardware emulator from ros2_control. Useful for testing launch files, descriptions, etc.
+* ``headless_mode`` (default: *false*) - Start driver in :ref:`headless_mode`.
+* ``launch_rviz`` (default: *true*) - Start RViz together with the driver.
+* ``initial_joint_controller`` (default: *scaled_joint_trajectory_controller*) - Use this if you
+  want to start the robot with another controller.
+
+  .. note::
+     When the driver is started, you can list all loaded controllers using the ``ros2 control
+     list_controllers`` command. For this, the package ``ros2controlcli`` must be installed (``sudo
+     apt-get install ros-${ROS_DISTRO}-ros2controlcli``).
+
+
+For all other arguments, please see
+
+
+.. code-block:: console
+
+   $ ros2 launch ur_robot_driver ur_control.launch.py --show-args
+
+Also, there are predefined launch files for all supported types of UR robots.
+
+.. _robot_startup_program:
+
+Finish startup on the robot
+---------------------------
+
+Unless :ref:`headless_mode` is used, you will now have to start the *External Control URCap* program on
+the robot that you have created earlier.
+
+Depending on the :ref:`robot control mode<operation_modes>` do the following:
+
+* In *local control mode*, load the program on the robot and press the "Play" button |play_button| on the teach pendant.
+* In *remote control mode* load and start the program using the following dashboard calls:
+
+  .. code-block:: console
+
+     $ ros2 service call /dashboard_client/load_program ur_dashboard_msgs/srv/Load "filename: my_robot_program.urp"``
+     $ ros2 service call /dashboard_client/play std_srvs/srv/Trigger {}
+
+* When the driver is started with ``headless_mode:=true`` nothing is needed. The driver is running
+  already.
+
+
+.. _verify_calibration:
+
+Verify calibration info is being used correctly
+-----------------------------------------------
+
+
+If you passed a path to an extracted calibration via the *kinematics_params_file*
+parameter, ensure that the loaded calibration matches that of the robot by inspecting the console
+output after launching the ``ur_robot_driver``. If the calibration does not match, you will see an error:
+
+.. code-block::
+
+  [INFO] [1694437690.406932381] [URPositionHardwareInterface]: Calibration checksum: 'calib_xxxxxxxxxxxxxxxxxxx'
+  [ERROR] [1694437690.516957265] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file.
+
+With the correct calibration you should see:
+
+.. code-block::
+
+  [INFO] [1694437690.406932381] [URPositionHardwareInterface]: Calibration checksum: 'calib_xxxxxxxxxxxxxxxxxxx'
+  [INFO] [1694437690.516957265] [URPositionHardwareInterface]: Calibration checked successfully.
+
+Alternatively, search for the term *checksum* in the console output after launching the driver.
+Verify that the printed checksum matches that on the final line of your extracted calibration file.
+
+
+.. _continuation_after_interruptions:
+
+Continuation after interruptions
+--------------------------------
+
+
+Whenever the *External Control URCap* program gets interrupted, it has to be unpaused / restarted.
+
+If that happens, you will see the output ``Connection to reverse interface dropped.``
+
+This can happen, e,g, when
+
+* The running program is actively stopped.
+* The robot goes into a protective stop / EM stop. (The program will be paused, then)
+* The communication is stopped, since the external source did not receive a command in time.
+* There was another script sent for execution e.g.
+
+  * Script code was sent to the robot via its primary interface
+  * Robot motion is performed using the Teach pendant
+
+Depending on the operation mode, perform one of the following steps:
+
+* In *local control mode*, simply press the "Play" button |play_button| on the teach pendant.
+* In *remote control mode* start the program using the following dashboard call:
+
+  .. code-block:: console
+
+     $ ros2 service call /dashboard_client/play std_srvs/srv/Trigger {}
+
+* When the driver is started with ``headless_mode:=true`` perform the following service call:
+
+  .. code-block:: console
+
+     $ ros2 service call /io_and_status_controller/resend_robot_program std_srvs/srv/Trigger {}
+
+
+
+
+
+.. |play_button| image:: ../resources/play_button.svg
+                 :height: 20px
+                 :width: 20px

examples/examples.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+# Copyright 2024, 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.
+
+import rclpy
+from builtin_interfaces.msg import Duration
+from control_msgs.action import FollowJointTrajectory
+
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from ur_msgs.srv import SetIO
+from controller_manager_msgs.srv import SwitchController
+from std_srvs.srv import Trigger
+
+TIMEOUT_WAIT_SERVICE = 10
+TIMEOUT_WAIT_SERVICE_INITIAL = 60
+TIMEOUT_WAIT_ACTION = 10
+
+ROBOT_JOINTS = [
+    "shoulder_pan_joint",
+    "shoulder_lift_joint",
+    "elbow_joint",
+    "wrist_1_joint",
+    "wrist_2_joint",
+    "wrist_3_joint",
+]
+
+
+# Helper functions
+def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE):
+    client = node.create_client(srv_type, srv_name)
+    if client.wait_for_service(timeout) is False:
+        raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}")
+
+    node.get_logger().info(f"Successfully connected to service '{srv_name}'")
+    return client
+
+
+def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION):
+    client = ActionClient(node, action_type, action_name)
+    if client.wait_for_server(timeout) is False:
+        raise Exception(
+            f"Could not reach action server '{action_name}' within timeout of {timeout}"
+        )
+
+    node.get_logger().info(f"Successfully connected to action '{action_name}'")
+    return client
+
+
+class Robot:
+    def __init__(self, node):
+        self.node = node
+        self.service_interfaces = {
+            "/io_and_status_controller/set_io": SetIO,
+            "/dashboard_client/play": Trigger,
+            "/controller_manager/switch_controller": SwitchController,
+        }
+        self.init_robot()
+
+    def init_robot(self):
+        self.service_clients = {
+            srv_name: waitForService(self.node, srv_name, srv_type)
+            for (srv_name, srv_type) in self.service_interfaces.items()
+        }
+
+        self.jtc_action_client = waitForAction(
+            self.node,
+            "/scaled_joint_trajectory_controller/follow_joint_trajectory",
+            FollowJointTrajectory,
+        )
+        self.passthrough_trajectory_action_client = waitForAction(
+            self.node,
+            "/passthrough_trajectory_controller/follow_joint_trajectory",
+            FollowJointTrajectory,
+        )
+
+    def set_io(self, pin, value):
+        """Test to set an IO."""
+        set_io_req = SetIO.Request()
+        set_io_req.fun = 1
+        set_io_req.pin = pin
+        set_io_req.state = value
+
+        self.call_service("/io_and_status_controller/set_io", set_io_req)
+
+    def send_trajectory(self, waypts, time_vec, action_client):
+        """Send robot trajectory."""
+        if len(waypts) != len(time_vec):
+            raise Exception("waypoints vector and time vec should be same length")
+
+        # Construct test trajectory
+        joint_trajectory = JointTrajectory()
+        joint_trajectory.joint_names = ROBOT_JOINTS
+        for i in range(len(waypts)):
+            point = JointTrajectoryPoint()
+            point.positions = waypts[i]
+            point.time_from_start = time_vec[i]
+            joint_trajectory.points.append(point)
+
+        # Sending trajectory goal
+        goal_response = self.call_action(
+            action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory)
+        )
+        if not goal_response.accepted:
+            raise Exception("trajectory was not accepted")
+
+        # Verify execution
+        result = self.get_result(action_client, goal_response)
+        return result.error_code == FollowJointTrajectory.Result.SUCCESSFUL
+
+    def call_service(self, srv_name, request):
+        future = self.service_clients[srv_name].call_async(request)
+        rclpy.spin_until_future_complete(self.node, future)
+        if future.result() is not None:
+            return future.result()
+        else:
+            raise Exception(f"Exception while calling service: {future.exception()}")
+
+    def call_action(self, ac_client, g):
+        future = ac_client.send_goal_async(g)
+        rclpy.spin_until_future_complete(self.node, future)
+
+        if future.result() is not None:
+            return future.result()
+        else:
+            raise Exception(f"Exception while calling action: {future.exception()}")
+
+    def get_result(self, ac_client, goal_response):
+        future_res = ac_client._get_result_async(goal_response)
+        rclpy.spin_until_future_complete(self.node, future_res)
+        if future_res.result() is not None:
+            return future_res.result().result
+        else:
+            raise Exception(f"Exception while calling action: {future_res.exception()}")
+
+
+if __name__ == "__main__":
+    rclpy.init()
+    node = Node("robot_driver_test")
+    robot = Robot(node)
+
+    # The following list are arbitrary joint positions, change according to your own needs
+    waypts = [
+        [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
+        [-1.2, -1.4, -1.9, -1.2, 1.5951, -0.0311],
+        [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
+    ]
+    time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)]
+
+    # Execute trajectory on robot, make sure that the robot is booted and the control script is running
+    robot.send_trajectory(waypts, time_vec)
+
+    # Set digital output 1 to true
+    robot.set_io(1, 1.0)

examples/force_mode.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+# Copyright 2024, 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.
+
+import time
+
+import rclpy
+from rclpy.node import Node
+from controller_manager_msgs.srv import SwitchController
+from builtin_interfaces.msg import Duration
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Header
+from std_srvs.srv import Trigger
+
+from geometry_msgs.msg import (
+    Point,
+    Quaternion,
+    Pose,
+    PoseStamped,
+    Wrench,
+    Vector3,
+)
+
+from ur_msgs.srv import SetForceMode
+
+from examples import Robot
+
+if __name__ == "__main__":
+    rclpy.init()
+    node = Node("robot_driver_test")
+    robot = Robot(node)
+
+    # Add force mode service to service interfaces and re-init robot
+    robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode})
+    robot.service_interfaces.update({"/force_mode_controller/stop_force_mode": Trigger})
+    robot.init_robot()
+    time.sleep(0.5)
+    # Press play on the robot
+    robot.call_service("/dashboard_client/play", Trigger.Request())
+
+    time.sleep(0.5)
+    # Start controllers
+    robot.call_service(
+        "/controller_manager/switch_controller",
+        SwitchController.Request(
+            deactivate_controllers=["scaled_joint_trajectory_controller"],
+            activate_controllers=["passthrough_trajectory_controller", "force_mode_controller"],
+            strictness=SwitchController.Request.BEST_EFFORT,
+        ),
+    )
+
+    # Move robot in to position
+    robot.send_trajectory(
+        waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]],
+        time_vec=[Duration(sec=5, nanosec=0)],
+        action_client=robot.passthrough_trajectory_action_client,
+    )
+
+    # Finished moving
+    # Create task frame for force mode
+    point = Point(x=0.0, y=0.0, z=0.0)
+    orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+    task_frame_pose = Pose()
+    task_frame_pose.position = point
+    task_frame_pose.orientation = orientation
+    header = Header(seq=1, frame_id="world")
+    header.stamp.sec = int(time.time()) + 1
+    header.stamp.nanosec = 0
+    frame_stamp = PoseStamped()
+    frame_stamp.header = header
+    frame_stamp.pose = task_frame_pose
+
+    # Create compliance vector (which axes should be force controlled)
+    compliance = [False, False, True, False, False, False]
+
+    # Create Wrench message for force mode
+    wrench_vec = Wrench(force=Vector3(x=0.0, y=0.0, z=-20.0), torque=Vector3(x=0.0, y=0.0, z=0.0))
+    # Specify interpretation of task frame (no transform)
+    type_spec = SetForceMode.Request.NO_TRANSFORM
+
+    # Specify max speeds and deviations of force mode
+    speed_limits = Twist()
+    speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0)
+    speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0)
+    deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+    # specify damping and gain scaling
+    damping_factor = 0.1
+    gain_scale = 0.8
+
+    req = SetForceMode.Request()
+    req.task_frame = frame_stamp
+    req.selection_vector_x = compliance[0]
+    req.selection_vector_y = compliance[1]
+    req.selection_vector_z = compliance[2]
+    req.selection_vector_rx = compliance[3]
+    req.selection_vector_ry = compliance[4]
+    req.selection_vector_rz = compliance[5]
+    req.wrench = wrench_vec
+    req.type = type_spec
+    req.speed_limits = speed_limits
+    req.deviation_limits = deviation_limits
+    req.damping_factor = damping_factor
+    req.gain_scaling = gain_scale
+
+    # Send request to controller
+    node.get_logger().info(f"Starting force mode with {req}")
+    robot.call_service("/force_mode_controller/start_force_mode", req)
+    robot.send_trajectory(
+        waypts=[[1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]],
+        time_vec=[Duration(sec=5, nanosec=0)],
+        action_client=robot.passthrough_trajectory_action_client,
+    )
+
+    time.sleep(3)
+    node.get_logger().info("Deactivating force mode controller.")
+    robot.call_service("/force_mode_controller/stop_force_mode", Trigger.Request())

hardware_interface_plugin.xml

@@ -0,0 +1,9 @@
+<library path="ur_robot_driver_plugin">
+  <class name="ur_robot_driver/URPositionHardwareInterface"
+         type="ur_robot_driver::URPositionHardwareInterface"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+		ROS2 Control System Driver for the Universal Robots series.
+    </description>
+  </class>
+</library>

include/ur_robot_driver/controller_stopper.hpp

@@ -0,0 +1,89 @@
+// Copyright 2019, 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 exner@fzi.de
+ * \date    2019-06-12
+ *
+ * \author  Mads Holm Peters
+ * \date    2022-02-25
+ *
+ */
+//----------------------------------------------------------------------
+#ifndef UR_ROBOT_DRIVER__CONTROLLER_STOPPER_HPP_
+#define UR_ROBOT_DRIVER__CONTROLLER_STOPPER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "controller_manager_msgs/srv/list_controllers.hpp"
+#include "controller_manager_msgs/srv/switch_controller.hpp"
+#include "std_msgs/msg/bool.hpp"
+
+class ControllerStopper
+{
+public:
+  ControllerStopper() = delete;
+  ControllerStopper(const rclcpp::Node::SharedPtr& node, bool stop_controllers_on_startup);
+  virtual ~ControllerStopper() = default;
+
+private:
+  void robotRunningCallback(const std_msgs::msg::Bool::ConstSharedPtr msg);
+
+  /*!
+   * \brief Queries running stoppable controllers and the controllers are stopped.
+   *
+   * Queries the controller manager for running controllers and compares the result with the
+   * consistent_controllers_. The remaining running controllers are stored in stopped_controllers_
+   * and stopped afterwards.
+   */
+  void findAndStopControllers();
+
+  /*!
+   * \brief Starts the controllers stored in stopped_controllers_.
+   *
+   */
+  void startControllers();
+
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr controller_manager_srv_;
+  rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr controller_list_srv_;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr robot_running_sub_;
+
+  std::vector<std::string> consistent_controllers_;
+  std::vector<std::string> stopped_controllers_;
+
+  bool stop_controllers_on_startup_;
+  bool robot_running_;
+};
+#endif  // UR_ROBOT_DRIVER__CONTROLLER_STOPPER_HPP_

include/ur_robot_driver/dashboard_client_ros.hpp

@@ -0,0 +1,154 @@
+// Copyright 2019, FZI Forschungszentrum Informatik
+//
+// 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 exner@fzi.de
+ * \date    2019-10-21
+ * \author  Marvin Große Besselmann grosse@fzi.de
+ * \date    2021-03-22
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_ROBOT_DRIVER__DASHBOARD_CLIENT_ROS_HPP_
+#define UR_ROBOT_DRIVER__DASHBOARD_CLIENT_ROS_HPP_
+
+// System
+#include <regex>
+#include <string>
+#include <memory>
+
+// ROS
+#include "rclcpp/rclcpp.hpp"
+#include "std_srvs/srv/trigger.hpp"
+// UR client library
+#include "ur_client_library/ur/dashboard_client.h"
+#include "ur_client_library/exceptions.h"
+#include "ur_dashboard_msgs/msg/program_state.hpp"
+#include "ur_dashboard_msgs/srv/add_to_log.hpp"
+#include "ur_dashboard_msgs/srv/get_loaded_program.hpp"
+#include "ur_dashboard_msgs/srv/get_program_state.hpp"
+#include "ur_dashboard_msgs/srv/get_robot_mode.hpp"
+#include "ur_dashboard_msgs/srv/get_safety_mode.hpp"
+#include "ur_dashboard_msgs/srv/is_program_running.hpp"
+#include "ur_dashboard_msgs/srv/is_program_saved.hpp"
+#include "ur_dashboard_msgs/srv/load.hpp"
+#include "ur_dashboard_msgs/srv/popup.hpp"
+#include "ur_dashboard_msgs/srv/raw_request.hpp"
+
+namespace ur_robot_driver
+{
+/*!
+ * \brief ROS wrapper for UR's dashboard server access. Many (not necessarily all) dashboard
+ * functionalities are wrapped into ROS services here.
+ */
+class DashboardClientROS
+{
+public:
+  /*!
+   * \brief Constructor that shall be used by default
+   *
+   * \param nh Node handle pointing to the name space the dashboard-related functionalities are to
+   * be found
+   * \param robot_ip IP address of the robot
+   */
+  DashboardClientROS(const rclcpp::Node::SharedPtr& node, const std::string& robot_ip);
+  DashboardClientROS() = delete;
+  virtual ~DashboardClientROS() = default;
+
+private:
+  inline rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr createDashboardTriggerSrv(const std::string& topic,
+                                                                                      const std::string& command,
+                                                                                      const std::string& expected)
+  {
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service = node_->create_service<std_srvs::srv::Trigger>(
+        topic, [&, command, expected](const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
+                                      const std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
+          try {
+            resp->message = this->client_.sendAndReceive(command);
+            resp->success = std::regex_match(resp->message, std::regex(expected));
+          } catch (const urcl::UrException& e) {
+            RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+            resp->message = e.what();
+            resp->success = false;
+          }
+        });
+    return service;
+  }
+
+  bool handleRunningQuery(ur_dashboard_msgs::srv::IsProgramRunning::Request::SharedPtr req,
+                          ur_dashboard_msgs::srv::IsProgramRunning::Response::SharedPtr resp);
+
+  bool handleSavedQuery(ur_dashboard_msgs::srv::IsProgramSaved::Request::SharedPtr req,
+                        ur_dashboard_msgs::srv::IsProgramSaved::Response::SharedPtr resp);
+
+  bool handleSafetyModeQuery(ur_dashboard_msgs::srv::GetSafetyMode::Request::SharedPtr req,
+                             ur_dashboard_msgs::srv::GetSafetyMode::Response::SharedPtr resp);
+
+  bool handleRobotModeQuery(ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req,
+                            ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp);
+
+  bool connect();
+
+  std::shared_ptr<rclcpp::Node> node_;
+  urcl::DashboardClient client_;
+
+  // Commanding services
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr brake_release_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr clear_operational_mode_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr close_popup_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr close_safety_popup_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pause_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr play_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr power_off_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr power_on_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr restart_safety_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr shutdown_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::Load>::SharedPtr load_installation_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::Load>::SharedPtr load_program_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr quit_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::AddToLog>::SharedPtr add_to_log_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reconnect_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::RawRequest>::SharedPtr raw_request_service_;
+
+  // Query services
+  rclcpp::Service<ur_dashboard_msgs::srv::IsProgramRunning>::SharedPtr running_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetLoadedProgram>::SharedPtr get_loaded_program_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::IsProgramSaved>::SharedPtr is_program_saved_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::Popup>::SharedPtr popup_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetProgramState>::SharedPtr program_state_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetSafetyMode>::SharedPtr safety_mode_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetRobotMode>::SharedPtr robot_mode_service_;
+};
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__DASHBOARD_CLIENT_ROS_HPP_

include/ur_robot_driver/hardware_interface.hpp

@@ -0,0 +1,312 @@
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// 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  Lovro Ivanov lovro.ivanov@gmail.com
+ * \author  Andy Zelenak zelenak@picknik.ai
+ * \author  Marvin Große Besselmann grosse@fzi.de
+ * \date    2019-04-11
+ *
+ */
+//----------------------------------------------------------------------
+#ifndef UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_
+#define UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_
+
+// System
+#include <memory>
+#include <string>
+#include <vector>
+#include <limits>
+
+// ros2_control hardware_interface
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+
+// UR stuff
+#include "ur_client_library/ur/ur_driver.h"
+#include "ur_client_library/ur/robot_receive_timeout.h"
+#include "ur_robot_driver/dashboard_client_ros.hpp"
+#include "ur_dashboard_msgs/msg/robot_mode.hpp"
+
+// ROS
+#include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace ur_robot_driver
+{
+enum class PausingState
+{
+  PAUSED,
+  RUNNING,
+  RAMPUP
+};
+
+enum StoppingInterface
+{
+  NONE,
+  STOP_POSITION,
+  STOP_VELOCITY,
+  STOP_PASSTHROUGH,
+  STOP_FORCE_MODE,
+  STOP_FREEDRIVE,
+};
+
+// We define our own quaternion to use it as a buffer, since we need to pass pointers to the state
+// interfaces.
+struct Quaternion
+{
+  Quaternion() : x(0), y(0), z(0), w(0)
+  {
+  }
+
+  void set(const tf2::Quaternion& q)
+  {
+    x = q.x();
+    y = q.y();
+    z = q.z();
+    w = q.w();
+  }
+
+  double x;
+  double y;
+  double z;
+  double w;
+};
+
+/*!
+ * \brief The HardwareInterface class handles the interface between the ROS system and the main
+ * driver. It contains the read and write methods of the main control loop and registers various ROS
+ * topics and services.
+ */
+class URPositionHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(URPositionHardwareInterface);
+  virtual ~URPositionHardwareInterface();
+
+  hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info) final;
+
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() final;
+
+  hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) final;
+  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) final;
+  hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) final;
+  hardware_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) final;
+
+  hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) final;
+  hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) final;
+
+  hardware_interface::return_type prepare_command_mode_switch(const std::vector<std::string>& start_interfaces,
+                                                              const std::vector<std::string>& stop_interfaces) final;
+
+  hardware_interface::return_type perform_command_mode_switch(const std::vector<std::string>& start_interfaces,
+                                                              const std::vector<std::string>& stop_interfaces) final;
+
+  /*!
+   * \brief Callback to handle a change in the current state of the URCaps program running on the
+   * robot. Executed only on the state change.
+   *
+   * \param program_running True when the URCap program is running on the robot.
+   */
+  void handleRobotProgramState(bool program_running);
+
+  static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
+
+  void asyncThread();
+
+protected:
+  template <typename T>
+  void readData(const std::unique_ptr<urcl::rtde_interface::DataPackage>& data_pkg, const std::string& var_name,
+                T& data);
+  template <typename T, size_t N>
+  void readBitsetData(const std::unique_ptr<urcl::rtde_interface::DataPackage>& data_pkg, const std::string& var_name,
+                      std::bitset<N>& data);
+
+  // stop function used by on_shutdown and on_cleanup
+  hardware_interface::CallbackReturn stop();
+
+  void initAsyncIO();
+  void checkAsyncIO();
+  void updateNonDoubleValues();
+  void extractToolPose();
+  void transformForceTorque();
+  void start_force_mode();
+  void stop_force_mode();
+  void check_passthrough_trajectory_controller();
+  void trajectory_done_callback(urcl::control::TrajectoryResult result);
+  bool has_accelerations(std::vector<std::array<double, 6>> accelerations);
+  bool has_velocities(std::vector<std::array<double, 6>> velocities);
+
+  urcl::vector6d_t urcl_position_commands_;
+  urcl::vector6d_t urcl_position_commands_old_;
+  urcl::vector6d_t urcl_velocity_commands_;
+  urcl::vector6d_t urcl_joint_positions_;
+  urcl::vector6d_t urcl_joint_velocities_;
+  urcl::vector6d_t urcl_joint_efforts_;
+  urcl::vector6d_t urcl_ft_sensor_measurements_;
+  urcl::vector6d_t urcl_tcp_pose_;
+  urcl::vector6d_t urcl_target_tcp_pose_;
+  urcl::vector6d_t tcp_offset_;
+  tf2::Quaternion tcp_rotation_quat_;
+  Quaternion tcp_rotation_buffer;
+
+  bool packet_read_;
+
+  uint32_t runtime_state_;
+  bool controllers_initialized_;
+
+  std::bitset<18> actual_dig_out_bits_;
+  std::bitset<18> actual_dig_in_bits_;
+  std::array<double, 2> standard_analog_input_;
+  std::array<double, 2> standard_analog_output_;
+  std::bitset<4> analog_io_types_;
+  uint32_t tool_mode_;
+  std::bitset<2> tool_analog_input_types_;
+  std::array<double, 2> tool_analog_input_;
+  int32_t tool_output_voltage_;
+  double tool_output_current_;
+  double tool_temperature_;
+  double speed_scaling_;
+  double target_speed_fraction_;
+  double speed_scaling_combined_;
+  int32_t robot_mode_;
+  int32_t safety_mode_;
+  std::bitset<4> robot_status_bits_;
+  std::bitset<11> safety_status_bits_;
+
+  // asynchronous commands
+  std::array<double, 18> standard_dig_out_bits_cmd_;
+  std::array<double, 2> standard_analog_output_cmd_;
+  double analog_output_domain_cmd_;
+  double tool_voltage_cmd_;
+  double io_async_success_;
+  double target_speed_fraction_cmd_;
+  double scaling_async_success_;
+  double resend_robot_program_cmd_;
+  double resend_robot_program_async_success_;
+  double zero_ftsensor_cmd_;
+  double zero_ftsensor_async_success_;
+  double hand_back_control_cmd_;
+  double hand_back_control_async_success_;
+  bool first_pass_;
+  bool initialized_;
+  double system_interface_initialized_;
+  std::atomic_bool async_thread_shutdown_;
+  double get_robot_software_version_major_;
+  double get_robot_software_version_minor_;
+  double get_robot_software_version_bugfix_;
+  double get_robot_software_version_build_;
+
+  // Freedrive mode controller interface values
+  bool freedrive_activated_;
+  bool freedrive_mode_controller_running_;
+  double freedrive_mode_async_success_;
+  double freedrive_mode_enable_;
+  double freedrive_mode_abort_;
+
+  // Passthrough trajectory controller interface values
+  double passthrough_trajectory_transfer_state_;
+  double passthrough_trajectory_abort_;
+  bool passthrough_trajectory_controller_running_;
+  urcl::vector6d_t passthrough_trajectory_positions_;
+  urcl::vector6d_t passthrough_trajectory_velocities_;
+  urcl::vector6d_t passthrough_trajectory_accelerations_;
+  double passthrough_trajectory_time_from_start_;
+
+  // payload stuff
+  urcl::vector3d_t payload_center_of_gravity_;
+  double payload_mass_;
+  double payload_async_success_;
+
+  // force mode parameters
+  urcl::vector6d_t force_mode_task_frame_;
+  urcl::vector6d_t force_mode_selection_vector_;
+  urcl::vector6uint32_t force_mode_selection_vector_copy_;
+  urcl::vector6d_t force_mode_wrench_;
+  urcl::vector6d_t force_mode_limits_;
+  double force_mode_type_;
+  double force_mode_async_success_;
+  double force_mode_disable_cmd_;
+  double force_mode_damping_;
+  double force_mode_gain_scaling_;
+
+  // copy of non double values
+  std::array<double, 18> actual_dig_out_bits_copy_;
+  std::array<double, 18> actual_dig_in_bits_copy_;
+  std::array<double, 4> analog_io_types_copy_;
+  double tool_mode_copy_;
+  std::array<double, 2> tool_analog_input_types_copy_;
+  double tool_output_voltage_copy_;
+  double robot_mode_copy_;
+  double safety_mode_copy_;
+  std::array<double, 4> robot_status_bits_copy_;
+  std::array<double, 11> safety_status_bits_copy_;
+
+  bool robot_program_running_;
+  bool non_blocking_read_;
+  double robot_program_running_copy_;
+
+  /* Vectors used to store the trajectory received from the passthrough trajectory controller. The whole trajectory is
+   * received before it is sent to the robot. */
+  std::vector<std::array<double, 6>> trajectory_joint_positions_;
+  std::vector<std::array<double, 6>> trajectory_joint_velocities_;
+  std::vector<std::array<double, 6>> trajectory_joint_accelerations_;
+  std::vector<double> trajectory_times_;
+
+  PausingState pausing_state_;
+  double pausing_ramp_up_increment_;
+
+  // resources switching aux vars
+  std::vector<std::vector<uint>> stop_modes_;
+  std::vector<std::vector<std::string>> start_modes_;
+  bool position_controller_running_;
+  bool velocity_controller_running_;
+  bool force_mode_controller_running_ = false;
+
+  std::unique_ptr<urcl::UrDriver> ur_driver_;
+  std::shared_ptr<std::thread> async_thread_;
+
+  std::atomic_bool rtde_comm_has_been_started_ = false;
+
+  urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20);
+
+  const std::string PASSTHROUGH_GPIO = "trajectory_passthrough";
+  const std::string FORCE_MODE_GPIO = "force_mode";
+  const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode";
+};
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_

include/ur_robot_driver/robot_state_helper.hpp

@@ -0,0 +1,114 @@
+// Copyright 2024, 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.
+
+#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
+#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
+
+#include <chrono>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/create_server.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "std_srvs/srv/trigger.hpp"
+
+#include "ur_dashboard_msgs/action/set_mode.hpp"
+#include "ur_dashboard_msgs/msg/safety_mode.hpp"
+#include "ur_dashboard_msgs/msg/robot_mode.hpp"
+#include "ur_client_library/ur/datatypes.h"
+
+namespace ur_robot_driver
+{
+class RobotStateHelper
+{
+public:
+  using SetModeGoalHandle = rclcpp_action::ServerGoalHandle<ur_dashboard_msgs::action::SetMode>;
+
+  explicit RobotStateHelper(const rclcpp::Node::SharedPtr& node);
+  RobotStateHelper() = delete;
+  virtual ~RobotStateHelper() = default;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+
+  void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg);
+  void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg);
+
+  void updateRobotState();
+
+  bool recoverFromSafety();
+  bool doTransition(const urcl::RobotMode target_mode);
+  bool jumpToRobotMode(const urcl::RobotMode target_mode);
+
+  bool safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv);
+
+  bool stopProgram();
+
+  void setModeAcceptCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+  rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid,
+                                                  std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal);
+  rclcpp_action::CancelResponse setModeCancelCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+
+  void setModeExecute(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+
+  bool headless_mode_;
+
+  std::shared_ptr<ur_dashboard_msgs::action::SetMode::Result> result_;
+  std::shared_ptr<ur_dashboard_msgs::action::SetMode::Feedback> feedback_;
+  std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal_;
+  std::shared_ptr<SetModeGoalHandle> current_goal_handle_;
+
+  std::atomic<urcl::RobotMode> robot_mode_;
+  std::atomic<urcl::SafetyMode> safety_mode_;
+  std::atomic<bool> error_ = false;
+  std::atomic<bool> in_action_;
+  std::atomic<bool> program_running_;
+  std::mutex goal_mutex_;
+
+  rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;
+
+  rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_;
+
+  rclcpp::Subscription<ur_dashboard_msgs::msg::RobotMode>::SharedPtr robot_mode_sub_;
+  rclcpp::Subscription<ur_dashboard_msgs::msg::SafetyMode>::SharedPtr safety_mode_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr program_running_sub;
+
+  rclcpp::CallbackGroup::SharedPtr service_cb_grp_;
+
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr restart_safety_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_on_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_off_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr brake_release_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_program_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr play_program_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resend_robot_program_srv_;
+};
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_

include/ur_robot_driver/urcl_log_handler.hpp

@@ -0,0 +1,112 @@
+// Copyright 2021 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.
+
+// All source code contained in and/or linked to in this message (the “Source Code”) is subject to the copyright of
+// Universal Robots A/S and/or its licensors. THE SOURCE CODE IS PROVIDED “AS IS” WITHOUT WARRANTY OF ANY KIND, EXPRESS
+// OR IMPLIED, INCLUDING – BUT NOT LIMITED TO – WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR
+// NONINFRINGEMENT. USE OF THE SOURCE CODE IS AT YOUR OWN RISK AND UNIVERSAL ROBOTS A/S AND ITS LICENSORS SHALL, TO THE
+// MAXIMUM EXTENT PERMITTED BY LAW, NOT BE LIABLE FOR ANY ERRORS OR MALICIOUS CODE IN THE SOURCE CODE, ANY THIRD-PARTY
+// CLAIMS, OR ANY OTHER CLAIMS AND DAMAGES, INCLUDING INDIRECT, INCIDENTAL, SPECIAL, CONSEQUENTIAL OR PUNITIVE DAMAGES,
+// OR ANY LOSS OF PROFITS, EXPECTED SAVINGS, OR REVENUES, WHETHER INCURRED DIRECTLY OR INDIRECTLY, OR ANY LOSS OF DATA,
+// USE, GOODWILL, OR OTHER INTANGIBLE LOSSES, RESULTING FROM YOUR USE OF THE SOURCE CODE. You may make copies of the
+// Source Code for use in connection with a Universal Robots or UR+ product, provided that you include (i) an
+// appropriate copyright notice (“©  [the year in which you received the Source Code or the Source Code was first
+// published, e.g. “2021”] Universal Robots A/S and/or its licensors”) along with the capitalized section of this notice
+// in all copies of the Source Code. By using the Source Code, you agree to the above terms. For more information,
+// please contact legal@universal-robots.com.
+
+#ifndef UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_
+#define UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_
+
+#include <string>
+#include "ur_client_library/log.h"
+namespace ur_robot_driver
+{
+
+/*!
+ * \brief Register the UrclLoghHandler, this will start logging messages from the client library with ROS2 logging.
+ * This function has to be called inside your node, to enable the log handler.
+ */
+void registerUrclLogHandler(const std::string& tf_prefix = "");
+
+/*!
+ * \brief Unregister the UrclLoghHandler, stop logging messages from the client library with ROS2 logging.
+ */
+void unregisterUrclLogHandler();
+
+/*!
+ * \brief Loghandler for handling messages logged with the C++ client library. This loghandler will log the messages
+ * from the client library with ROS2s logging.
+ * Use registerLogHandler to register this LogHandler. This class shouldn't be instantiated directly.
+ */
+class UrclLogHandler : public urcl::LogHandler
+{
+public:
+  /*!
+   * \brief Default constructor
+   */
+  UrclLogHandler();
+
+  /*!
+   * \brief Function to log a message
+   *
+   * \param file The log message comes from this file
+   * \param line The log message comes from this line
+   * \param loglevel Indicates the severity of the log message
+   * \param log Log message
+   */
+  void log(const char* file, int line, urcl::LogLevel loglevel, const char* message) override;
+
+  /**
+   * @brief getTFPrefix - obtain the currently set tf_prefix
+   * @return
+   */
+  const std::string& getTFPrefix() const
+  {
+    return tf_prefix_;
+  }
+
+private:
+  std::string tf_prefix_{};
+
+  /**
+   * @brief setTFPrefix - set the tf_prefix the logger will append to the node name
+   * @param tf_prefix
+   */
+  void setTFPrefix(const std::string& tf_prefix)
+  {
+    tf_prefix_ = tf_prefix;
+  }
+
+  // Declare the register method as a friend so that we can access setTFPrefix from it
+  friend void registerUrclLogHandler(const std::string& tf_prefix);
+};
+
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_

launch/test_forward_velocity_controller.launch.py

@@ -0,0 +1,56 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Lovro Ivanov
+#
+# Description: After a robot has been loaded, this will execute a series of trajectories.
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    velocity_goals = PathJoinSubstitution(
+        [FindPackageShare("ur_robot_driver"), "config", "test_velocity_goal_publishers_config.yaml"]
+    )
+
+    return LaunchDescription(
+        [
+            Node(
+                package="ros2_controllers_test_nodes",
+                executable="publisher_forward_position_controller",
+                name="publisher_forward_velocity_controller",
+                parameters=[velocity_goals],
+                output="screen",
+            )
+        ]
+    )

launch/test_joint_trajectory_controller.launch.py

@@ -0,0 +1,56 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+#
+# Description: After a robot has been loaded, this will execute a series of trajectories.
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    position_goals = PathJoinSubstitution(
+        [FindPackageShare("ur_robot_driver"), "config", "test_goal_publishers_config.yaml"]
+    )
+
+    return LaunchDescription(
+        [
+            Node(
+                package="ros2_controllers_test_nodes",
+                executable="publisher_joint_trajectory_controller",
+                name="publisher_joint_trajectory_controller",
+                parameters=[position_goals],
+                output="screen",
+            )
+        ]
+    )

launch/test_scaled_joint_trajectory_controller.launch.py

@@ -0,0 +1,56 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+#
+# Description: After a robot has been loaded, this will execute a series of trajectories.
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    position_goals = PathJoinSubstitution(
+        [FindPackageShare("ur_robot_driver"), "config", "test_goal_publishers_config.yaml"]
+    )
+
+    return LaunchDescription(
+        [
+            Node(
+                package="ros2_controllers_test_nodes",
+                executable="publisher_joint_trajectory_controller",
+                name="publisher_scaled_joint_trajectory_controller",
+                parameters=[position_goals],
+                output="screen",
+            )
+        ]
+    )

launch/ur10.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur10",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur10e.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur10e",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur16e.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur16e",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur20.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur20",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur3.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur3",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur30.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur30",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur3e.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]), # so basically it's only return the values to ur_control.launch.py
+        launch_arguments={
+            "ur_type": "ur3e",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch]) # declared_arguments is already a dictionary

launch/ur5.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur5",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur5e.launch.py

@@ -0,0 +1,104 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initially loaded robot controller.",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ur_control.launch.py"]),
+        launch_arguments={
+            "ur_type": "ur5e",
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "initial_joint_controller": initial_joint_controller,
+            "activate_joint_controller": activate_joint_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])

launch/ur_control.launch.py

@@ -0,0 +1,646 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+#
+# Author: Denis Stogl
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import (
+    AndSubstitution,
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    NotSubstitution,
+    PathJoinSubstitution,
+)
+
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    ur_type = LaunchConfiguration("ur_type")
+    robot_ip = LaunchConfiguration("robot_ip")
+    safety_limits = LaunchConfiguration("safety_limits")
+    safety_pos_margin = LaunchConfiguration("safety_pos_margin")
+    safety_k_position = LaunchConfiguration("safety_k_position")
+    # General arguments
+    runtime_config_package = LaunchConfiguration("runtime_config_package")
+    controllers_file = LaunchConfiguration("controllers_file")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    kinematics_params_file = LaunchConfiguration("kinematics_params_file")
+    tf_prefix = LaunchConfiguration("tf_prefix")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+    headless_mode = LaunchConfiguration("headless_mode")
+    launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
+    use_tool_communication = LaunchConfiguration("use_tool_communication")
+    tool_parity = LaunchConfiguration("tool_parity")
+    tool_baud_rate = LaunchConfiguration("tool_baud_rate")
+    tool_stop_bits = LaunchConfiguration("tool_stop_bits")
+    tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars")
+    tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars")
+    tool_device_name = LaunchConfiguration("tool_device_name")
+    tool_tcp_port = LaunchConfiguration("tool_tcp_port")
+    tool_voltage = LaunchConfiguration("tool_voltage")
+    reverse_ip = LaunchConfiguration("reverse_ip")
+    script_command_port = LaunchConfiguration("script_command_port")
+    reverse_port = LaunchConfiguration("reverse_port")
+    script_sender_port = LaunchConfiguration("script_sender_port")
+    trajectory_port = LaunchConfiguration("trajectory_port")
+
+    joint_limit_params = PathJoinSubstitution(
+        [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"]
+    )
+    physical_params = PathJoinSubstitution(
+        [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"]
+    )
+    visual_params = PathJoinSubstitution(
+        [FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"]
+    )
+    script_filename = PathJoinSubstitution(
+        [FindPackageShare("ur_client_library"), "resources", "external_control.urscript"]
+    )
+    input_recipe_filename = PathJoinSubstitution(
+        [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
+    )
+    output_recipe_filename = PathJoinSubstitution(
+        [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"]
+    )
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
+            " ",
+            "robot_ip:=",
+            robot_ip,
+            " ",
+            "joint_limit_params:=",
+            joint_limit_params,
+            " ",
+            "kinematics_params:=",
+            kinematics_params_file,
+            " ",
+            "physical_params:=",
+            physical_params,
+            " ",
+            "visual_params:=",
+            visual_params,
+            " ",
+            "safety_limits:=",
+            safety_limits,
+            " ",
+            "safety_pos_margin:=",
+            safety_pos_margin,
+            " ",
+            "safety_k_position:=",
+            safety_k_position,
+            " ",
+            "name:=",
+            ur_type,
+            " ",
+            "script_filename:=",
+            script_filename,
+            " ",
+            "input_recipe_filename:=",
+            input_recipe_filename,
+            " ",
+            "output_recipe_filename:=",
+            output_recipe_filename,
+            " ",
+            "tf_prefix:=",
+            tf_prefix,
+            " ",
+            "use_fake_hardware:=",
+            use_fake_hardware,
+            " ",
+            "fake_sensor_commands:=",
+            fake_sensor_commands,
+            " ",
+            "headless_mode:=",
+            headless_mode,
+            " ",
+            "use_tool_communication:=",
+            use_tool_communication,
+            " ",
+            "tool_parity:=",
+            tool_parity,
+            " ",
+            "tool_baud_rate:=",
+            tool_baud_rate,
+            " ",
+            "tool_stop_bits:=",
+            tool_stop_bits,
+            " ",
+            "tool_rx_idle_chars:=",
+            tool_rx_idle_chars,
+            " ",
+            "tool_tx_idle_chars:=",
+            tool_tx_idle_chars,
+            " ",
+            "tool_device_name:=",
+            tool_device_name,
+            " ",
+            "tool_tcp_port:=",
+            tool_tcp_port,
+            " ",
+            "tool_voltage:=",
+            tool_voltage,
+            " ",
+            "reverse_ip:=",
+            reverse_ip,
+            " ",
+            "script_command_port:=",
+            script_command_port,
+            " ",
+            "reverse_port:=",
+            reverse_port,
+            " ",
+            "script_sender_port:=",
+            script_sender_port,
+            " ",
+            "trajectory_port:=",
+            trajectory_port,
+            " ",
+        ]
+    )
+    robot_description = {
+        "robot_description": ParameterValue(value=robot_description_content, value_type=str)
+    }
+
+    initial_joint_controllers = PathJoinSubstitution(
+        [FindPackageShare(runtime_config_package), "config", controllers_file]
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), "rviz", "view_robot.rviz"]
+    )
+
+    # define update rate
+    update_rate_config_file = PathJoinSubstitution(
+        [
+            FindPackageShare(runtime_config_package),
+            "config",
+            ur_type.perform(context) + "_update_rate.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[
+            robot_description,
+            update_rate_config_file,
+            ParameterFile(initial_joint_controllers, allow_substs=True),
+        ],
+        output="screen",
+        condition=IfCondition(use_fake_hardware),
+    )
+
+    ur_control_node = Node(
+        package="ur_robot_driver",
+        executable="ur_ros2_control_node",
+        parameters=[
+            robot_description,
+            update_rate_config_file,
+            ParameterFile(initial_joint_controllers, allow_substs=True),
+        ],
+        output="screen",
+        condition=UnlessCondition(use_fake_hardware),
+    )
+
+    dashboard_client_node = Node(
+        package="ur_robot_driver",
+        condition=IfCondition(
+            AndSubstitution(launch_dashboard_client, NotSubstitution(use_fake_hardware))
+        ),
+        executable="dashboard_client",
+        name="dashboard_client",
+        output="screen",
+        emulate_tty=True,
+        parameters=[{"robot_ip": robot_ip}],
+    )
+
+    robot_state_helper_node = Node(
+        package="ur_robot_driver",
+        executable="robot_state_helper",
+        name="ur_robot_state_helper",
+        output="screen",
+        parameters=[
+            {"headless_mode": headless_mode},
+        ],
+    )
+
+    tool_communication_node = Node(
+        package="ur_robot_driver",
+        condition=IfCondition(use_tool_communication),
+        executable="tool_communication.py",
+        name="ur_tool_comm",
+        output="screen",
+        parameters=[
+            {
+                "robot_ip": robot_ip,
+                "tcp_port": tool_tcp_port,
+                "device_name": tool_device_name,
+            }
+        ],
+    )
+
+    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",
+        name="controller_stopper",
+        output="screen",
+        emulate_tty=True,
+        condition=UnlessCondition(use_fake_hardware),
+        parameters=[
+            {"headless_mode": headless_mode},
+            {"joint_controller_active": activate_joint_controller},
+            {
+                "consistent_controllers": [
+                    "io_and_status_controller",
+                    "force_torque_sensor_broadcaster",
+                    "joint_state_broadcaster",
+                    "speed_scaling_state_broadcaster",
+                    "tcp_pose_broadcaster",
+                    "ur_configuration_controller",
+                ]
+            },
+        ],
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        condition=IfCondition(launch_rviz),
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+    )
+
+    # Spawn controllers
+    def controller_spawner(controllers, active=True):
+        inactive_flags = ["--inactive"] if not active else []
+        return Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=[
+                "--controller-manager",
+                "/controller_manager",
+                "--controller-manager-timeout",
+                controller_spawner_timeout,
+            ]
+            + inactive_flags
+            + controllers,
+        )
+
+    controllers_active = [
+        "joint_state_broadcaster",
+        "io_and_status_controller",
+        "speed_scaling_state_broadcaster",
+        "force_torque_sensor_broadcaster",
+        "tcp_pose_broadcaster",
+        "ur_configuration_controller",
+    ]
+    controllers_inactive = [
+        "scaled_joint_trajectory_controller",
+        "joint_trajectory_controller",
+        "forward_velocity_controller",
+        "forward_position_controller",
+        "force_mode_controller",
+        "passthrough_trajectory_controller",
+        "freedrive_mode_controller",
+    ]
+    if activate_joint_controller.perform(context) == "true":
+        controllers_active.append(initial_joint_controller.perform(context))
+        controllers_inactive.remove(initial_joint_controller.perform(context))
+
+    if use_fake_hardware.perform(context) == "true":
+        controllers_active.remove("tcp_pose_broadcaster")
+
+    controller_spawners = [
+        controller_spawner(controllers_active),
+        controller_spawner(controllers_inactive, active=False),
+    ]
+
+    nodes_to_start = [
+        control_node,
+        ur_control_node,
+        dashboard_client_node,
+        robot_state_helper_node,
+        tool_communication_node,
+        controller_stopper_node,
+        urscript_interface,
+        robot_state_publisher_node,
+        rviz_node,
+    ] + controller_spawners
+
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = []
+    # UR specific arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_type",
+            description="Type/series of used UR robot.",
+            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"],
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip", description="IP address by which the robot can be reached."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_limits",
+            default_value="true",
+            description="Enables the safety limits controller if true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_pos_margin",
+            default_value="0.15",
+            description="The margin to lower and upper limits in the safety controller.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_k_position",
+            default_value="20",
+            description="k-position factor in the safety controller.",
+        )
+    )
+    # General arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "runtime_config_package",
+            default_value="ur_robot_driver",
+            description='Package with the controller\'s configuration in "config" folder. '
+            "Usually the argument is not set, it enables use of a custom setup.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controllers_file",
+            default_value="ur_controllers.yaml",
+            description="YAML file with the controllers configuration.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="ur_description",
+            description="Description package with robot URDF/XACRO files. Usually the argument "
+            "is not set, it enables use of a custom description.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_file",
+            default_value="ur.urdf.xacro",
+            description="URDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "kinematics_params_file",
+            default_value=PathJoinSubstitution(
+                [
+                    FindPackageShare(LaunchConfiguration("description_package")),
+                    "config",
+                    LaunchConfiguration("ur_type"),
+                    "default_kinematics.yaml",
+                ]
+            ),
+            description="The calibration configuration of the actual robot used.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="",
+            description="tf_prefix of the joint names, useful for "
+            "multi-robot setup. If changed, also joint names in the controllers' configuration "
+            "have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "headless_mode",
+            default_value="false",
+            description="Enable headless mode for robot control",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_spawner_timeout",
+            default_value="10",
+            description="Timeout used when spawning controllers.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+                "freedrive_mode_controller",
+                "passthrough_trajectory_controller",
+            ],
+            description="Initially loaded robot controller.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "activate_joint_controller",
+            default_value="true",
+            description="Activate loaded joint controller.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_dashboard_client", default_value="true", description="Launch Dashboard Client?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_tool_communication",
+            default_value="false",
+            description="Only available for e series!",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_parity",
+            default_value="0",
+            description="Parity configuration for serial communication. Only effective, if "
+            "use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_baud_rate",
+            default_value="115200",
+            description="Baud rate configuration for serial communication. Only effective, if "
+            "use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_stop_bits",
+            default_value="1",
+            description="Stop bits configuration for serial communication. Only effective, if "
+            "use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_rx_idle_chars",
+            default_value="1.5",
+            description="RX idle chars configuration for serial communication. Only effective, "
+            "if use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_tx_idle_chars",
+            default_value="3.5",
+            description="TX idle chars configuration for serial communication. Only effective, "
+            "if use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_device_name",
+            default_value="/tmp/ttyUR",
+            description="File descriptor that will be generated for the tool communication device. "
+            "The user has be be allowed to write to this location. "
+            "Only effective, if use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_tcp_port",
+            default_value="54321",
+            description="Remote port that will be used for bridging the tool's serial device. "
+            "Only effective, if use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_voltage",
+            default_value="0",  # 0 being a conservative value that won't destroy anything
+            description="Tool voltage that will be setup.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "reverse_ip",
+            default_value="0.0.0.0",
+            description="IP that will be used for the robot controller to communicate back to the driver.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "script_command_port",
+            default_value="50004",
+            description="Port that will be opened to forward URScript commands to the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "reverse_port",
+            default_value="50001",
+            description="Port that will be opened to send cyclic instructions from the driver to the robot controller.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "script_sender_port",
+            default_value="50002",
+            description="The driver will offer an interface to query the external_control URScript on this port.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "trajectory_port",
+            default_value="50003",
+            description="Port that will be opened for trajectory control.",
+        )
+    )
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

launch/ur_dashboard_client.launch.py

@@ -0,0 +1,58 @@
+# Copyright (c) 2021, PickNik, Inc.
+#
+# 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.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+
+    dashboard_client_node = Node(
+        package="ur_robot_driver",
+        executable="dashboard_client",
+        name="dashboard_client",
+        output="screen",
+        emulate_tty=True,
+        parameters=[{"robot_ip": robot_ip}],
+    )
+
+    return LaunchDescription(declared_arguments + [dashboard_client_node])

package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ur_robot_driver</name>
+  <version>2.6.0</version>
+  <description>The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series.</description>
+
+  <maintainer email="feex@universal-robots.com">Felix Exner</maintainer>
+  <maintainer email="rsk@universal-robots.com">Rune Søe-Knudsen</maintainer>
+  <maintainer email="ros@universal-robots.com">Universal Robots A/S</maintainer>
+
+  <license>BSD-3-Clause</license>
+
+  <url type="bugtracker">https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues</url>
+  <url type="repository">https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver</url>
+
+  <author email="denis@stoglrobotics.de">Denis Stogl</author>
+  <author email="grosse@fzi.de">Marvin Große Besselmann</author>
+  <author email="lovro.ivanov@gmail.com">Lovro Ivanov</author>
+  <author email="zelenak@picknik.ai">Andy Zelenak</author>
+  <author email="dipentima@fzi.de">Vincenzo Di Pentima</author>
+  <author>Thomas Timm Andersen</author>
+  <author>Simon Rasmussen</author>
+  <author>Felix Exner</author>
+  <author>Lea Steffen</author>
+  <author>Tristan Schnell</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>backward_ros</depend>
+  <depend>controller_manager</depend>
+  <depend>controller_manager_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>ur_client_library</depend>
+  <depend>ur_controllers</depend>
+  <depend>ur_dashboard_msgs</depend>
+  <depend>ur_description</depend>
+  <depend>ur_msgs</depend>
+
+  <exec_depend>force_torque_sensor_broadcaster</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>pose_broadcaster</exec_depend>
+  <exec_depend>position_controllers</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros2_controllers_test_nodes</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>socat</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>velocity_controllers</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>launch_testing_ament_cmake</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

resources/ros_control.urscript

@@ -0,0 +1,143 @@
+# HEADER_BEGIN
+
+{{BEGIN_REPLACE}}
+
+steptime = get_steptime()
+
+textmsg("ExternalControl: steptime=", steptime)
+MULT_jointstate = {{JOINT_STATE_REPLACE}}
+
+#Constants
+SERVO_UNINITIALIZED = -1
+SERVO_IDLE = 0
+SERVO_RUNNING = 1
+
+MODE_STOPPED = -2
+MODE_UNINITIALIZED = -1
+MODE_IDLE = 0
+MODE_SERVOJ = 1
+MODE_SPEEDJ = 2
+
+#Global variables are also showed in the Teach pendants variable list
+global cmd_servo_state = SERVO_UNINITIALIZED
+global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+global cmd_servo_q = get_actual_joint_positions()
+global cmd_servo_q_last = get_actual_joint_positions()
+global extrapolate_count = 0
+global extrapolate_max_count = 0
+global control_mode = MODE_UNINITIALIZED
+cmd_speedj_active = True
+
+def set_servo_setpoint(q):
+  cmd_servo_state = SERVO_RUNNING
+  cmd_servo_q_last = cmd_servo_q
+  cmd_servo_q = q
+end
+
+def extrapolate():
+  diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
+  cmd_servo_q_last = cmd_servo_q
+  cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
+
+  return cmd_servo_q
+end
+
+thread servoThread():
+  textmsg("ExternalControl: Starting servo thread")
+  state = SERVO_IDLE
+  while control_mode == MODE_SERVOJ:
+    enter_critical
+    q = cmd_servo_q
+    do_extrapolate = False
+    if (cmd_servo_state == SERVO_IDLE):
+      do_extrapolate = True
+    end
+    state = cmd_servo_state
+    if cmd_servo_state > SERVO_UNINITIALIZED:
+      cmd_servo_state = SERVO_IDLE
+    end
+
+    if do_extrapolate:
+      extrapolate_count = extrapolate_count + 1
+      if extrapolate_count > extrapolate_max_count:
+        extrapolate_max_count = extrapolate_count
+      end
+
+      q = extrapolate()
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+
+    elif state == SERVO_RUNNING:
+      extrapolate_count = 0
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+    else:
+      extrapolate_count = 0
+      sync()
+    end
+    exit_critical
+  end
+  textmsg("ExternalControl: servo thread ended")
+  stopj(4.0)
+end
+
+# Helpers for speed control
+def set_speed(qd):
+  cmd_servo_qd = qd
+  control_mode = MODE_SPEEDJ
+end
+
+thread speedThread():
+  textmsg("ExternalControl: Starting speed thread")
+  while control_mode == MODE_SPEEDJ:
+    qd = cmd_servo_qd
+    speedj(qd, 40.0, steptime)
+  end
+  textmsg("ExternalControl: speedj thread ended")
+  stopj(5.0)
+end
+
+# HEADER_END
+
+# NODE_CONTROL_LOOP_BEGINS
+
+socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
+
+control_mode = MODE_UNINITIALIZED
+thread_move = 0
+global keepalive = -2
+params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0)
+textmsg("ExternalControl: External control active")
+keepalive = params_mult[1]
+while keepalive > 0 and control_mode > MODE_STOPPED:
+  enter_critical
+  params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
+  if params_mult[0] > 0:
+    keepalive = params_mult[1]
+    if control_mode != params_mult[8]:
+      control_mode = params_mult[8]
+      join thread_move
+      if control_mode == MODE_SERVOJ:
+        thread_move = run servoThread()
+      elif control_mode == MODE_SPEEDJ:
+        thread_move = run speedThread()
+      end
+    end
+    if control_mode == MODE_SERVOJ:
+      q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
+      set_servo_setpoint(q)
+    elif control_mode == MODE_SPEEDJ:
+      qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
+      set_speed(qd)
+    end
+  else:
+    keepalive = keepalive - 1
+  end
+  exit_critical
+end
+
+textmsg("ExternalControl: Stopping communication and control")
+control_mode = MODE_STOPPED
+join thread_move
+textmsg("ExternalControl: All threads ended")
+socket_close("reverse_socket")
+
+# NODE_CONTROL_LOOP_ENDS

resources/rtde_input_recipe.txt

@@ -0,0 +1,12 @@
+speed_slider_mask
+speed_slider_fraction
+standard_digital_output_mask
+standard_digital_output
+configurable_digital_output_mask
+configurable_digital_output
+tool_digital_output_mask
+tool_digital_output
+standard_analog_output_mask
+standard_analog_output_type
+standard_analog_output_0
+standard_analog_output_1

resources/rtde_output_recipe.txt

@@ -0,0 +1,29 @@
+timestamp
+actual_q
+actual_qd
+speed_scaling
+target_speed_fraction
+runtime_state
+actual_TCP_force
+actual_TCP_pose
+target_TCP_pose
+actual_digital_input_bits
+actual_digital_output_bits
+standard_analog_input0
+standard_analog_input1
+standard_analog_output0
+standard_analog_output1
+analog_io_types
+tool_mode
+tool_analog_input_types
+tool_analog_input0
+tool_analog_input1
+tool_output_voltage
+tool_output_current
+tool_temperature
+robot_mode
+safety_mode
+robot_status_bits
+safety_status_bits
+actual_current
+tcp_offset

scripts/hi.py

@@ -0,0 +1,222 @@
+#!/usr/bin/env python3
+# Copyright (c) 2024 FZI Forschungszentrum Informatik
+#
+# 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.
+
+#
+# Author: Felix Exner
+
+# This is an example of how to interface the robot without any additional ROS components. For
+# real-life applications, we do recommend to use something like MoveIt!
+
+import time
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from builtin_interfaces.msg import Duration
+from action_msgs.msg import GoalStatus
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from control_msgs.action import FollowJointTrajectory
+from control_msgs.msg import JointTolerance
+
+TRAJECTORIES = {
+    "traj0": [
+        {
+            "positions": [-1.57, -2.07, 0.0, -1.04, 0.0, 0.0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=7, nanosec=0),
+        },
+        {
+            "positions": [-1.57, -1.04, 0.0, -2.35, 0.0, 0.0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=14, nanosec=0), 
+        },
+    ],
+    # "traj1": [
+    #     {
+    #         "positions": [-1.57, -1.04, 0.0, -2.35, 0.0, 0.0],
+    #         "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    #         "time_from_start": Duration(sec=0, nanosec=0),
+    #     },
+    #     {
+    #         "positions": [-1.57, -2.07, 0.0, -1.04, 0.0, 0.0],
+    #         "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    #         "time_from_start": Duration(sec=6, nanosec=0),
+    #     },
+    # ],
+}
+
+
+class JTCClient(Node):
+    """Small test client for the jtc."""
+
+    def __init__(self, name):
+        super().__init__(name)
+        self.declare_parameter("controller_name", "scaled_joint_trajectory_controller")
+        self.declare_parameter(
+            "joints",
+            [
+                "shoulder_pan_joint",
+                "shoulder_lift_joint",
+                "elbow_joint",
+                "wrist_1_joint",
+                "wrist_2_joint",
+                "wrist_3_joint",
+            ],
+        )
+
+        controller_name = self.get_parameter("controller_name").value + "/follow_joint_trajectory"
+        self.joints = self.get_parameter("joints").value
+
+        if self.joints is None or len(self.joints) == 0:
+            raise Exception('"joints" parameter is required')
+
+        self._action_client = ActionClient(self, FollowJointTrajectory, controller_name)
+        self.get_logger().info(f"Waiting for action server on {controller_name}")
+        self._action_client.wait_for_server()
+
+        self.parse_trajectories()
+        self.i = 0
+        self._send_goal_future = None
+        self._get_result_future = None
+        self.execute_next_trajectory()
+
+    def parse_trajectories(self):
+        self.goals = {}
+
+        for traj_name in TRAJECTORIES:
+            goal = JointTrajectory()
+            goal.joint_names = self.joints
+            for pt in TRAJECTORIES[traj_name]:
+                point = JointTrajectoryPoint()
+                point.positions = pt["positions"]
+                point.velocities = pt["velocities"]
+                point.time_from_start = pt["time_from_start"]
+                goal.points.append(point)
+
+            self.goals[traj_name] = goal
+
+    def execute_next_trajectory(self):
+        if self.i >= len(self.goals):
+            self.get_logger().info("Done with all trajectories")
+            raise SystemExit
+        traj_name = list(self.goals)[self.i]
+        self.i = self.i + 1
+        if traj_name:
+            self.execute_trajectory(traj_name)
+
+    def execute_trajectory(self, traj_name):
+        self.get_logger().info(f"Executing trajectory {traj_name}")
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory = self.goals[traj_name]
+
+        goal.goal_time_tolerance = Duration(sec=10, nanosec=0)
+        goal.goal_tolerance = [
+            JointTolerance(position=0.05, velocity=0.05, name=self.joints[i]) for i in range(6)
+        ]
+
+        self._send_goal_future = self._action_client.send_goal_async(goal)
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().error("Goal rejected :(")
+            raise RuntimeError("Goal rejected :(")
+
+        self.get_logger().debug("Goal accepted :)")
+
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.get_result_callback)
+
+    def get_result_callback(self, future):
+        result = future.result().result
+        status = future.result().status
+        self.get_logger().info(f"Done with result: {self.status_to_str(status)}")
+        if status == GoalStatus.STATUS_SUCCEEDED:
+            # time.sleep(2)
+            self.execute_next_trajectory()
+        else:
+            if result.error_code != FollowJointTrajectory.Result.SUCCESSFUL:
+                self.get_logger().error(
+                    f"Done with result: {self.error_code_to_str(result.error_code)}"
+                )
+            raise RuntimeError("Executing trajectory failed. " + result.error_string)
+
+    @staticmethod
+    def error_code_to_str(error_code):
+        if error_code == FollowJointTrajectory.Result.SUCCESSFUL:
+            return "SUCCESSFUL"
+        if error_code == FollowJointTrajectory.Result.INVALID_GOAL:
+            return "INVALID_GOAL"
+        if error_code == FollowJointTrajectory.Result.INVALID_JOINTS:
+            return "INVALID_JOINTS"
+        if error_code == FollowJointTrajectory.Result.OLD_HEADER_TIMESTAMP:
+            return "OLD_HEADER_TIMESTAMP"
+        if error_code == FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED:
+            return "PATH_TOLERANCE_VIOLATED"
+        if error_code == FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED:
+            return "GOAL_TOLERANCE_VIOLATED"
+
+    @staticmethod
+    def status_to_str(error_code):
+        if error_code == GoalStatus.STATUS_UNKNOWN:
+            return "UNKNOWN"
+        if error_code == GoalStatus.STATUS_ACCEPTED:
+            return "ACCEPTED"
+        if error_code == GoalStatus.STATUS_EXECUTING:
+            return "EXECUTING"
+        if error_code == GoalStatus.STATUS_CANCELING:
+            return "CANCELING"
+        if error_code == GoalStatus.STATUS_SUCCEEDED:
+            return "SUCCEEDED"
+        if error_code == GoalStatus.STATUS_CANCELED:
+            return "CANCELED"
+        if error_code == GoalStatus.STATUS_ABORTED:
+            return "ABORTED"
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    jtc_client = JTCClient('jtc_client')
+    try:
+        rclpy.spin(jtc_client)
+    except RuntimeError as err:
+        jtc_client.get_logger().error(str(err))
+    except SystemExit:
+        rclpy.logging.get_logger("jtc_client").info("Done")
+    jtc_client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    
+    while True:
+        main()

scripts/start_ursim.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+
+# copyright 2022 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.
+
+ORANGE='\033[0;33m'
+NC='\033[0m' # No Color
+
+URSIM_CMD="ros2 run ur_client_library start_ursim.sh"
+
+echo -e "${ORANGE} DEPRECATION WARNING: The script starting URSim was moved to the ur_client_library package. This script here does still work, but will be removed with ROS Jazzy. Please use `${URSIM_CMD}` to start URSim in future."
+$URSIM_CMD

scripts/tool_communication.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+# Copyright (c) 2021 PickNik, Inc.
+#
+# 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.
+
+
+"""Small helper script to start the tool communication interface."""
+
+import subprocess
+
+import rclpy.logging
+from rclpy.node import Node
+
+
+class UrToolCommunication(Node):
+    """Starts socat."""
+
+    def __init__(self):
+        super().__init__("ur_tool_communication")
+        # IP address of the robot
+        self.declare_parameter("robot_ip", "192.168.56.101")
+        robot_ip = self.get_parameter("robot_ip").get_parameter_value().string_value
+        self.get_logger().info(robot_ip)
+        # Port on which the remote pc (robot) publishes the interface
+        self.declare_parameter("tcp_port", 54321)
+        tcp_port = self.get_parameter_or("tcp_port", 54321).get_parameter_value().integer_value
+        # By default, socat will create a pty in /dev/pts/N with n being an increasing number.
+        # Additionally, a symlink at the given location will be created. Use an absolute path here.
+        self.declare_parameter("device_name", "/tmp/ttyUR")
+        local_device = self.get_parameter("device_name").get_parameter_value().string_value
+
+        self.get_logger().info("Remote device is available at '" + local_device + "'")
+
+        cfg_params = ["pty"]
+        cfg_params.append("link=" + local_device)
+        cfg_params.append("raw")
+        cfg_params.append("ignoreeof")
+        cfg_params.append("waitslave")
+
+        cmd = ["socat"]
+        cmd.append(",".join(cfg_params))
+        cmd.append(":".join(["tcp", robot_ip, str(tcp_port)]))
+
+        self.get_logger().info("Starting socat with following command:\n" + " ".join(cmd))
+        subprocess.call(cmd)
+
+
+def main():
+    rclpy.init()
+    node = UrToolCommunication()
+    rclpy.spin(node)
+
+
+if __name__ == "__main__":
+    main()

scripts/ur3e_actions.py

@@ -0,0 +1,278 @@
+#!/usr/bin/env python3
+# Copyright (c) 2024 FZI Forschungszentrum Informatik
+#
+# 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.
+
+#
+# Author: Felix Exner
+
+# This is an example of how to interface the robot without any additional ROS components. For
+# real-life applications, we do recommend to use something like MoveIt!
+
+import time
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from builtin_interfaces.msg import Duration
+from action_msgs.msg import GoalStatus
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from control_msgs.action import FollowJointTrajectory
+from control_msgs.msg import JointTolerance
+
+TRAJECTORIES = {
+    "traj0": [
+        {
+            "positions": [0.785, -1.57, 0.785, 0.785, 0.785, 0.785],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=6, nanosec=0),
+        },
+        {
+            "positions": [0.0, -1.57, 0.0, 0.0, 0.0, 0.0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=12, nanosec=0), 
+        },
+    ],
+    "traj1": [
+        {
+            "positions": [0.0, -1.57, 0.0, 0.0, 0.0, 0.0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=0, nanosec=0),
+        },
+        {
+            "positions": [0.0, -1.57, 0.0, 0.0, -0.785, 0.0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=5, nanosec=0),
+        },
+    ],
+    "traj2": [
+        {
+            "positions": [0.0, -1.57, 0.0, 0.0, -0.785, 0.0], 
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=0, nanosec=0),
+        },
+        {
+            "positions": [0.0, -1.57, 0.0, 0.0, 0.0, 0.0], 
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            "time_from_start": Duration(sec=6, nanosec=0),
+        },
+    ],
+    # "traj0": [
+    #     {
+    #         "positions": [-4.3, -3.0, 0.0, -1.57, 1.57, 0.67],
+    #         "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    #         "time_from_start": Duration(sec=15, nanosec=0),
+    #     },
+    #     {
+    #         "positions": [0.0, -1.57, 0.0, 0.0, 0.0, 0.0],
+    #         "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    #         "time_from_start": Duration(sec=30, nanosec=0), 
+    #     },
+    # ],
+}
+
+
+class JTCClient(Node):
+    """Small test client for the jtc."""
+
+    def __init__(self, name):
+        super().__init__(name)
+        self.declare_parameter("controller_name", "scaled_joint_trajectory_controller")
+        self.declare_parameter(
+            "joints",
+            [
+                "shoulder_pan_joint",
+                "shoulder_lift_joint",
+                "elbow_joint",
+                "wrist_1_joint",
+                "wrist_2_joint",
+                "wrist_3_joint",
+            ],
+        )
+
+        controller_name = self.get_parameter("controller_name").value + "/follow_joint_trajectory"
+        self.joints = self.get_parameter("joints").value
+
+        if self.joints is None or len(self.joints) == 0:
+            raise Exception('"joints" parameter is required')
+
+        self._action_client = ActionClient(self, FollowJointTrajectory, controller_name)
+        self.get_logger().info(f"Waiting for action server on {controller_name}")
+        self._action_client.wait_for_server()
+
+        self.parse_trajectories()
+        self.i = 0
+        self._send_goal_future = None
+        self._get_result_future = None
+        # wait fot the previous loop succdessful.
+        # self.rclpy.sleep(2)
+        # this will execute immediately after you call this node - turn it off for now so that it will execute it while camera detect sth.
+        #-----------------------------------------------------------------------------------------------
+        # self.execute_next_trajectory()
+
+    def parse_trajectories(self):
+        self.goals = {}
+
+        for traj_name in TRAJECTORIES:
+            goal = JointTrajectory()
+            goal.joint_names = self.joints
+            for pt in TRAJECTORIES[traj_name]:
+                point = JointTrajectoryPoint()
+                point.positions = pt["positions"]
+                point.velocities = pt["velocities"]
+                point.time_from_start = pt["time_from_start"]
+                goal.points.append(point)
+
+            self.goals[traj_name] = goal
+
+    def execute_next_trajectory(self):
+        if self.i >= len(self.goals):
+            self.get_logger().info("Done with all trajectories")
+            raise SystemExit
+        traj_name = list(self.goals)[self.i]
+        self.i = self.i + 1
+        if traj_name:
+            self.execute_trajectory(traj_name)
+
+    
+    def execute_trajectory(self, traj_name):
+        self.get_logger().info(f"Executing trajectory {traj_name}")
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory = self.goals[traj_name]
+
+        # ───── send goal ─────────────────────────────────────────
+        self._send_goal_future = self._action_client.send_goal_async(goal)
+
+        # === create future for returning ====================
+        result_future = rclpy.task.Future()
+
+        def _on_goal_response(send_fut):
+            """after receiving GoalHandle, wait for real result"""
+            goal_handle = send_fut.result()
+
+            # if canceled，put the exception into result_future
+            if not goal_handle.accepted:
+                result_future.set_exception(RuntimeError("Goal rejected :("))
+                return
+
+            # let old result callback still working（if wanna keep it for log）
+            self.goal_response_callback(send_fut)
+
+            # wait for all the trajectory movement ends.
+            get_result_fut = goal_handle.get_result_async()
+
+            # give me the inner future to the one for returning
+            def _on_result(res_fut):
+                result_future.set_result(res_fut.result())
+            get_result_fut.add_done_callback(_on_result)
+
+        # after giving goal wait for goal to response
+        self._send_goal_future.add_done_callback(_on_goal_response)
+
+        # -------★ return this future ★--------------------------
+        return result_future
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().error("Goal rejected :(")
+            raise RuntimeError("Goal rejected :(")
+
+        self.get_logger().debug("Goal accepted :)")
+
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.get_result_callback)
+
+    def get_result_callback(self, future):
+        result = future.result().result
+        status = future.result().status
+        self.get_logger().info(f"Done with result: {self.status_to_str(status)}")
+
+        # if this don't comment - it will automatically run the next movement until no traj anymore.
+        #-----------------------------------------------------------------------------------------------
+        # if status == GoalStatus.STATUS_SUCCEEDED:
+        #     # time.sleep(2)
+        #     self.execute_next_trajectory()
+        # else:
+        #     if result.error_code != FollowJointTrajectory.Result.SUCCESSFUL:
+        #         self.get_logger().error(
+        #             f"Done with result: {self.error_code_to_str(result.error_code)}"
+        #         )
+        #     raise RuntimeError("Executing trajectory failed. " + result.error_string)
+
+    @staticmethod
+    def error_code_to_str(error_code):
+        if error_code == FollowJointTrajectory.Result.SUCCESSFUL:
+            return "SUCCESSFUL"
+        if error_code == FollowJointTrajectory.Result.INVALID_GOAL:
+            return "INVALID_GOAL"
+        if error_code == FollowJointTrajectory.Result.INVALID_JOINTS:
+            return "INVALID_JOINTS"
+        if error_code == FollowJointTrajectory.Result.OLD_HEADER_TIMESTAMP:
+            return "OLD_HEADER_TIMESTAMP"
+        if error_code == FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED:
+            return "PATH_TOLERANCE_VIOLATED"
+        if error_code == FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED:
+            return "GOAL_TOLERANCE_VIOLATED"
+
+    @staticmethod
+    def status_to_str(error_code):
+        if error_code == GoalStatus.STATUS_UNKNOWN:
+            return "UNKNOWN"
+        if error_code == GoalStatus.STATUS_ACCEPTED:
+            return "ACCEPTED"
+        if error_code == GoalStatus.STATUS_EXECUTING:
+            return "EXECUTING"
+        if error_code == GoalStatus.STATUS_CANCELING:
+            return "CANCELING"
+        if error_code == GoalStatus.STATUS_SUCCEEDED:
+            return "SUCCEEDED"
+        if error_code == GoalStatus.STATUS_CANCELED:
+            return "CANCELED"
+        if error_code == GoalStatus.STATUS_ABORTED:
+            return "ABORTED"
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    jtc_client = JTCClient('ur3e_actions')
+    try:
+        rclpy.spin(jtc_client)
+    except RuntimeError as err:
+        jtc_client.get_logger().error(str(err))
+    except SystemExit:
+        rclpy.logging.get_logger("jtc_client").info("Done")
+
+    jtc_client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    
+    main()

scripts/ur3e_topic.py

@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from builtin_interfaces.msg import Duration
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class UnifiedTrajectoryPublisher(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        
+        self.controller_name = "scaled_joint_trajectory_controller"  # 或 "joint_trajectory_controller" could also exchange to oother controller
+        self.wait_sec_between_publish = 6
+        self.goal_name = ["pos1", "pos2"] # can also add pos3, 4 and so on
+        self.joints = [
+            "shoulder_pan_joint",
+            "shoulder_lift_joint",
+            "elbow_joint",
+            "wrist_1_joint",
+            "wrist_2_joint",
+            "wrist_3_joint"
+        ]
+        self.check_starting_point = False  # if we didn't check the starting poin，set it to False
+
+        # if we need to chekc the starting point limit, can set it here -> but not really need here (the setting here is 0, 90, 0, 90, 0, 0) which is the HOME position
+        self.starting_point_limits = {
+            "shoulder_pan_joint": [-0.1, 0.1], # this interval is -0.1 <= x <= 0.1 joint is able to work within this interval
+            "shoulder_lift_joint": [-1.6, -1.5],
+            "elbow_joint": [-0.1, 0.1],
+            "wrist_1_joint": [-1.6, -1.5],
+            "wrist_2_joint": [-0.1, 0.1],
+            "wrist_3_joint": [-0.1, 0.1],
+        }
+        
+        # 宏義各個目標
+        self.declare_parameter("Joints1", [-1.57, -2.07, 0.0, -1.04, 0.0, 0.0])
+        self.declare_parameter("Joints2", [-1.57, -1.04, 0.0, -2.35, 0.0, 0.0])
+        
+
+        self.goals_dict = {
+            "pos1": {"positions": self.get_parameter("Joints1").value},
+            "pos2": {"positions": self.get_parameter("Joints2").value},
+            # "pos3": {"positions": [0.0, -1.57, 0.0, 0.0, -0.785, 0.0]},
+            # "pos4": {"positions": [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]},
+        }
+        
+        # read the target and create JointTrajectoryPoint
+        self.goals = []
+        for goal in self.goal_name:
+            goal_data = self.goals_dict.get(goal, {})
+            positions = goal_data.get("positions", [])
+            if len(positions) == len(self.joints): # length of position should == how many joints (each joints should have one position)
+
+                point = JointTrajectoryPoint()
+                point.positions = positions
+                point.time_from_start = Duration(sec=self.wait_sec_between_publish, nanosec=0)
+                self.goals.append(point)
+
+                self.get_logger().info(f"Loaded goal {goal}: {positions}")
+            else:
+
+                self.get_logger().warn(f"Goal {goal} positions not defined properly.")
+         
+        if len(self.goals) == 0:
+            self.get_logger().error("No valid goal found. Exiting...")
+            exit(1)
+
+        # set Publisher，chosse the correct topic that is going to publish
+        publish_topic = "/" + self.controller_name + "/" + "joint_trajectory"
+        self.get_logger().info(f"Publishing {len(self.goals)} goals on topic '{publish_topic}' every {self.wait_sec_between_publish} second")
+        self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 10)
+        self.timer = self.create_timer(self.wait_sec_between_publish, self.timer_callback)
+        self.current_goal_index = 0
+
+    def timer_callback(self):
+        traj = JointTrajectory()
+        traj.joint_names = self.joints
+        traj.points.append(self.goals[self.current_goal_index])
+        self.publisher_.publish(traj)
+        self.get_logger().info(f"Published goal: {self.goals[self.current_goal_index].positions}")
+        self.current_goal_index = (self.current_goal_index + 1) % len(self.goals)
+
+def main(args=None):
+    
+    rclpy.init(args=args)
+    node = UnifiedTrajectoryPublisher("ur3e_topic")
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("shutting down...")
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    
+    main()

scripts/wait_dashboard_server.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+netcat -z 192.168.56.101 29999
+while [ $? -eq 1 ]
+do
+    echo "Dashboard server not accepting connections..."
+    sleep 3
+    netcat -z 192.168.56.101 29999
+done
+echo "Dashboard server connections are possible."
+sleep 5

src/controller_stopper.cpp

@@ -0,0 +1,180 @@
+// Copyright 2019, 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 exner@fzi.de
+ * \date    2019-06-12
+ *
+ * \author  Mads Holm Peters
+ * \date    2022-02-25
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <rclcpp/utilities.hpp>
+
+#include "ur_robot_driver/controller_stopper.hpp"
+
+ControllerStopper::ControllerStopper(const rclcpp::Node::SharedPtr& node, bool stop_controllers_on_startup)
+  : node_(node), stop_controllers_on_startup_(stop_controllers_on_startup), robot_running_(true)
+{
+  // Subscribes to a robot's running state topic. Ideally this topic is latched and only publishes
+  // on changes. However, this node only reacts on state changes, so a state published each cycle
+  // would also be fine.
+  robot_running_sub_ = node->create_subscription<std_msgs::msg::Bool>(
+      "io_and_status_controller/robot_program_running", 1,
+      std::bind(&ControllerStopper::robotRunningCallback, this, std::placeholders::_1));
+
+  // Controller manager service to switch controllers
+  controller_manager_srv_ = node_->create_client<controller_manager_msgs::srv::SwitchController>("controller_manager/"
+                                                                                                 "switch_controller");
+  // Controller manager service to list controllers
+  controller_list_srv_ = node_->create_client<controller_manager_msgs::srv::ListControllers>("controller_manager/"
+                                                                                             "list_controllers");
+
+  RCLCPP_INFO(rclcpp::get_logger("Controller stopper"), "Waiting for switch controller service to come up on "
+                                                        "controller_manager/switch_controller");
+  controller_manager_srv_->wait_for_service();
+  RCLCPP_INFO(rclcpp::get_logger("Controller stopper"), "Service available");
+  RCLCPP_INFO(rclcpp::get_logger("Controller stopper"), "Waiting for list controllers service to come up on "
+                                                        "controller_manager/list_controllers");
+  controller_list_srv_->wait_for_service();
+  RCLCPP_INFO(rclcpp::get_logger("Controller stopper"), "Service available");
+
+  consistent_controllers_ = node_->declare_parameter<std::vector<std::string>>("consistent_controllers");
+
+  if (stop_controllers_on_startup_ == true) {
+    while (stopped_controllers_.empty()) {
+      auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>();
+      auto future = controller_list_srv_->async_send_request(request);
+      rclcpp::spin_until_future_complete(node_, future);
+      auto result = future.get();
+      for (auto& controller : result->controller) {
+        // Check if in consistent_controllers
+        // Else:
+        //   Add to stopped_controllers
+        if (controller.state == "active") {
+          auto it = std::find(consistent_controllers_.begin(), consistent_controllers_.end(), controller.name);
+          if (it == consistent_controllers_.end()) {
+            stopped_controllers_.push_back(controller.name);
+          }
+        }
+      }
+      rclcpp::sleep_for(std::chrono::milliseconds(100));
+    }
+    auto request_switch_controller = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+    request_switch_controller->strictness = request_switch_controller->STRICT;
+    request_switch_controller->deactivate_controllers = stopped_controllers_;
+    auto future = controller_manager_srv_->async_send_request(request_switch_controller);
+    rclcpp::spin_until_future_complete(node_, future);
+    if (future.get()->ok == false) {
+      RCLCPP_ERROR(rclcpp::get_logger("Controller stopper"), "Could not deactivate requested controllers");
+    }
+    robot_running_ = false;
+  }
+}
+
+void ControllerStopper::findAndStopControllers()
+{
+  stopped_controllers_.clear();
+  auto request_switch_controller = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+  auto request_list_controllers = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>();
+
+  // Callback to switch controllers
+  auto callback_switch_controller =
+      [this](rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture future_response) {
+        auto result = future_response.get();
+        if (result->ok == false) {
+          RCLCPP_ERROR(rclcpp::get_logger("Controller stopper"), "Could not deactivate requested controllers");
+        }
+      };
+
+  // Callback to list controllers
+  auto callback_list_controller =
+      [this, request_switch_controller, callback_switch_controller](
+          rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedFuture future_response) {
+        auto result = future_response.get();
+        for (auto& controller : result->controller) {
+          // Check if in consistent_controllers
+          // Else:
+          //   Add to stopped_controllers
+          if (controller.state == "active") {
+            auto it = std::find(consistent_controllers_.begin(), consistent_controllers_.end(), controller.name);
+            if (it == consistent_controllers_.end()) {
+              stopped_controllers_.push_back(controller.name);
+            }
+          }
+        }
+        request_switch_controller->strictness = request_switch_controller->STRICT;
+        if (!stopped_controllers_.empty()) {
+          request_switch_controller->deactivate_controllers = stopped_controllers_;
+          auto future =
+              controller_manager_srv_->async_send_request(request_switch_controller, callback_switch_controller);
+        }
+      };
+
+  auto future = controller_list_srv_->async_send_request(request_list_controllers, callback_list_controller);
+}
+
+void ControllerStopper::startControllers()
+{
+  // Callback to switch controllers
+  auto callback = [this](rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture future_response) {
+    auto result = future_response.get();
+    if (result->ok == false) {
+      RCLCPP_ERROR(rclcpp::get_logger("Controller stopper"), "Could not activate requested controllers");
+    }
+  };
+  if (!stopped_controllers_.empty()) {
+    auto request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+    request->strictness = request->STRICT;
+    request->activate_controllers = stopped_controllers_;
+    auto future = controller_manager_srv_->async_send_request(request, callback);
+  }
+}
+
+void ControllerStopper::robotRunningCallback(const std_msgs::msg::Bool::ConstSharedPtr msg)
+{
+  RCLCPP_DEBUG(rclcpp::get_logger("Controller stopper"), "robotRunningCallback with data %d", msg->data);
+
+  if (msg->data && !robot_running_) {
+    RCLCPP_DEBUG(rclcpp::get_logger("Controller stopper"), "Starting controllers");
+    startControllers();
+  } else if (!msg->data && robot_running_) {
+    RCLCPP_DEBUG(rclcpp::get_logger("Controller stopper"), "Stopping controllers");
+    // stop all controllers except the once in consistent_controllers_
+    findAndStopControllers();
+  }
+  robot_running_ = msg->data;
+}

src/controller_stopper_node.cpp

@@ -0,0 +1,66 @@
+// Copyright 2019, 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 exner@fzi.de
+ * \date    2019-06-12
+ *
+ * \author  Mads Holm Peters
+ * \date    2022-02-25
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "rclcpp/rclcpp.hpp"
+#include "ur_robot_driver/controller_stopper.hpp"
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("controller_stopper_node");
+
+  bool headless_mode = node->declare_parameter<bool>("headless_mode", false);
+  node->get_parameter<bool>("headless_mode", headless_mode);
+  bool joint_controller_active = node->declare_parameter<bool>("joint_controller_active", true);
+  node->get_parameter<bool>("joint_controller_active", joint_controller_active);
+
+  // If headless mode is not active, but the joint controllers are we should stop the joint controllers during startup
+  // of the node
+  bool stop_controllers_on_startup = false;
+  if (joint_controller_active == true && headless_mode == false) {
+    stop_controllers_on_startup = true;
+  }
+
+  ControllerStopper stopper(node, stop_controllers_on_startup);
+
+  rclcpp::spin(node);
+
+  return 0;
+}

src/dashboard_client_node.cpp

@@ -0,0 +1,61 @@
+// Copyright 2019, FZI Forschungszentrum Informatik
+//
+// 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 exner@fzi.de
+ * \date    2019-10-21
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_robot_driver/dashboard_client_ros.hpp"
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "ur_robot_driver/urcl_log_handler.hpp"
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("ur_dashboard_client");
+
+  // The IP address under which the robot is reachable.
+  std::string robot_ip = node->declare_parameter<std::string>("robot_ip", "192.168.56.101");
+  node->get_parameter<std::string>("robot_ip", robot_ip);
+
+  ur_robot_driver::registerUrclLogHandler("");  // Set empty tf_prefix at the moment
+
+  ur_robot_driver::DashboardClientROS client(node, robot_ip);
+
+  rclcpp::spin(node);
+
+  return 0;
+}

src/dashboard_client_ros.cpp

@@ -0,0 +1,402 @@
+// Copyright 2019, FZI Forschungszentrum Informatik
+//
+// 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 exner@fzi.de
+ * \date    2019-10-21
+ * \author  Marvin Große Besselmann grosse@fzi.de
+ * \date    2021-03-22
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_robot_driver/dashboard_client_ros.hpp>
+
+#include <string>
+
+namespace ur_robot_driver
+{
+DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, const std::string& robot_ip)
+  : node_(node), client_(robot_ip)
+{
+  node_->declare_parameter<double>("receive_timeout", 1);
+  connect();
+
+  // Service to release the brakes. If the robot is currently powered off, it will get powered on on the fly.
+  brake_release_service_ = createDashboardTriggerSrv("~/brake_release", "brake release\n", "Brake releasing");
+
+  // If this service is called the operational mode can again be changed from PolyScope, and the user password is
+  // enabled.
+  clear_operational_mode_service_ = createDashboardTriggerSrv("~/clear_operational_mode", "clear operational mode\n",
+                                                              "No longer controlling the operational mode\\. "
+                                                              "Current "
+                                                              "operational mode: "
+                                                              "'(MANUAL|AUTOMATIC)'\\.");
+
+  // Close a (non-safety) popup on the teach pendant.
+  close_popup_service_ = createDashboardTriggerSrv("~/close_popup", "close popup\n", "closing popup");
+
+  // Close a safety popup on the teach pendant.
+  close_safety_popup_service_ =
+      createDashboardTriggerSrv("~/close_safety_popup", "close safety popup\n", "closing safety popup");
+
+  // Pause a running program.
+  pause_service_ = createDashboardTriggerSrv("~/pause", "pause\n", "Pausing program");
+
+  // Start execution of a previously loaded program
+  play_service_ = createDashboardTriggerSrv("~/play", "play\n", "Starting program");
+
+  // Power off the robot motors
+  power_off_service_ = createDashboardTriggerSrv("~/power_off", "power off\n", "Powering off");
+
+  // Power on the robot motors. To fully start the robot, call 'brake_release' afterwards.
+  power_on_service_ = createDashboardTriggerSrv("~/power_on", "power on\n", "Powering on");
+
+  // Used when robot gets a safety fault or violation to restart the safety. After safety has been rebooted the robot
+  // will be in Power Off. NOTE: You should always ensure it is okay to restart the system. It is highly recommended to
+  // check the error log before using this command (either via PolyScope or e.g. ssh connection).
+  restart_safety_service_ = createDashboardTriggerSrv("~/restart_safety", "restart safety\n", "Restarting safety");
+
+  // Shutdown the robot controller
+  shutdown_service_ = createDashboardTriggerSrv("~/shutdown", "shutdown\n", "Shutting down");
+
+  // Stop program execution on the robot
+  stop_service_ = createDashboardTriggerSrv("~/stop", "stop\n", "Stopped");
+
+  // Dismiss a protective stop to continue robot movements. NOTE: It is the responsibility of the user to ensure the
+  // cause of the protective stop is resolved before calling this service.
+  unlock_protective_stop_service_ =
+      createDashboardTriggerSrv("~/unlock_protective_stop", "unlock protective stop\n", "Protective stop releasing");
+
+  // Query whether there is currently a program running
+  running_service_ = node_->create_service<ur_dashboard_msgs::srv::IsProgramRunning>(
+      "~/program_running",
+      std::bind(&DashboardClientROS::handleRunningQuery, this, std::placeholders::_1, std::placeholders::_2));
+
+  // Load a robot installation from a file
+  get_loaded_program_service_ = node_->create_service<ur_dashboard_msgs::srv::GetLoadedProgram>(
+      "~/get_loaded_program", [&](const ur_dashboard_msgs::srv::GetLoadedProgram::Request::SharedPtr req,
+                                  ur_dashboard_msgs::srv::GetLoadedProgram::Response::SharedPtr resp) {
+        try {
+          resp->answer = this->client_.sendAndReceive("get loaded program\n");
+          std::smatch match;
+          std::regex expected("Loaded program: (.+)");
+          resp->success = std::regex_match(resp->answer, match, expected);
+          if (resp->success) {
+            resp->program_name = match[1];
+          }
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->answer = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+
+  // Load a robot installation from a file
+  load_installation_service_ = node_->create_service<ur_dashboard_msgs::srv::Load>(
+      "~/load_installation", [&](const ur_dashboard_msgs::srv::Load::Request::SharedPtr req,
+                                 ur_dashboard_msgs::srv::Load::Response::SharedPtr resp) {
+        try {
+          resp->answer = this->client_.sendAndReceive("load installation " + req->filename + "\n");
+          resp->success = std::regex_match(resp->answer, std::regex("Loading installation: .+"));
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->answer = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+
+  // Load a robot program from a file
+  load_program_service_ = node->create_service<ur_dashboard_msgs::srv::Load>(
+      "~/load_program", [&](const ur_dashboard_msgs::srv::Load::Request::SharedPtr req,
+                            ur_dashboard_msgs::srv::Load::Response::SharedPtr resp) {
+        try {
+          resp->answer = this->client_.sendAndReceive("load " + req->filename + "\n");
+          resp->success = std::regex_match(resp->answer, std::regex("Loading program: .+"));
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->answer = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+
+  // // Query whether the current program is saved
+  is_program_saved_service_ = node_->create_service<ur_dashboard_msgs::srv::IsProgramSaved>(
+      "~/program_saved",
+      std::bind(&DashboardClientROS::handleSavedQuery, this, std::placeholders::_1, std::placeholders::_2));
+
+  // Service to show a popup on the UR Teach pendant.
+  popup_service_ = node_->create_service<ur_dashboard_msgs::srv::Popup>(
+      "~/popup", [&](ur_dashboard_msgs::srv::Popup::Request::SharedPtr req,
+                     ur_dashboard_msgs::srv::Popup::Response::SharedPtr resp) {
+        try {
+          resp->answer = this->client_.sendAndReceive("popup " + req->message + "\n");
+          resp->success = std::regex_match(resp->answer, std::regex("showing popup"));
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->answer = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+
+  // Service to query the current program state
+  program_state_service_ = node_->create_service<ur_dashboard_msgs::srv::GetProgramState>(
+      "~/program_state", [&](const ur_dashboard_msgs::srv::GetProgramState::Request::SharedPtr /*unused*/,
+                             ur_dashboard_msgs::srv::GetProgramState::Response::SharedPtr resp) {
+        try {
+          resp->answer = this->client_.sendAndReceive("programState\n");
+          std::smatch match;
+          std::regex expected("(STOPPED|PLAYING|PAUSED) (.+)");
+          resp->success = std::regex_match(resp->answer, match, expected);
+          if (resp->success) {
+            resp->state.state = match[1];
+            resp->program_name = match[2];
+          }
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->answer = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+
+  // Service to query the current safety mode
+  safety_mode_service_ = node_->create_service<ur_dashboard_msgs::srv::GetSafetyMode>(
+      "~/get_safety_mode",
+      std::bind(&DashboardClientROS::handleSafetyModeQuery, this, std::placeholders::_1, std::placeholders::_2));
+
+  // Service to query the current robot mode
+  robot_mode_service_ = node_->create_service<ur_dashboard_msgs::srv::GetRobotMode>(
+      "~/get_robot_mode",
+      std::bind(&DashboardClientROS::handleRobotModeQuery, this, std::placeholders::_1, std::placeholders::_2));
+
+  // Service to add a message to the robot's log
+  add_to_log_service_ = node->create_service<ur_dashboard_msgs::srv::AddToLog>(
+      "~/add_to_log", [&](const ur_dashboard_msgs::srv::AddToLog::Request::SharedPtr req,
+                          ur_dashboard_msgs::srv::AddToLog::Response::SharedPtr resp) {
+        try {
+          resp->answer = this->client_.sendAndReceive("addToLog " + req->message + "\n");
+          resp->success = std::regex_match(resp->answer, std::regex("(Added log message|No log message to add)"));
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->answer = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+
+  // General purpose service to send arbitrary messages to the dashboard server
+  raw_request_service_ = node_->create_service<ur_dashboard_msgs::srv::RawRequest>(
+      "~/raw_request", [&](const ur_dashboard_msgs::srv::RawRequest::Request::SharedPtr req,
+                           ur_dashboard_msgs::srv::RawRequest::Response::SharedPtr resp) {
+        try {
+          resp->answer = this->client_.sendAndReceive(req->query + "\n");
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->answer = e.what();
+        }
+        return true;
+      });
+
+  // Service to reconnect to the dashboard server
+  reconnect_service_ = node_->create_service<std_srvs::srv::Trigger>(
+      "~/connect",
+      [&](const std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp) {
+        try {
+          resp->success = connect();
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->message = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+
+  // Disconnect from the dashboard service.
+  quit_service_ =
+      node_->create_service<std_srvs::srv::Trigger>("~/quit", [&](const std_srvs::srv::Trigger::Request::SharedPtr req,
+                                                                  std_srvs::srv::Trigger::Response::SharedPtr resp) {
+        try {
+          resp->message = this->client_.sendAndReceive("quit\n");
+          resp->success = std::regex_match(resp->message, std::regex("Disconnected"));
+          client_.disconnect();
+        } catch (const urcl::UrException& e) {
+          RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+          resp->message = e.what();
+          resp->success = false;
+        }
+        return true;
+      });
+}
+
+bool DashboardClientROS::connect()
+{
+  timeval tv;
+  // Timeout after which a call to the dashboard server will be considered failure if no answer has been received.
+  double time_buffer = 0;
+  node_->get_parameter("receive_timeout", time_buffer);
+  tv.tv_sec = time_buffer;
+  tv.tv_usec = 0;
+  client_.setReceiveTimeout(tv);
+  return client_.connect();
+}
+
+bool DashboardClientROS::handleRunningQuery(const ur_dashboard_msgs::srv::IsProgramRunning::Request::SharedPtr req,
+                                            ur_dashboard_msgs::srv::IsProgramRunning::Response::SharedPtr resp)
+{
+  try {
+    resp->answer = this->client_.sendAndReceive("running\n");
+    std::regex expected("Program running: (true|false)");
+    std::smatch match;
+    resp->success = std::regex_match(resp->answer, match, expected);
+
+    if (resp->success) {
+      resp->program_running = (match[1] == "true");
+    }
+  } catch (const urcl::UrException& e) {
+    RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+    resp->answer = e.what();
+    resp->success = false;
+  }
+
+  return true;
+}
+
+bool DashboardClientROS::handleSavedQuery(ur_dashboard_msgs::srv::IsProgramSaved::Request::SharedPtr req,
+                                          ur_dashboard_msgs::srv::IsProgramSaved::Response::SharedPtr resp)
+{
+  try {
+    resp->answer = this->client_.sendAndReceive("isProgramSaved\n");
+    std::regex expected("(true|false) ([^\\s]+)");
+    std::smatch match;
+    resp->success = std::regex_match(resp->answer, match, expected);
+
+    if (resp->success) {
+      resp->program_saved = (match[1] == "true");
+      resp->program_name = match[2];
+    }
+  } catch (const urcl::UrException& e) {
+    RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+    resp->answer = e.what();
+    resp->success = false;
+  }
+
+  return true;
+}
+
+bool DashboardClientROS::handleSafetyModeQuery(const ur_dashboard_msgs::srv::GetSafetyMode::Request::SharedPtr req,
+                                               ur_dashboard_msgs::srv::GetSafetyMode::Response::SharedPtr resp)
+{
+  try {
+    resp->answer = this->client_.sendAndReceive("safetymode\n");
+    std::smatch match;
+    std::regex expected("Safetymode: (.+)");
+    resp->success = std::regex_match(resp->answer, match, expected);
+    if (resp->success) {
+      if (match[1] == "NORMAL") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::NORMAL;
+      } else if (match[1] == "REDUCED") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::REDUCED;
+      } else if (match[1] == "PROTECTIVE_STOP") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::PROTECTIVE_STOP;
+      } else if (match[1] == "RECOVERY") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::RECOVERY;
+      } else if (match[1] == "SAFEGUARD_STOP") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SAFEGUARD_STOP;
+      } else if (match[1] == "SYSTEM_EMERGENCY_STOP") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SYSTEM_EMERGENCY_STOP;
+      } else if (match[1] == "ROBOT_EMERGENCY_STOP") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::ROBOT_EMERGENCY_STOP;
+      } else if (match[1] == "VIOLATION") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::VIOLATION;
+      } else if (match[1] == "FAULT") {
+        resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::FAULT;
+      }
+      // The following are only available in SafetyStatus from 5.5 on
+      // else if (match[1] == "AUTOMATIC_MODE_SAFEGUARD_STOP")
+      //{
+      // resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::AUTOMATIC_MODE_SAFEGUARD_STOP;
+      //}
+      // else if (match[1] == "SYSTEM_THREE_POSITION_ENABLING_STOP")
+      //{
+      // resp->safety_mode.mode = ur_dashboard_msgs::msg::SafetyMode::SYSTEM_THREE_POSITION_ENABLING_STOP;
+      //}
+    }
+  } catch (const urcl::UrException& e) {
+    RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+    resp->answer = e.what();
+    resp->success = false;
+  }
+  return true;
+}
+
+bool DashboardClientROS::handleRobotModeQuery(const ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req,
+                                              ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp)
+{
+  try {
+    resp->answer = this->client_.sendAndReceive("robotmode\n");
+    std::smatch match;
+    std::regex expected("Robotmode: (.+)");
+    resp->success = std::regex_match(resp->answer, match, expected);
+    if (resp->success) {
+      if (match[1] == "NO_CONTROLLER") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::NO_CONTROLLER;
+      } else if (match[1] == "DISCONNECTED") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::DISCONNECTED;
+      } else if (match[1] == "CONFIRM_SAFETY") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::CONFIRM_SAFETY;
+      } else if (match[1] == "BOOTING") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::BOOTING;
+      } else if (match[1] == "POWER_OFF") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::POWER_OFF;
+      } else if (match[1] == "POWER_ON") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::POWER_ON;
+      } else if (match[1] == "IDLE") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::IDLE;
+      } else if (match[1] == "BACKDRIVE") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::BACKDRIVE;
+      } else if (match[1] == "RUNNING") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::RUNNING;
+      } else if (match[1] == "UPDATING_FIRMWARE") {
+        resp->robot_mode.mode = ur_dashboard_msgs::msg::RobotMode::UPDATING_FIRMWARE;
+      }
+    }
+  } catch (const urcl::UrException& e) {
+    RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+    resp->answer = e.what();
+    resp->success = false;
+  }
+  return true;
+}
+}  // namespace ur_robot_driver

src/robot_state_helper.cpp

@@ -0,0 +1,397 @@
+// Copyright 2024, 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.
+
+#include <mutex>
+#include <ur_robot_driver/robot_state_helper.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/create_server.hpp"
+#include "std_srvs/srv/trigger.hpp"
+
+#include "ur_dashboard_msgs/action/set_mode.hpp"
+#include "ur_dashboard_msgs/msg/safety_mode.hpp"
+#include "ur_dashboard_msgs/msg/robot_mode.hpp"
+#include "ur_client_library/ur/datatypes.h"
+
+namespace ur_robot_driver
+{
+RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node)
+  : node_(node)
+  , robot_mode_(urcl::RobotMode::UNKNOWN)
+  , safety_mode_(urcl::SafetyMode::UNDEFINED_SAFETY_MODE)
+  , in_action_(false)
+{
+  robot_mode_sub_cb_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::SubscriptionOptions options;
+  options.callback_group = robot_mode_sub_cb_;
+  // Topic on which the robot_mode is published by the driver
+  robot_mode_sub_ = node_->create_subscription<ur_dashboard_msgs::msg::RobotMode>(
+      "io_and_status_controller/robot_mode", rclcpp::SensorDataQoS(),
+      std::bind(&RobotStateHelper::robotModeCallback, this, std::placeholders::_1), options);
+  // Topic on which the safety is published by the driver
+  safety_mode_sub_ = node_->create_subscription<ur_dashboard_msgs::msg::SafetyMode>(
+      "io_and_status_controller/safety_mode", 1,
+      std::bind(&RobotStateHelper::safetyModeCallback, this, std::placeholders::_1));
+  program_running_sub = node_->create_subscription<std_msgs::msg::Bool>(
+      "io_and_status_controller/robot_program_running", 1,
+      [this](std_msgs::msg::Bool::UniquePtr msg) -> void { program_running_ = msg->data; });
+
+  service_cb_grp_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+  node->declare_parameter("headless_mode", false);
+  headless_mode_ = node->get_parameter("headless_mode").as_bool();
+
+  // Service to unlock protective stop
+  unlock_protective_stop_srv_ = node_->create_client<std_srvs::srv::Trigger>(
+      "dashboard_client/unlock_protective_stop", rmw_qos_profile_services_default, service_cb_grp_);
+  // Service to restart safety
+  restart_safety_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/restart_safety",
+                                                                     rmw_qos_profile_services_default, service_cb_grp_);
+  // Service to power on the robot
+  power_on_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_on",
+                                                               rmw_qos_profile_services_default, service_cb_grp_);
+  // Service to power off the robot
+  power_off_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_off",
+                                                                rmw_qos_profile_services_default, service_cb_grp_);
+  // Service to release the robot's brakes
+  brake_release_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/brake_release",
+                                                                    rmw_qos_profile_services_default, service_cb_grp_);
+  // Service to stop UR program execution on the robot
+  stop_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/stop",
+                                                                   rmw_qos_profile_services_default, service_cb_grp_);
+  // Service to start UR program execution on the robot
+  play_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/play",
+                                                                   rmw_qos_profile_services_default, service_cb_grp_);
+  play_program_srv_->wait_for_service();
+
+  resend_robot_program_srv_ = node_->create_client<std_srvs::srv::Trigger>(
+      "io_and_status_controller/resend_robot_program", rmw_qos_profile_services_default, service_cb_grp_);
+  resend_robot_program_srv_->wait_for_service();
+
+  feedback_ = std::make_shared<ur_dashboard_msgs::action::SetMode::Feedback>();
+  result_ = std::make_shared<ur_dashboard_msgs::action::SetMode::Result>();
+  set_mode_as_ = rclcpp_action::create_server<ur_dashboard_msgs::action::SetMode>(
+      node_, "~/set_mode",
+      std::bind(&RobotStateHelper::setModeGoalCallback, this, std::placeholders::_1, std::placeholders::_2),
+      std::bind(&RobotStateHelper::setModeCancelCallback, this, std::placeholders::_1),
+      std::bind(&RobotStateHelper::setModeAcceptCallback, this, std::placeholders::_1));
+}
+
+void RobotStateHelper::robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg)
+{
+  if (robot_mode_ != static_cast<urcl::RobotMode>(msg->mode)) {
+    robot_mode_ = urcl::RobotMode(msg->mode);
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
+                       "The robot is currently in mode " << robotModeString(robot_mode_) << ".");
+    if (in_action_) {
+      std::scoped_lock lock(goal_mutex_);
+      feedback_->current_robot_mode =
+          static_cast<ur_dashboard_msgs::action::SetMode::Feedback::_current_robot_mode_type>(robot_mode_.load());
+      current_goal_handle_->publish_feedback(feedback_);
+    }
+  }
+}
+
+void RobotStateHelper::safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg)
+{
+  if (safety_mode_ != static_cast<urcl::SafetyMode>(msg->mode)) {
+    safety_mode_ = urcl::SafetyMode(msg->mode);
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
+                       "The robot is currently in safety mode " << safetyModeString(safety_mode_) << ".");
+    if (in_action_) {
+      std::scoped_lock lock(goal_mutex_);
+      feedback_->current_safety_mode =
+          static_cast<ur_dashboard_msgs::action::SetMode::Feedback::_current_safety_mode_type>(safety_mode_.load());
+      current_goal_handle_->publish_feedback(feedback_);
+    }
+  }
+}
+
+bool RobotStateHelper::recoverFromSafety()
+{
+  switch (safety_mode_) {
+    case urcl::SafetyMode::PROTECTIVE_STOP:
+      return safeDashboardTrigger(this->unlock_protective_stop_srv_);
+    case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:;
+    case urcl::SafetyMode::ROBOT_EMERGENCY_STOP:
+      RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in safety mode."
+                                                                       << safetyModeString(safety_mode_)
+                                                                       << ". Please release the EM-Stop to proceed.");
+      return false;
+    case urcl::SafetyMode::VIOLATION:;
+    case urcl::SafetyMode::FAULT:
+      return safeDashboardTrigger(this->restart_safety_srv_);
+    default:
+      // nothing to do
+      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("robot_state_helper"), "No safety recovery needed.");
+  }
+  return true;
+}
+
+bool RobotStateHelper::jumpToRobotMode(const urcl::RobotMode target_mode)
+{
+  switch (target_mode) {
+    case urcl::RobotMode::POWER_OFF:
+      return safeDashboardTrigger(this->power_off_srv_);
+    case urcl::RobotMode::IDLE:
+      return safeDashboardTrigger(this->power_on_srv_);
+    case urcl::RobotMode::RUNNING:
+      return safeDashboardTrigger(this->brake_release_srv_);
+    default:
+      RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode.");
+  }
+  return false;
+}
+
+bool RobotStateHelper::doTransition(const urcl::RobotMode target_mode)
+{
+  if (!recoverFromSafety()) {
+    return false;
+  }
+  switch (robot_mode_) {
+    case urcl::RobotMode::CONFIRM_SAFETY:
+      RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode "
+                                                                       << robotModeString(robot_mode_)
+                                                                       << ". It is required to interact with "
+                                                                          "the "
+                                                                          "teach pendant at this point.");
+      break;
+    case urcl::RobotMode::BOOTING:
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode "
+                                                                       << robotModeString(robot_mode_)
+                                                                       << ". Please wait until the robot is "
+                                                                          "booted up...");
+      break;
+    case urcl::RobotMode::POWER_OFF:
+      return jumpToRobotMode(target_mode);
+    case urcl::RobotMode::POWER_ON:
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode "
+                                                                       << robotModeString(robot_mode_)
+                                                                       << ". Please wait until the robot is in "
+                                                                          "mode "
+                                                                       << robotModeString(urcl::RobotMode::IDLE));
+      break;
+    case urcl::RobotMode::IDLE:
+      return jumpToRobotMode(target_mode);
+      break;
+    case urcl::RobotMode::BACKDRIVE:
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode "
+                                                                       << robotModeString(robot_mode_)
+                                                                       << ". It will automatically return to "
+                                                                          "mode "
+                                                                       << robotModeString(urcl::RobotMode::IDLE)
+                                                                       << " once the teach button is "
+                                                                          "released.");
+      break;
+    case urcl::RobotMode::RUNNING:
+      if (target_mode == urcl::RobotMode::IDLE) {
+        // We cannot engage the brakes directly.
+        if (!jumpToRobotMode(urcl::RobotMode::POWER_OFF)) {
+          return false;
+        }
+      }
+      return jumpToRobotMode(target_mode);
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
+                         "The robot has reached operational mode " << robotModeString(robot_mode_));
+      break;
+    default:
+      RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode "
+                                                                       << robotModeString(robot_mode_)
+                                                                       << ". This won't be handled by this "
+                                                                          "helper. Please resolve this "
+                                                                          "manually.");
+  }
+  return false;
+}
+
+bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv)
+{
+  assert(srv != nullptr);
+  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
+  auto future = srv->async_send_request(request);
+  future.wait();
+  auto result = future.get();
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "Service response received: " << result->message);
+  return result->success;
+}
+
+void RobotStateHelper::setModeAcceptCallback(const std::shared_ptr<RobotStateHelper::SetModeGoalHandle> goal_handle)
+{
+  std::thread{ std::bind(&RobotStateHelper::setModeExecute, this, std::placeholders::_1), goal_handle }.detach();
+}
+
+bool RobotStateHelper::stopProgram()
+{
+  if (safeDashboardTrigger(this->stop_program_srv_)) {
+    auto start = std::chrono::steady_clock::now();
+    while (program_running_ && std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(2));
+      return true;
+    }
+  }
+  return false;
+}
+
+void RobotStateHelper::setModeExecute(const std::shared_ptr<RobotStateHelper::SetModeGoalHandle> goal_handle)
+{
+  {
+    std::scoped_lock lock(goal_mutex_);
+    current_goal_handle_ = goal_handle;
+  }
+  in_action_ = true;
+  const auto goal = goal_handle->get_goal();
+  this->goal_ = goal;
+  urcl::RobotMode target_mode;
+  try {
+    target_mode = static_cast<urcl::RobotMode>(goal->target_robot_mode);
+    switch (target_mode) {
+      case urcl::RobotMode::POWER_OFF:
+      case urcl::RobotMode::IDLE:
+      case urcl::RobotMode::RUNNING:
+        if (goal_->stop_program && program_running_) {
+          if (!stopProgram()) {
+            result_->message = "Stopping the program failed.";
+            result_->success = false;
+            std::scoped_lock lock(goal_mutex_);
+            current_goal_handle_->abort(result_);
+            return;
+          }
+        }
+        if (robot_mode_ != target_mode || safety_mode_ > urcl::SafetyMode::REDUCED) {
+          RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
+                             "Target mode was set to " << robotModeString(target_mode) << ".");
+          if (!doTransition(target_mode)) {
+            result_->message = "Transition to target mode failed.";
+            result_->success = false;
+            std::scoped_lock lock(goal_mutex_);
+            current_goal_handle_->abort(result_);
+            return;
+          }
+        }
+        break;
+      case urcl::RobotMode::NO_CONTROLLER:
+      case urcl::RobotMode::DISCONNECTED:
+      case urcl::RobotMode::CONFIRM_SAFETY:
+      case urcl::RobotMode::BOOTING:
+      case urcl::RobotMode::POWER_ON:
+      case urcl::RobotMode::BACKDRIVE:
+      case urcl::RobotMode::UPDATING_FIRMWARE:
+        result_->message =
+            "Requested target mode " + robotModeString(target_mode) + " which cannot be explicitly selected.";
+        result_->success = false;
+        {
+          std::scoped_lock lock(goal_mutex_);
+          current_goal_handle_->abort(result_);
+        }
+        return;
+        break;
+      default:
+        result_->message = "Requested illegal mode.";
+        result_->success = false;
+        {
+          std::scoped_lock lock(goal_mutex_);
+          current_goal_handle_->abort(result_);
+        }
+        return;
+        break;
+    }
+  } catch (const std::invalid_argument& e) {
+    result_->message = e.what();
+    result_->success = false;
+    {
+      std::scoped_lock lock(goal_mutex_);
+      current_goal_handle_->abort(result_);
+    }
+    return;
+  }
+
+  // Wait until the robot reached the target mode or something went wrong
+  while (robot_mode_ != target_mode && !error_) {
+    RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "Waiting for robot to reach target mode... Current_mode: %s",
+                robotModeString(robot_mode_).c_str());
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  }
+
+  if (robot_mode_ == target_mode) {
+    result_->success = true;
+    result_->message = "Reached target robot mode.";
+    if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program && !program_running_) {
+      if (headless_mode_) {
+        result_->success = safeDashboardTrigger(this->resend_robot_program_srv_);
+      } else {
+        // The dashboard denies playing immediately after switching the mode to RUNNING
+        sleep(1);
+        result_->success = safeDashboardTrigger(this->play_program_srv_);
+      }
+    }
+    if (result_->success) {
+      std::scoped_lock lock(goal_mutex_);
+      current_goal_handle_->succeed(result_);
+    } else {
+      std::scoped_lock lock(goal_mutex_);
+      current_goal_handle_->abort(result_);
+    }
+  } else {
+    result_->success = false;
+    result_->message = "Robot reached higher mode than requested during recovery. This either means that something "
+                       "went wrong or that a higher mode was requested from somewhere else (e.g. the teach "
+                       "pendant.)";
+    {
+      std::scoped_lock lock(goal_mutex_);
+      current_goal_handle_->abort(result_);
+    }
+  }
+}
+
+rclcpp_action::GoalResponse RobotStateHelper::setModeGoalCallback(
+    const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal)
+{
+  (void)uuid;
+  if (robot_mode_ == urcl::RobotMode::UNKNOWN) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Robot mode is unknown. Cannot accept goal, yet. Is "
+                                                                  "the robot switched on and connected to the driver?");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+
+  if (safety_mode_ == urcl::SafetyMode::UNDEFINED_SAFETY_MODE) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Safety mode is unknown. Cannot accept goal, yet. Is "
+                                                                  "the robot switched on and connected to the driver?");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse
+RobotStateHelper::setModeCancelCallback(const std::shared_ptr<RobotStateHelper::SetModeGoalHandle> goal_handle)
+{
+  RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "Received request to cancel goal");
+  (void)goal_handle;
+  return rclcpp_action::CancelResponse::REJECT;
+}
+
+}  // namespace ur_robot_driver

src/robot_state_helper_node.cpp

@@ -0,0 +1,42 @@
+// Copyright 2024, 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.
+
+#include "ur_robot_driver/robot_state_helper.hpp"
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper");
+  ur_robot_driver::RobotStateHelper state_helper(node);
+
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+
+  return 0;
+}

src/ur_ros2_control_node.cpp

@@ -0,0 +1,100 @@
+// Copyright 2022 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 Universal Robots A/S 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  Lovro Ivanov lovro.ivanov@gmail.com
+ * \date    2022-1-7
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <thread>
+#include <memory>
+
+// ROS includes
+#include "controller_manager/controller_manager.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_helpers.hpp"
+
+// code is inspired by
+// https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/ros2_control_node.cpp
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  // create executor
+  std::shared_ptr<rclcpp::Executor> e = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+  // create controller manager instance
+  auto controller_manager = std::make_shared<controller_manager::ControllerManager>(e, "controller_manager");
+
+  // control loop thread
+  std::thread control_loop([controller_manager]() {
+    if (!realtime_tools::configure_sched_fifo(50)) {
+      RCLCPP_WARN(controller_manager->get_logger(), "Could not enable FIFO RT scheduling policy");
+    }
+
+    // for calculating sleep time
+    auto const period = std::chrono::nanoseconds(1'000'000'000 / controller_manager->get_update_rate());
+    auto const cm_now = std::chrono::nanoseconds(controller_manager->now().nanoseconds());
+    std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> next_iteration_time{ cm_now };
+
+    // for calculating the measured period of the loop
+    rclcpp::Time previous_time = controller_manager->now();
+
+    while (rclcpp::ok()) {
+      // calculate measured period
+      auto const current_time = controller_manager->now();
+      auto const measured_period = current_time - previous_time;
+      previous_time = current_time;
+
+      // execute update loop
+      controller_manager->read(controller_manager->now(), measured_period);
+      controller_manager->update(controller_manager->now(), measured_period);
+      controller_manager->write(controller_manager->now(), measured_period);
+
+      // wait until we hit the end of the period
+      next_iteration_time += period;
+      std::this_thread::sleep_until(next_iteration_time);
+    }
+  });
+
+  // spin the executor with controller manager node
+  e->add_node(controller_manager);
+  e->spin();
+
+  // wait for control loop to finish
+  control_loop.join();
+
+  // shutdown
+  rclcpp::shutdown();
+
+  return 0;
+}

src/urcl_log_handler.cpp

@@ -0,0 +1,101 @@
+// Copyright 2021 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.
+
+// All source code contained in and/or linked to in this message (the “Source Code”) is subject to the copyright of
+// Universal Robots A/S and/or its licensors. THE SOURCE CODE IS PROVIDED “AS IS” WITHOUT WARRANTY OF ANY KIND, EXPRESS
+// OR IMPLIED, INCLUDING – BUT NOT LIMITED TO – WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR
+// NONINFRINGEMENT. USE OF THE SOURCE CODE IS AT YOUR OWN RISK AND UNIVERSAL ROBOTS A/S AND ITS LICENSORS SHALL, TO THE
+// MAXIMUM EXTENT PERMITTED BY LAW, NOT BE LIABLE FOR ANY ERRORS OR MALICIOUS CODE IN THE SOURCE CODE, ANY THIRD-PARTY
+// CLAIMS, OR ANY OTHER CLAIMS AND DAMAGES, INCLUDING INDIRECT, INCIDENTAL, SPECIAL, CONSEQUENTIAL OR PUNITIVE DAMAGES,
+// OR ANY LOSS OF PROFITS, EXPECTED SAVINGS, OR REVENUES, WHETHER INCURRED DIRECTLY OR INDIRECTLY, OR ANY LOSS OF DATA,
+// USE, GOODWILL, OR OTHER INTANGIBLE LOSSES, RESULTING FROM YOUR USE OF THE SOURCE CODE. You may make copies of the
+// Source Code for use in connection with a Universal Robots or UR+ product, provided that you include (i) an
+// appropriate copyright notice (“©  [the year in which you received the Source Code or the Source Code was first
+// published, e.g. “2021”] Universal Robots A/S and/or its licensors”) along with the capitalized section of this notice
+// in all copies of the Source Code. By using the Source Code, you agree to the above terms. For more information,
+// please contact legal@universal-robots.com.
+
+#include <memory>
+#include <utility>
+
+#include "ur_robot_driver/urcl_log_handler.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace ur_robot_driver
+{
+bool g_registered = false;
+
+UrclLogHandler::UrclLogHandler() = default;
+
+void UrclLogHandler::log(const char* file, int line, urcl::LogLevel loglevel, const char* message)
+{
+  rcutils_log_location_t location = { "", file, static_cast<size_t>(line) };
+
+  const auto logger_name = "UR_Client_Library:" + tf_prefix_;
+  switch (loglevel) {
+    case urcl::LogLevel::DEBUG:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_DEBUG, logger_name.c_str(), "%s", message);
+      break;
+    case urcl::LogLevel::INFO:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_INFO, logger_name.c_str(), "%s", message);
+      break;
+    case urcl::LogLevel::WARN:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_WARN, logger_name.c_str(), "%s", message);
+      break;
+    case urcl::LogLevel::ERROR:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_ERROR, logger_name.c_str(), "%s", message);
+      break;
+    case urcl::LogLevel::FATAL:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_FATAL, logger_name.c_str(), "%s", message);
+      break;
+    default:
+      break;
+  }
+}
+
+void registerUrclLogHandler(const std::string& tf_prefix)
+{
+  if (g_registered == false) {
+    std::unique_ptr<UrclLogHandler> log_handler(new UrclLogHandler);
+    log_handler->setTFPrefix(tf_prefix);
+    // Log level is decided by ROS2 log level
+    urcl::setLogLevel(urcl::LogLevel::DEBUG);
+    urcl::registerLogHandler(std::move(log_handler));
+    g_registered = true;
+  }
+}
+
+void unregisterUrclLogHandler()
+{
+  if (g_registered == true) {
+    urcl::unregisterLogHandler();
+    g_registered = false;
+  }
+}
+
+}  // namespace 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 exner@fzi.de
+ * \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;
+}

test/dashboard_client.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python
+# Copyright 2019, FZI Forschungszentrum Informatik
+#
+# 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.
+import os
+import sys
+import time
+import unittest
+
+import pytest
+import rclpy
+from rclpy.node import Node
+from ur_dashboard_msgs.msg import RobotMode
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import DashboardInterface, generate_dashboard_test_description  # noqa: E402
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    return generate_dashboard_test_description()
+
+
+class DashboardClientTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = Node("dashboard_client_test")
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        self._dashboard_interface = DashboardInterface(self.node)
+
+    def test_switch_on(self):
+        """Test power on a robot."""
+        # Wait until the robot is booted completely
+        end_time = time.time() + 10
+        mode = RobotMode.DISCONNECTED
+        while mode != RobotMode.POWER_OFF and time.time() < end_time:
+            time.sleep(0.1)
+            result = self._dashboard_interface.get_robot_mode()
+            self.assertTrue(result.success)
+            mode = result.robot_mode.mode
+
+        # Power on robot
+        self.assertTrue(self._dashboard_interface.power_on().success)
+
+        # Wait until robot mode changes
+        end_time = time.time() + 10
+        mode = RobotMode.DISCONNECTED
+        while mode not in (RobotMode.IDLE, RobotMode.RUNNING) and time.time() < end_time:
+            time.sleep(0.1)
+            result = self._dashboard_interface.get_robot_mode()
+            self.assertTrue(result.success)
+            mode = result.robot_mode.mode
+
+        self.assertIn(mode, (RobotMode.IDLE, RobotMode.RUNNING))
+
+        # Release robot brakes
+        self.assertTrue(self._dashboard_interface.brake_release().success)
+
+        # Wait until robot mode is RUNNING
+        end_time = time.time() + 10
+        mode = RobotMode.DISCONNECTED
+        while mode != RobotMode.RUNNING and time.time() < end_time:
+            time.sleep(0.1)
+            result = self._dashboard_interface.get_robot_mode()
+            self.assertTrue(result.success)
+            mode = result.robot_mode.mode
+
+        self.assertEqual(mode, RobotMode.RUNNING)

test/integration_test_controller_switch.py

@@ -0,0 +1,407 @@
+#!/usr/bin/env python
+# Copyright 2019, 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.
+
+import os
+import sys
+import time
+import unittest
+
+import pytest
+
+import launch_testing
+import rclpy
+from rclpy.node import Node
+
+from controller_manager_msgs.srv import SwitchController
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import (  # noqa: E402
+    ControllerManagerInterface,
+    DashboardInterface,
+    IoStatusInterface,
+    generate_driver_test_description,
+)
+
+
+@pytest.mark.launch_test
+@launch_testing.parametrize(
+    "tf_prefix",
+    [""],
+    # [(""), ("my_ur_")],
+)
+def generate_test_description(tf_prefix):
+    return generate_driver_test_description(tf_prefix=tf_prefix)
+
+
+class RobotDriverTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = Node("robot_driver_test")
+        time.sleep(1)
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        self._dashboard_interface = DashboardInterface(self.node)
+        self._controller_manager_interface = ControllerManagerInterface(self.node)
+        self._io_status_controller_interface = IoStatusInterface(self.node)
+
+    def setUp(self):
+        self._dashboard_interface.start_robot()
+        time.sleep(1)
+        self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
+
+    def test_activating_multiple_controllers_same_interface_fails(self):
+        # Deactivate all writing controllers
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                    "passthrough_trajectory_controller",
+                    "force_mode_controller",
+                    "freedrive_mode_controller",
+                ],
+            ).ok
+        )
+
+        # Activating different motion controllers should not be possible
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "forward_position_controller",
+                ],
+            ).ok
+        )
+
+    def test_activating_multiple_controllers_different_interface_fails(self):
+        # Deactivate all writing controllers
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                    "force_mode_controller",
+                    "passthrough_trajectory_controller",
+                    "freedrive_mode_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "forward_velocity_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "passthrough_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "forward_velocity_controller",
+                    "passthrough_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "force_mode_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "freedrive_mode_controller",
+                ],
+            ).ok
+        )
+
+    def test_activating_controller_with_running_position_controller_fails(self):
+        # Having a position-based controller active, no other controller should be able to
+        # activate.
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                ],
+                deactivate_controllers=[
+                    "joint_trajectory_controller",
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                    "force_mode_controller",
+                    "freedrive_mode_controller",
+                    "passthrough_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "forward_position_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "forward_velocity_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "freedrive_mode_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "passthrough_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+            ).ok
+        )
+        # Stop controller again
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                ],
+            ).ok
+        )
+
+    def test_activating_controller_with_running_passthrough_trajectory_controller_fails(self):
+        # Having a position-based controller active, no other controller should be able to
+        # activate.
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["passthrough_trajectory_controller"],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                    "force_mode_controller",  # tested in separate test
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "forward_position_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "forward_velocity_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "freedrive_mode_controller",
+                ],
+            ).ok
+        )
+        # Stop the controller again
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                deactivate_controllers=[
+                    "passthrough_trajectory_controller",
+                ],
+            ).ok
+        )
+
+    def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self):
+        # Deactivate all writing controllers
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                    "passthrough_trajectory_controller",
+                    "force_mode_controller",
+                ],
+            ).ok
+        )
+
+        time.sleep(3)
+
+        # Start both together
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "passthrough_trajectory_controller",
+                    "force_mode_controller",
+                ],
+            ).ok
+        )
+
+        # With passthrough traj controller running, start force mode controller
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "passthrough_trajectory_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                    "force_mode_controller",
+                ],
+            ).ok
+        )
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+            ).ok
+        )
+
+        # With start force mode controller running, passthrough traj controller
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                    "forward_position_controller",
+                    "forward_velocity_controller",
+                    "passthrough_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "passthrough_trajectory_controller",
+                ],
+            ).ok
+        )

test/integration_test_force_mode.py

@@ -0,0 +1,514 @@
+#!/usr/bin/env python
+# Copyright 2019, 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.
+
+import os
+import sys
+import time
+import unittest
+
+import pytest
+
+import launch_testing
+import rclpy
+from rclpy.node import Node
+
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+import std_msgs
+from controller_manager_msgs.srv import SwitchController
+from geometry_msgs.msg import (
+    Pose,
+    PoseStamped,
+    Quaternion,
+    Point,
+    Twist,
+    Wrench,
+    Vector3,
+)
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import (  # noqa: E402
+    ControllerManagerInterface,
+    DashboardInterface,
+    ForceModeInterface,
+    IoStatusInterface,
+    ConfigurationInterface,
+    generate_driver_test_description,
+)
+
+TIMEOUT_EXECUTE_TRAJECTORY = 30
+
+
+def are_quaternions_same(q1, q2, tolerance):
+    dot_product = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w
+    return (abs(dot_product) - 1.0) < tolerance
+
+
+@pytest.mark.launch_test
+@launch_testing.parametrize(
+    "tf_prefix",
+    [(""), ("my_ur_")],
+)
+def generate_test_description(tf_prefix):
+    return generate_driver_test_description(tf_prefix=tf_prefix)
+
+
+class RobotDriverTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = Node("robot_driver_test")
+        time.sleep(1)
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        self._dashboard_interface = DashboardInterface(self.node)
+        self._controller_manager_interface = ControllerManagerInterface(self.node)
+        self._io_status_controller_interface = IoStatusInterface(self.node)
+        self._configuration_controller_interface = ConfigurationInterface(self.node)
+
+    def setUp(self):
+        self._dashboard_interface.start_robot()
+        time.sleep(1)
+        self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self.node)
+
+    def lookup_tcp_in_base(self, tf_prefix, timepoint):
+        trans = None
+        while not trans:
+            rclpy.spin_once(self.node)
+            try:
+                trans = self.tf_buffer.lookup_transform(
+                    tf_prefix + "base", tf_prefix + "tool0", timepoint
+                )
+            except TransformException:
+                pass
+        return trans
+
+    def test_force_mode_controller(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.8, y=0.8, z=0.8)
+        orientation = Quaternion(x=0.7071, y=0.0, z=0.0, w=0.7071)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        # Create compliance vector (which axes should be force controlled)
+        compliance = [False, False, True, False, False, False]
+
+        # Create Wrench message for force mode
+        wrench = Wrench()
+        wrench.force = Vector3(x=0.0, y=0.0, z=5.0)
+        wrench.torque = Vector3(x=0.0, y=0.0, z=0.0)
+
+        # Specify interpretation of task frame (no transform)
+        type_spec = 2
+
+        # Specify max speeds and deviations of force mode
+        speed_limits = Twist()
+        speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0)
+        speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0)
+        deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+        # specify damping and gain scaling
+        damping_factor = 0.1
+        gain_scale = 0.8
+
+        trans_before = self.lookup_tcp_in_base(tf_prefix, rclpy.time.Time())
+
+        # Send request to controller
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp,
+            selection_vector_x=compliance[0],
+            selection_vector_y=compliance[1],
+            selection_vector_z=compliance[2],
+            selection_vector_rx=compliance[3],
+            selection_vector_ry=compliance[4],
+            selection_vector_rz=compliance[5],
+            wrench=wrench,
+            type=type_spec,
+            speed_limits=speed_limits,
+            deviation_limits=deviation_limits,
+            damping_factor=damping_factor,
+            gain_scaling=gain_scale,
+        )
+        self.assertTrue(res.success)
+
+        time.sleep(5.0)
+
+        trans_after = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
+
+        # task frame and wrench determines the expected motion
+        # In the example we used
+        #   - a task frame rotated pi/2 deg around the base frame's x axis
+        #   - a wrench with a positive z component for the force
+        # => we should expect a motion in negative y of the base frame
+        self.assertTrue(trans_after.transform.translation.y < trans_before.transform.translation.y)
+        self.assertAlmostEqual(
+            trans_after.transform.translation.x,
+            trans_before.transform.translation.x,
+            delta=0.001,
+        )
+        self.assertAlmostEqual(
+            trans_after.transform.translation.z,
+            trans_before.transform.translation.z,
+            delta=0.001,
+        )
+        self.assertTrue(
+            are_quaternions_same(
+                trans_after.transform.rotation, trans_before.transform.rotation, 0.001
+            )
+        )
+
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        # Deactivate controller
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                deactivate_controllers=["force_mode_controller"],
+            ).ok
+        )
+
+    def test_illegal_force_mode_types(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=0)
+        self.assertFalse(res.success)
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=4)
+        self.assertFalse(res.success)
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=1)
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=2)
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=3)
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+
+    def test_illegal_task_frame(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        # Illegal frame name produces error
+        header.frame_id = "nonexisting6t54"
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp,
+        )
+        self.assertFalse(res.success)
+        header.frame_id = "base"
+
+        # Illegal quaternion produces error
+        task_frame_pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp,
+        )
+        self.assertFalse(res.success)
+
+    def test_start_force_mode_on_inactive_controller_fails(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[],
+                deactivate_controllers=[
+                    "force_mode_controller",
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp,
+        )
+        self.assertFalse(res.success)
+
+    def test_deactivating_controller_stops_force_mode(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        # Create compliance vector (which axes should be force controlled)
+        compliance = [False, False, True, False, False, False]
+
+        # Create Wrench message for force mode
+        wrench = Wrench()
+        wrench.force = Vector3(x=0.0, y=0.0, z=5.0)
+        wrench.torque = Vector3(x=0.0, y=0.0, z=0.0)
+
+        # Specify interpretation of task frame (no transform)
+        type_spec = 2
+
+        # Specify max speeds and deviations of force mode
+        speed_limits = Twist()
+        speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0)
+        speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0)
+        deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+        # specify damping and gain scaling
+        damping_factor = 0.1
+        gain_scale = 0.8
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp,
+            selection_vector_x=compliance[0],
+            selection_vector_y=compliance[1],
+            selection_vector_z=compliance[2],
+            selection_vector_rx=compliance[3],
+            selection_vector_ry=compliance[4],
+            selection_vector_rz=compliance[5],
+            wrench=wrench,
+            type=type_spec,
+            speed_limits=speed_limits,
+            deviation_limits=deviation_limits,
+            damping_factor=damping_factor,
+            gain_scaling=gain_scale,
+        )
+        self.assertTrue(res.success)
+
+        time.sleep(0.5)
+
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[],
+                deactivate_controllers=[
+                    "force_mode_controller",
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        time.sleep(0.5)
+        trans_before_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
+
+        # Make sure the robot didn't move anymore
+        time.sleep(0.5)
+        trans_after_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
+
+        self.assertAlmostEqual(
+            trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z
+        )
+
+    def test_params_out_of_range_fails(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=-0.1
+        )
+        self.assertFalse(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=0.0
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=2.0
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=2.1
+        )
+        self.assertFalse(res.success)
+
+        # damping factor
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=-0.1
+        )
+        self.assertFalse(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=0.0
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=1.0
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=1.1
+        )
+        self.assertFalse(res.success)

test/launch_args.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python
+# Copyright 2024, FZI Forschungszentrum Informatik
+#
+# 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.
+
+import logging
+import os
+import pytest
+import sys
+import time
+import unittest
+
+import rclpy
+from rclpy.node import Node
+
+
+from launch import LaunchDescription
+from launch.actions import (
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackagePrefix, FindPackageShare
+
+import launch_testing
+from launch_testing.actions import ReadyToTest
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import (  # noqa: E402
+    ControllerManagerInterface,
+    _declare_launch_arguments,
+    _ursim_action,
+)
+
+
+@pytest.mark.launch_test
+@launch_testing.parametrize(
+    "launch_dashboard_client",
+    [("true"), ("false")],
+)
+def generate_test_description(launch_dashboard_client):
+    ur_type = LaunchConfiguration("ur_type")
+    launch_arguments = {
+        "robot_ip": "192.168.56.101",
+        "ur_type": ur_type,
+        "launch_rviz": "false",
+        "controller_spawner_timeout": str(120),
+        "initial_joint_controller": "scaled_joint_trajectory_controller",
+        "headless_mode": "true",
+        "launch_dashboard_client": launch_dashboard_client,
+        "start_joint_controller": "false",
+    }
+
+    robot_driver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
+            )
+        ),
+        launch_arguments=launch_arguments.items(),
+    )
+    wait_dashboard_server = ExecuteProcess(
+        cmd=[
+            PathJoinSubstitution(
+                [
+                    FindPackagePrefix("ur_robot_driver"),
+                    "bin",
+                    "wait_dashboard_server.sh",
+                ]
+            )
+        ],
+        name="wait_dashboard_server",
+        output="screen",
+    )
+    driver_starter = RegisterEventHandler(
+        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
+    )
+
+    return LaunchDescription(
+        _declare_launch_arguments()
+        + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter]
+    )
+
+
+class DashboardClientTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = Node("robot_driver_launch_test")
+        time.sleep(1)
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        # This waits for the controller_manager to be available
+        self._controller_manager_interface = ControllerManagerInterface(self.node)
+
+    def setUp(self):
+        pass
+
+    def test_dashboard_client_exists(self, launch_dashboard_client):
+        # Use the get_node_names_and_namespaces() method to get the list of nodes and their namespaces
+        nodes_with_ns = self.node.get_node_names_and_namespaces()
+
+        nodes = [namespace + name for (name, namespace) in nodes_with_ns]
+
+        for node in nodes:
+            print(node)
+
+        # Print out the nodes and their namespaces
+        logging.info("Discovered ROS nodes:")
+        if launch_dashboard_client == "true":
+            self.assertIn("/dashboard_client", nodes)
+        else:
+            self.assertNotIn("/dashboard_client", nodes)

test/robot_driver.py

@@ -0,0 +1,439 @@
+#!/usr/bin/env python3
+# Copyright 2019, FZI Forschungszentrum Informatik
+#
+# 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.
+import logging
+import os
+import sys
+import time
+import unittest
+
+import launch_testing
+import pytest
+import rclpy
+from builtin_interfaces.msg import Duration
+from control_msgs.action import FollowJointTrajectory
+from control_msgs.msg import JointTolerance
+from controller_manager_msgs.srv import SwitchController
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from ur_msgs.msg import IOStates
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import (  # noqa: E402
+    ActionInterface,
+    ControllerManagerInterface,
+    DashboardInterface,
+    IoStatusInterface,
+    ConfigurationInterface,
+    generate_driver_test_description,
+    ROBOT_JOINTS,
+)
+
+TIMEOUT_EXECUTE_TRAJECTORY = 30
+
+
+@pytest.mark.launch_test
+@launch_testing.parametrize(
+    "tf_prefix",
+    [(""), ("my_ur_")],
+)
+def generate_test_description(tf_prefix):
+    return generate_driver_test_description(tf_prefix=tf_prefix)
+
+
+class RobotDriverTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = Node("robot_driver_test")
+        time.sleep(1)
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        self._dashboard_interface = DashboardInterface(self.node)
+        self._controller_manager_interface = ControllerManagerInterface(self.node)
+        self._io_status_controller_interface = IoStatusInterface(self.node)
+        self._configuration_controller_interface = ConfigurationInterface(self.node)
+
+        self._scaled_follow_joint_trajectory = ActionInterface(
+            self.node,
+            "/scaled_joint_trajectory_controller/follow_joint_trajectory",
+            FollowJointTrajectory,
+        )
+        self._passthrough_forward_joint_trajectory = ActionInterface(
+            self.node,
+            "/passthrough_trajectory_controller/follow_joint_trajectory",
+            FollowJointTrajectory,
+        )
+
+    def setUp(self):
+        self._dashboard_interface.start_robot()
+        time.sleep(1)
+        self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
+
+    #
+    # Test functions
+    #
+
+    def test_get_robot_software_version(self):
+        self.assertNotEqual(
+            self._configuration_controller_interface.get_robot_software_version().major, 0
+        )
+
+    def test_start_scaled_jtc_controller(self):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
+
+    def test_start_passthrough_controller(self):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["passthrough_trajectory_controller"],
+                deactivate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["passthrough_trajectory_controller"],
+                activate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
+
+    def test_set_io(self):
+        """Test to set an IO and check whether it has been set."""
+        # Create io callback to verify result
+        io_msg = None
+
+        def io_msg_cb(msg):
+            nonlocal io_msg
+            io_msg = msg
+
+        io_states_sub = self.node.create_subscription(
+            IOStates,
+            "/io_and_status_controller/io_states",
+            io_msg_cb,
+            rclpy.qos.qos_profile_system_default,
+        )
+
+        # Set pin 0 to 1.0
+        test_pin = 0
+
+        logging.info("Setting pin %d to 1.0", test_pin)
+        self._io_status_controller_interface.set_io(fun=1, pin=test_pin, state=1.0)
+
+        # Wait until the pin state has changed
+        pin_state = False
+        end_time = time.time() + 5
+        while not pin_state and time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            if io_msg is not None:
+                pin_state = io_msg.digital_out_states[test_pin].state
+
+        self.assertEqual(pin_state, 1.0)
+
+        # Set pin 0 to 0.0
+        logging.info("Setting pin %d to 0.0", test_pin)
+        self._io_status_controller_interface.set_io(fun=1, pin=test_pin, state=0.0)
+
+        # Wait until the pin state has changed back
+        end_time = time.time() + 5
+        while pin_state and time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            if io_msg is not None:
+                pin_state = io_msg.digital_out_states[test_pin].state
+
+        self.assertEqual(pin_state, 0.0)
+
+        # Clean up io subscription
+        self.node.destroy_subscription(io_states_sub)
+
+    def test_trajectory(self, tf_prefix):
+        """Test robot movement."""
+        # Construct test trajectory
+        test_trajectory = [
+            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
+            (Duration(sec=9, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
+            (Duration(sec=12, nanosec=0), [-1.0 for j in ROBOT_JOINTS]),
+        ]
+
+        trajectory = JointTrajectory(
+            joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+            points=[
+                JointTrajectoryPoint(positions=test_pos, time_from_start=test_time)
+                for (test_time, test_pos) in test_trajectory
+            ],
+        )
+
+        # Sending trajectory goal
+        logging.info("Sending simple goal")
+        goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory)
+        self.assertTrue(goal_handle.accepted)
+
+        # Verify execution
+        result = self._scaled_follow_joint_trajectory.get_result(
+            goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+        )
+        self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+
+    def test_illegal_trajectory(self, tf_prefix):
+        """
+        Test trajectory server.
+
+        This is more of a validation test that the testing suite does the right thing
+        """
+        # Construct test trajectory, the second point wrongly starts before the first
+        test_trajectory = [
+            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
+            (Duration(sec=3, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
+        ]
+
+        trajectory = JointTrajectory(
+            joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+            points=[
+                JointTrajectoryPoint(positions=test_pos, time_from_start=test_time)
+                for (test_time, test_pos) in test_trajectory
+            ],
+        )
+
+        # Send illegal goal
+        logging.info("Sending illegal goal")
+        goal_handle = self._scaled_follow_joint_trajectory.send_goal(
+            trajectory=trajectory,
+        )
+
+        # Verify the failure is correctly detected
+        self.assertFalse(goal_handle.accepted)
+
+    def test_trajectory_scaled(self, tf_prefix):
+        """Test robot movement."""
+        # Construct test trajectory
+        test_trajectory = [
+            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
+            (Duration(sec=6, nanosec=500000000), [-1.0 for j in ROBOT_JOINTS]),
+        ]
+
+        trajectory = JointTrajectory(
+            joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+            points=[
+                JointTrajectoryPoint(positions=test_pos, time_from_start=test_time)
+                for (test_time, test_pos) in test_trajectory
+            ],
+        )
+
+        # Execute trajectory
+        logging.info("Sending goal for robot to follow")
+        goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory)
+        self.assertTrue(goal_handle.accepted)
+
+        # Verify execution
+        result = self._scaled_follow_joint_trajectory.get_result(
+            goal_handle,
+            TIMEOUT_EXECUTE_TRAJECTORY,
+        )
+        self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+
+    def test_trajectory_scaled_aborts_on_violation(self, tf_prefix):
+        """Test that the robot correctly aborts the trajectory when the constraints are violated."""
+        # Construct test trajectory
+        test_trajectory = [
+            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
+            (
+                Duration(sec=6, nanosec=50000000),
+                [-1.0 for j in ROBOT_JOINTS],
+            ),  # physically unfeasible
+            (
+                Duration(sec=8, nanosec=0),
+                [-1.5 for j in ROBOT_JOINTS],
+            ),  # physically unfeasible
+        ]
+
+        trajectory = JointTrajectory(
+            joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+            points=[
+                JointTrajectoryPoint(positions=test_pos, time_from_start=test_time)
+                for (test_time, test_pos) in test_trajectory
+            ],
+        )
+
+        last_joint_state = None
+
+        def js_cb(msg):
+            nonlocal last_joint_state
+            last_joint_state = msg
+
+        joint_state_sub = self.node.create_subscription(JointState, "/joint_states", js_cb, 1)
+        joint_state_sub  # prevent warning about unused variable
+
+        # Send goal
+        logging.info("Sending goal for robot to follow")
+        goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory)
+        self.assertTrue(goal_handle.accepted)
+
+        # Get result
+        result = self._scaled_follow_joint_trajectory.get_result(
+            goal_handle,
+            TIMEOUT_EXECUTE_TRAJECTORY,
+        )
+        self.assertEqual(result.error_code, FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED)
+
+        state_when_aborted = last_joint_state
+
+        # This section is to make sure the robot stopped moving once the trajectory was aborted
+        time.sleep(2.0)
+        # Ugly workaround since we want to wait for a joint state in the same thread
+        while last_joint_state == state_when_aborted:
+            rclpy.spin_once(self.node)
+        state_after_sleep = last_joint_state
+
+        logging.info("Joint states before sleep:\t %s", state_when_aborted.position.tolist())
+        logging.info("Joint states after sleep:\t %s", state_after_sleep.position.tolist())
+
+        self.assertTrue(
+            all(
+                [
+                    abs(a - b) < 0.01
+                    for a, b in zip(state_after_sleep.position, state_when_aborted.position)
+                ]
+            )
+        )
+
+    # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message
+    # see https://github.com/ros-controls/ros2_controllers/issues/249
+    # Now do the same again, but with a goal time constraint
+    # self.node.get_logger().info("Sending scaled goal with time restrictions")
+    #
+    # goal.goal_time_tolerance = Duration(nanosec=10000000)
+    # goal_response = self.call_action("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal)
+    #
+    # self.assertEqual(goal_response.accepted, True)
+    #
+    # if goal_response.accepted:
+    #     result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY)
+    #     self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED)
+    #     self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED")
+
+    def test_passthrough_trajectory(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["passthrough_trajectory_controller"],
+                deactivate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
+        waypts = [
+            [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
+            [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
+            [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
+        ]
+        time_vec = [
+            Duration(sec=4, nanosec=0),
+            Duration(sec=8, nanosec=0),
+            Duration(sec=12, nanosec=0),
+        ]
+        goal_tolerance = [
+            JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i])
+            for i in range(len(ROBOT_JOINTS))
+        ]
+        goal_time_tolerance = Duration(sec=1, nanosec=0)
+        test_trajectory = zip(time_vec, waypts)
+        trajectory = JointTrajectory(
+            points=[
+                JointTrajectoryPoint(positions=pos, time_from_start=times)
+                for (times, pos) in test_trajectory
+            ],
+            joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))],
+        )
+        goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
+            trajectory=trajectory,
+            goal_time_tolerance=goal_time_tolerance,
+            goal_tolerance=goal_tolerance,
+        )
+        self.assertTrue(goal_handle.accepted)
+        if goal_handle.accepted:
+            result = self._passthrough_forward_joint_trajectory.get_result(
+                goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+            )
+            self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+        # Test impossible goal tolerance, should fail.
+        goal_tolerance = [
+            JointTolerance(position=0.000000001, name=tf_prefix + ROBOT_JOINTS[i])
+            for i in range(len(ROBOT_JOINTS))
+        ]
+        goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
+            trajectory=trajectory,
+            goal_time_tolerance=goal_time_tolerance,
+            goal_tolerance=goal_tolerance,
+        )
+        self.assertTrue(goal_handle.accepted)
+        if goal_handle.accepted:
+            result = self._passthrough_forward_joint_trajectory.get_result(
+                goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+            )
+            self.assertEqual(
+                result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
+            )
+
+        # Test impossible goal time
+        goal_tolerance = [
+            JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i]) for i in range(6)
+        ]
+        goal_time_tolerance.sec = 0
+        goal_time_tolerance.nanosec = 10
+        goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
+            trajectory=trajectory,
+            goal_time_tolerance=goal_time_tolerance,
+            goal_tolerance=goal_tolerance,
+        )
+        self.assertTrue(goal_handle.accepted)
+        if goal_handle.accepted:
+            result = self._passthrough_forward_joint_trajectory.get_result(
+                goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+            )
+            self.assertEqual(
+                result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
+            )
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["passthrough_trajectory_controller"],
+                activate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )

test/test_common.py

@@ -0,0 +1,385 @@
+# Copyright 2023, FZI Forschungszentrum Informatik
+#
+# 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.
+import logging
+import time
+
+import rclpy
+from controller_manager_msgs.srv import (
+    ListControllers,
+    SwitchController,
+    LoadController,
+    UnloadController,
+)
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackagePrefix, FindPackageShare
+from launch_testing.actions import ReadyToTest
+from rclpy.action import ActionClient
+from std_srvs.srv import Trigger
+from ur_dashboard_msgs.msg import RobotMode
+from ur_dashboard_msgs.srv import (
+    GetLoadedProgram,
+    GetProgramState,
+    GetRobotMode,
+    IsProgramRunning,
+    Load,
+)
+from ur_msgs.srv import SetIO, GetRobotSoftwareVersion, SetForceMode
+
+TIMEOUT_WAIT_SERVICE = 10
+TIMEOUT_WAIT_SERVICE_INITIAL = 120  # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable.
+TIMEOUT_WAIT_ACTION = 10
+
+ROBOT_JOINTS = [
+    "elbow_joint",
+    "shoulder_lift_joint",
+    "shoulder_pan_joint",
+    "wrist_1_joint",
+    "wrist_2_joint",
+    "wrist_3_joint",
+]
+
+
+def _wait_for_service(node, srv_name, srv_type, timeout):
+    client = node.create_client(srv_type, srv_name)
+
+    logging.info("Waiting for service '%s' with timeout %fs...", srv_name, timeout)
+    if client.wait_for_service(timeout) is False:
+        raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}")
+    logging.info("  Successfully connected to service '%s'", srv_name)
+
+    return client
+
+
+def _wait_for_action(node, action_name, action_type, timeout):
+    client = ActionClient(node, action_type, action_name)
+
+    logging.info("Waiting for action server '%s' with timeout %fs...", action_name, timeout)
+    if client.wait_for_server(timeout) is False:
+        raise Exception(
+            f"Could not reach action server '{action_name}' within timeout of {timeout}"
+        )
+
+    logging.info("  Successfully connected to action server '%s'", action_name)
+    return client
+
+
+def _call_service(node, client, request):
+    logging.info("Calling service client '%s' with request '%s'", client.srv_name, request)
+    future = client.call_async(request)
+
+    rclpy.spin_until_future_complete(node, future)
+
+    if future.result() is not None:
+        logging.info("  Received result: %s", future.result())
+        return future.result()
+
+    raise Exception(f"Error while calling service '{client.srv_name}': {future.exception()}")
+
+
+class _ServiceInterface:
+    def __init__(
+        self, node, initial_timeout=TIMEOUT_WAIT_SERVICE_INITIAL, timeout=TIMEOUT_WAIT_SERVICE
+    ):
+        self.__node = node
+
+        self.__service_clients = {
+            srv_name: (
+                _wait_for_service(self.__node, srv_name, srv_type, initial_timeout),
+                srv_type,
+            )
+            for srv_name, srv_type in self.__initial_services.items()
+        }
+        self.__service_clients.update(
+            {
+                srv_name: (_wait_for_service(self.__node, srv_name, srv_type, timeout), srv_type)
+                for srv_name, srv_type in self.__services.items()
+            }
+        )
+
+    def __init_subclass__(mcs, namespace="", initial_services={}, services={}, **kwargs):
+        super().__init_subclass__(**kwargs)
+
+        mcs.__initial_services = {
+            namespace + "/" + srv_name: srv_type for srv_name, srv_type in initial_services.items()
+        }
+        mcs.__services = {
+            namespace + "/" + srv_name: srv_type for srv_name, srv_type in services.items()
+        }
+
+        for srv_name, srv_type in list(initial_services.items()) + list(services.items()):
+            full_srv_name = namespace + "/" + srv_name
+
+            setattr(
+                mcs,
+                srv_name,
+                lambda s, full_srv_name=full_srv_name, *args, **kwargs: _call_service(
+                    s.__node,
+                    s.__service_clients[full_srv_name][0],
+                    s.__service_clients[full_srv_name][1].Request(*args, **kwargs),
+                ),
+            )
+
+
+class ActionInterface:
+    def __init__(self, node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION):
+        self.__node = node
+
+        self.__action_name = action_name
+        self.__action_type = action_type
+        self.__action_client = _wait_for_action(node, action_name, action_type, timeout)
+
+    def send_goal(self, *args, **kwargs):
+        goal = self.__action_type.Goal(*args, **kwargs)
+
+        logging.info("Sending goal to action server '%s': %s", self.__action_name, goal)
+        future = self.__action_client.send_goal_async(goal)
+
+        rclpy.spin_until_future_complete(self.__node, future)
+
+        if future.result() is not None:
+            logging.info("  Received result: %s", future.result())
+            return future.result()
+        pass
+
+    def get_result(self, goal_handle, timeout):
+        future_res = goal_handle.get_result_async()
+
+        logging.info(
+            "Waiting for action result from '%s' with timeout %fs", self.__action_name, timeout
+        )
+        rclpy.spin_until_future_complete(self.__node, future_res, timeout_sec=timeout)
+
+        if future_res.result() is not None:
+            logging.info("  Received result: %s", future_res.result().result)
+            return future_res.result().result
+        else:
+            raise Exception(
+                f"Exception while calling action '{self.__action_name}': {future_res.exception()}"
+            )
+
+
+class DashboardInterface(
+    _ServiceInterface,
+    namespace="/dashboard_client",
+    initial_services={
+        "power_on": Trigger,
+    },
+    services={
+        "power_off": Trigger,
+        "brake_release": Trigger,
+        "unlock_protective_stop": Trigger,
+        "restart_safety": Trigger,
+        "get_robot_mode": GetRobotMode,
+        "load_installation": Load,
+        "load_program": Load,
+        "close_popup": Trigger,
+        "get_loaded_program": GetLoadedProgram,
+        "program_state": GetProgramState,
+        "program_running": IsProgramRunning,
+        "play": Trigger,
+        "stop": Trigger,
+    },
+):
+    def start_robot(self):
+        self._check_call(self.power_on())
+        self._check_call(self.brake_release())
+
+        time.sleep(1)
+
+        robot_mode = self.get_robot_mode()
+        self._check_call(robot_mode)
+        if robot_mode.robot_mode.mode != RobotMode.RUNNING:
+            raise Exception(
+                f"Incorrect robot mode: Expected {RobotMode.RUNNING}, got {robot_mode.robot_mode.mode}"
+            )
+
+        self._check_call(self.stop())
+
+    def _check_call(self, result):
+        if not result.success:
+            raise Exception("Service call not successful")
+
+
+class ControllerManagerInterface(
+    _ServiceInterface,
+    namespace="/controller_manager",
+    initial_services={
+        "switch_controller": SwitchController,
+        "load_controller": LoadController,
+        "unload_controller": UnloadController,
+    },
+    services={"list_controllers": ListControllers},
+):
+    def wait_for_controller(self, controller_name, target_state="active"):
+        while True:
+            controllers = self.list_controllers().controller
+            for controller in controllers:
+                if (controller.name == controller_name) and (controller.state == target_state):
+                    return
+
+            time.sleep(1)
+
+
+class IoStatusInterface(
+    _ServiceInterface,
+    namespace="/io_and_status_controller",
+    initial_services={"set_io": SetIO},
+    services={
+        "resend_robot_program": Trigger,
+    },
+):
+    pass
+
+
+class ConfigurationInterface(
+    _ServiceInterface,
+    namespace="/ur_configuration_controller",
+    initial_services={"get_robot_software_version": GetRobotSoftwareVersion},
+    services={},
+):
+    pass
+
+
+class ForceModeInterface(
+    _ServiceInterface,
+    namespace="/force_mode_controller",
+    initial_services={},
+    services={"start_force_mode": SetForceMode, "stop_force_mode": Trigger},
+):
+    pass
+
+
+def _declare_launch_arguments():
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_type",
+            default_value="ur5e",
+            description="Type/series of used UR robot.",
+            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"],
+        )
+    )
+
+    return declared_arguments
+
+
+def _ursim_action():
+    ur_type = LaunchConfiguration("ur_type")
+
+    return ExecuteProcess(
+        cmd=[
+            PathJoinSubstitution(
+                [
+                    FindPackagePrefix("ur_client_library"),
+                    "lib",
+                    "ur_client_library",
+                    "start_ursim.sh",
+                ]
+            ),
+            "-m",
+            ur_type,
+        ],
+        name="start_ursim",
+        output="screen",
+    )
+
+
+def generate_dashboard_test_description():
+    dashboard_client = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ur_robot_driver"),
+                    "launch",
+                    "ur_dashboard_client.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "robot_ip": "192.168.56.101",
+        }.items(),
+    )
+
+    return LaunchDescription(
+        _declare_launch_arguments() + [ReadyToTest(), dashboard_client, _ursim_action()]
+    )
+
+
+def generate_driver_test_description(
+    tf_prefix="", controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL
+):
+    ur_type = LaunchConfiguration("ur_type")
+
+    launch_arguments = {
+        "robot_ip": "192.168.56.101",
+        "ur_type": ur_type,
+        "launch_rviz": "false",
+        "controller_spawner_timeout": str(controller_spawner_timeout),
+        "initial_joint_controller": "scaled_joint_trajectory_controller",
+        "headless_mode": "true",
+        "launch_dashboard_client": "true",
+        "start_joint_controller": "false",
+    }
+    if tf_prefix:
+        launch_arguments["tf_prefix"] = tf_prefix
+
+    robot_driver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
+            )
+        ),
+        launch_arguments=launch_arguments.items(),
+    )
+    wait_dashboard_server = ExecuteProcess(
+        cmd=[
+            PathJoinSubstitution(
+                [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"]
+            )
+        ],
+        name="wait_dashboard_server",
+        output="screen",
+    )
+    driver_starter = RegisterEventHandler(
+        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
+    )
+
+    return LaunchDescription(
+        _declare_launch_arguments()
+        + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter]
+    )

test/urscript_interface.py

@@ -0,0 +1,141 @@
+#!/usr/bin/env python
+# Copyright 2023, FZI Forschungszentrum Informatik
+#
+# 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.
+import os
+import sys
+import time
+import unittest
+
+import pytest
+import rclpy
+import rclpy.node
+from std_msgs.msg import String as StringMsg
+from ur_msgs.msg import IOStates
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import (  # noqa: E402
+    ControllerManagerInterface,
+    DashboardInterface,
+    IoStatusInterface,
+    generate_driver_test_description,
+)
+
+ROBOT_IP = "192.168.56.101"
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    return generate_driver_test_description()
+
+
+class URScriptInterfaceTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = rclpy.node.Node("urscript_interface_test")
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        self._dashboard_interface = DashboardInterface(self.node)
+        self._controller_manager_interface = ControllerManagerInterface(self.node)
+        self._io_status_controller_interface = IoStatusInterface(self.node)
+
+        self.urscript_pub = self.node.create_publisher(
+            StringMsg, "/urscript_interface/script_command", 1
+        )
+
+    def setUp(self):
+        self._dashboard_interface.start_robot()
+        time.sleep(1)
+        self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
+
+        self._controller_manager_interface.wait_for_controller("io_and_status_controller")
+
+    def test_set_io(self):
+        """Test setting an IO using a direct program call."""
+        self.io_states_sub = self.node.create_subscription(
+            IOStates,
+            "/io_and_status_controller/io_states",
+            self.io_msg_cb,
+            rclpy.qos.qos_profile_system_default,
+        )
+
+        self.set_digout_checked(0, True)
+        time.sleep(1)
+        self.set_digout_checked(0, False)
+
+        self.io_msg = None
+        self.io_states_sub = self.node.create_subscription(
+            IOStates,
+            "/io_and_status_controller/io_states",
+            self.io_msg_cb,
+            rclpy.qos.qos_profile_system_default,
+        )
+
+        script_msg = StringMsg(
+            data="sec my_program():\n  set_digital_out(0, False)\n  set_digital_out(1,True)\nend"
+        )
+        self.urscript_pub.publish(script_msg)
+        self.check_pin_states([0, 1], [False, True])
+
+        time.sleep(1)
+
+        script_msg = StringMsg(
+            data="sec my_program():\n  set_digital_out(0, True)\n  set_digital_out(1,False)\nend"
+        )
+        self.urscript_pub.publish(script_msg)
+        self.check_pin_states([0, 1], [True, False])
+
+    def io_msg_cb(self, msg):
+        self.io_msg = msg
+
+    def set_digout_checked(self, pin, state):
+        self.io_msg = None
+
+        script_msg = StringMsg(data=f"set_digital_out({pin}, {state})")
+        self.urscript_pub.publish(script_msg)
+
+        self.check_pin_states([pin], [state])
+
+    def check_pin_states(self, pins, states):
+        pin_states = [not x for x in states]
+        end_time = time.time() + 50
+        while pin_states != states and time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            if self.io_msg is not None:
+                for i, pin_id in enumerate(pins):
+                    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)