diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 94e0e760a..f93239780 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -16,11 +16,11 @@ jobs: runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: 3.10.4 - name: Install system hooks run: sudo apt-get install clang-format-14 cppcheck - - uses: pre-commit/action@v3.0.0 + - uses: pre-commit/action@v3.0.1 with: extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/coverage-build.yml b/.github/workflows/coverage-build.yml index f21160d56..a1ba98881 100644 --- a/.github/workflows/coverage-build.yml +++ b/.github/workflows/coverage-build.yml @@ -45,7 +45,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3 + - uses: actions/upload-artifact@v4 with: name: colcon-logs-${{ matrix.os }} path: ros_ws/log diff --git a/.github/workflows/iron-binary-main.yml b/.github/workflows/iron-binary-main.yml index 43c8df5ec..d089e1dc6 100644 --- a/.github/workflows/iron-binary-main.yml +++ b/.github/workflows/iron-binary-main.yml @@ -6,7 +6,6 @@ on: pull_request: branches: - iron - - main # as long as rolling and iron should be compatible push: branches: - iron diff --git a/.github/workflows/iron-binary-testing.yml b/.github/workflows/iron-binary-testing.yml index a59f1daa3..ba8918604 100644 --- a/.github/workflows/iron-binary-testing.yml +++ b/.github/workflows/iron-binary-testing.yml @@ -6,7 +6,6 @@ on: pull_request: branches: - iron - - main # as long as rolling and iron should be compatible push: branches: - iron diff --git a/.github/workflows/iron-semi-binary-main.yml b/.github/workflows/iron-semi-binary-main.yml index 60d0e3412..01340fd50 100644 --- a/.github/workflows/iron-semi-binary-main.yml +++ b/.github/workflows/iron-semi-binary-main.yml @@ -6,7 +6,6 @@ on: pull_request: branches: - iron - - main # as long as rolling and iron should be compatible push: branches: - iron diff --git a/.github/workflows/iron-semi-binary-testing.yml b/.github/workflows/iron-semi-binary-testing.yml index d9a172075..809cea567 100644 --- a/.github/workflows/iron-semi-binary-testing.yml +++ b/.github/workflows/iron-semi-binary-testing.yml @@ -6,7 +6,6 @@ on: pull_request: branches: - iron - - main # as long as rolling and iron should be compatible push: branches: - iron diff --git a/.github/workflows/update-ci.yml b/.github/workflows/update-ci.yml index e9c71f597..aa75b4f4b 100644 --- a/.github/workflows/update-ci.yml +++ b/.github/workflows/update-ci.yml @@ -15,7 +15,7 @@ jobs: steps: # Setup pre-commit - uses: actions/checkout@v4 - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: 3.10.4 - name: Install pre-commit @@ -28,7 +28,7 @@ jobs: # Create pull request - name: Create pull-request id: cpr - uses: peter-evans/create-pull-request@v5 + uses: peter-evans/create-pull-request@v6 with: branch: update-ci/pre-commit-autoupdate delete-branch: true diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d64d6bb68..5538e9f38 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,13 +33,13 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.13.0 + rev: v3.15.1 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 23.9.1 + rev: 24.2.0 hooks: - id: black args: ["--line-length=100"] @@ -51,7 +51,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404"] - repo: https://github.com/pycqa/flake8 - rev: 6.1.0 + rev: 7.0.0 hooks: - id: flake8 args: ["--ignore=E501,W503"] @@ -135,7 +135,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.5 + rev: v2.2.6 hooks: - id: codespell args: ['--write-changes'] diff --git a/README.md b/README.md index ed7d45d21..d24d01bca 100644 --- a/README.md +++ b/README.md @@ -85,12 +85,19 @@ users an overview of the current released state. - `ur_moveit_config` - example MoveIt configuration for UR robots. - `ur_robot_driver` - driver / hardware interface for communication with UR robots. +## System Requirements + +Please see the [requirements for the Universal_Robots_Client_Library](https://github.com/UniversalRobots/Universal_Robots_Client_Library#requirements), as this driver is build on top of Universal_Robots_Client_Library. + ## Getting Started For getting started, you'll basically need three steps: -1. **Install the driver** (see below). You can either install this driver from binary packages or build it from source. We recommend a -binary package installation unless you want to join development and submit changes. +1. **Install the driver** + ```bash + sudo apt-get install ros-rolling-ur + ``` + See the [installation instructions](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/installation/installation.html) for more details and source-build instructions. 2. **Start & Setup the robot**. Once you've installed the driver, [setup the robot](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/installation/robot_setup.html) @@ -110,65 +117,14 @@ binary package installation unless you want to join development and submit chang documentation](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/usage.html) for details. -4. Unless started in [headless mode](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/ROS_INTERFACE.html#headless-mode): Run the external_control program by **pressing `play` on the teach pendant**. - -### 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 - ``` - sudo apt-get install ros-${ROS_DISTRO}-ur + ```bash + # Replace ur5e with one of ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20, ur30 + # Replace the IP address with the IP address of your actual robot / URSim + ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 ``` -### 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: - ``` - sudo apt install python3-colcon-common-extensions python3-vcstool - ``` - -4. Create a new ROS2 workspace: - ``` - export COLCON_WS=~/workspace/ros_ur_driver - mkdir -p $COLCON_WS/src - ``` - -5. Clone relevant packages, install dependencies, compile, and source the workspace by using: - ``` - cd $COLCON_WS - git clone 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: +4. Unless started in [headless mode](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/ROS_INTERFACE.html#headless-mode): Run the external_control program by **pressing `play` on the teach pendant**. - ``` - 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 - ``` ## MoveIt! support diff --git a/Universal_Robots_ROS2_Driver.iron.repos b/Universal_Robots_ROS2_Driver.iron.repos index 38777168b..f59c5d0e5 100644 --- a/Universal_Robots_ROS2_Driver.iron.repos +++ b/Universal_Robots_ROS2_Driver.iron.repos @@ -14,11 +14,11 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master + version: iron ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers - version: master + version: iron kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index f46209fb1..5de9d6829 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.3 (2024-02-02) +------------------ + +2.4.2 (2023-11-23) +------------------ + 2.4.1 (2023-09-21) ------------------ diff --git a/ur/package.xml b/ur/package.xml index a1abed791..bd4246969 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.4.1 + 2.4.3 Metapackage for universal robots Felix Exner Robert Wilbrandt diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 38a0a25e0..a91a20482 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.3 (2024-02-02) +------------------ + +2.4.2 (2023-11-23) +------------------ + 2.4.1 (2023-09-21) ------------------ diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index b85f4f58e..eb75cf924 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.4.1 + 2.4.3 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 0f5bb74a5..dddce272e 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.3 (2024-02-02) +------------------ + +2.4.2 (2023-11-23) +------------------ +* Update read_state_from_hardware +* Renamed normalize_joint_error to joints_angle_wraparound +* Remove noisy controller log message +* Contributors: Felix Exner, Robert Wilbrandt + 2.4.1 (2023-09-21) ------------------ * Update sjtc to newest upstream API (`#810 `_) diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index e8a492e3f..da978c356 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.4.1 + 2.4.3 Provides controllers that use the speed scaling interface of Universal Robots. Denis Stogl diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index ca65375f4..670cf9c8b 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -48,7 +48,6 @@ controller_interface::CallbackReturn GPIOController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - } catch (const std::exception& e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; @@ -63,7 +62,6 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.type = controller_interface::interface_configuration_type::INDIVIDUAL; const std::string tf_prefix = params_.tf_prefix; - RCLCPP_INFO(get_node()->get_logger(), "Configure UR gpio controller with tf_prefix: %s", tf_prefix.c_str()); for (size_t i = 0; i < 18; ++i) { config.names.emplace_back(tf_prefix + "gpio/standard_digital_output_cmd_" + std::to_string(i)); diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index e7cee27de..d1a19b70d 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -90,7 +90,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const auto compute_error_for_joint = [&](JointTrajectoryPoint& error, size_t index, const JointTrajectoryPoint& current, const JointTrajectoryPoint& desired) { // error defined as the difference between current and desired - if (normalize_joint_error_[index]) { + if (joints_angle_wraparound_[index]) { // if desired, the shortest_angular_distance is calculated, i.e., the error is // normalized between -pi ur_dashboard_msgs - 2.4.1 + 2.4.3 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 9d7791ce8..22341d144 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.3 (2024-02-02) +------------------ +* fix move_group_node crash during initialization (`#906 `_) +* Add UR30 support (`#899 `_) +* Contributors: Chen Chen, Felix Exner (fexner) + +2.4.2 (2023-11-23) +------------------ +* moveit_servo package executable name has changed (`#854 `_) +* Contributors: Felix Durchdewald + 2.4.1 (2023-09-21) ------------------ * Added support for UR20 (`#797 `_) diff --git a/ur_moveit_config/config/ur_servo.yaml b/ur_moveit_config/config/ur_servo.yaml index 980270242..767481a8a 100644 --- a/ur_moveit_config/config/ur_servo.yaml +++ b/ur_moveit_config/config/ur_servo.yaml @@ -40,7 +40,7 @@ move_group_name: ur_manipulator # Often 'manipulator' or 'arm' planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose +ee_frame: tool0 # The name of the end effector link, used to return the EE pose robot_link_command_frame: tool0 # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index 256dc1aed..07a46ed60 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -156,9 +156,18 @@ def launch_setup(context, *args, **kwargs): # Planning Configuration ompl_planning_pipeline_config = { "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, + "planning_plugins": ["ompl_interface/OMPLPlanner"], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], } } ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml") @@ -242,7 +251,7 @@ def launch_setup(context, *args, **kwargs): servo_node = Node( package="moveit_servo", condition=IfCondition(launch_servo), - executable="servo_node_main", + executable="servo_node", parameters=[ servo_params, robot_description, @@ -263,7 +272,7 @@ def generate_launch_description(): DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"], + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], ) ) declared_arguments.append( diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 477e82a58..36d1bafa1 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.4.1 + 2.4.3 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 21b5ca06d..2a2df36b7 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,20 @@ +2.4.3 (2024-02-02) +------------------ +* Add UR30 support (`#899 `_) +* Add control description and ros2_control tag to driver. (`#877 `_) +* Contributors: Felix Exner (fexner) + +2.4.2 (2023-11-23) +------------------ +* [README] Move installation instructions to subpage (`#870 `_) +* Add backward_ros to driver (`#872 `_) +* Simplify tests (`#849 `_) +* Port configuration (`#835 `_) + Added possibility to change the reverse_port, script_sender_port and trajectory_port +* [README] Update link to MoveIt! documentation +* Do not start urscipt_interface when using mock hardware +* Contributors: Felix Durchdewald, Felix Exner, RobertWilbrandt + 2.4.1 (2023-09-21) ------------------ * Added a test that sjtc correctly aborts on violation of constraints (`#810 `_) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 0074db57b..864d25b86 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -19,6 +19,7 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 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) @@ -177,7 +178,7 @@ install(PROGRAMS scripts/start_ursim.sh DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY config launch +install(DIRECTORY config launch urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/ur_robot_driver/config/ur30_update_rate.yaml b/ur_robot_driver/config/ur30_update_rate.yaml new file mode 100644 index 000000000..66ef3d736 --- /dev/null +++ b/ur_robot_driver/config/ur30_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz diff --git a/ur_robot_driver/doc/installation/installation.rst b/ur_robot_driver/doc/installation/installation.rst new file mode 100644 index 000000000..eed955798 --- /dev/null +++ b/ur_robot_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 `_. 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 `_. This + branch supports only ROS2 Rolling. For other ROS2 versions, please see the respective branches. + + Once installed, please make sure to actually `source ROS2 `_ 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, install dependencies, compile, and source the workspace by using: + + .. code-block:: bash + + cd $COLCON_WS + git clone 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 diff --git a/ur_robot_driver/doc/installation/toc.rst b/ur_robot_driver/doc/installation/toc.rst index 926be1acd..2ee60aae8 100644 --- a/ur_robot_driver/doc/installation/toc.rst +++ b/ur_robot_driver/doc/installation/toc.rst @@ -9,6 +9,7 @@ This chapter explains how to install the ``ur_robot_driver`` :maxdepth: 4 :caption: Contents: + installation real_time robot_setup install_urcap_cb3 diff --git a/ur_robot_driver/doc/usage.rst b/ur_robot_driver/doc/usage.rst index 48c59c745..1a3acd329 100644 --- a/ur_robot_driver/doc/usage.rst +++ b/ur_robot_driver/doc/usage.rst @@ -20,7 +20,7 @@ The arguments for launch files can be listed using ``ros2 launch ur_robot_driver 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*\ ). +* ``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_mock_hardware`` (default: *false* ) - use simple hardware emulator from ros2_control. Useful for testing launch files, descriptions, etc. See explanation below. @@ -106,7 +106,7 @@ For details on the Docker image, please see the more detailed guide :ref:`here < Example Commands for Testing the Driver --------------------------------------- -Allowed UR - Type strings: ``ur3``\ , ``ur3e``\ , ``ur5``\ , ``ur5e``\ , ``ur10``\ , ``ur10e``\ , ``ur16e``\ , ``ur20``. +Allowed UR - Type strings: ``ur3``\ , ``ur3e``\ , ``ur5``\ , ``ur5e``\ , ``ur10``\ , ``ur10e``\ , ``ur16e``\ , ``ur20``, ``ur30``. 1. Start hardware, simulator or mockup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -189,7 +189,7 @@ To test the driver with the example MoveIt-setup, first start the driver as desc 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 `_. +robot as explained `here `_. Mock hardware ^^^^^^^^^^^^^ diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 63f762f09..fe1ede6f7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -96,8 +96,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::vector 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_deactivate(const rclcpp_lifecycle::State& previous_state) final; + hardware_interface::CallbackReturn on_cleanup(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; @@ -223,7 +224,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::unique_ptr ur_driver_; std::shared_ptr async_thread_; - bool rtde_comm_has_been_started_ = false; + std::atomic_bool rtde_comm_has_been_started_ = false; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur30.launch.py b/ur_robot_driver/launch/ur30.launch.py new file mode 100644 index 000000000..2ac76eb65 --- /dev/null +++ b/ur_robot_driver/launch/ur30.launch.py @@ -0,0 +1,102 @@ +# 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_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. \ + Used only if 'use_mock_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", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_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_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 46ba61b9d..91e0a19f1 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -36,7 +36,12 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) def launch_setup(context, *args, **kwargs): @@ -71,21 +76,43 @@ def launch_setup(context, *args, **kwargs): 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"] ) kinematics_params = PathJoinSubstitution( - [FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"] + [ + FindPackageShare(description_package), + "config", + ur_type, + "default_kinematics.yaml", + ] ) physical_params = PathJoinSubstitution( - [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"] + [ + FindPackageShare(description_package), + "config", + ur_type, + "physical_parameters.yaml", + ] ) visual_params = PathJoinSubstitution( - [FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"] + [ + FindPackageShare(description_package), + "config", + ur_type, + "visual_parameters.yaml", + ] ) script_filename = PathJoinSubstitution( - [FindPackageShare("ur_client_library"), "resources", "external_control.urscript"] + [ + FindPackageShare("ur_client_library"), + "resources", + "external_control.urscript", + ] ) input_recipe_filename = PathJoinSubstitution( [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"] @@ -98,7 +125,7 @@ def launch_setup(context, *args, **kwargs): [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", - PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]), + PathJoinSubstitution([FindPackageShare("ur_robot_driver"), "urdf", description_file]), " ", "robot_ip:=", robot_ip, @@ -181,6 +208,15 @@ def launch_setup(context, *args, **kwargs): "script_command_port:=", script_command_port, " ", + "reverse_port:=", + reverse_port, + " ", + "script_sender_port:=", + script_sender_port, + " ", + "trajectory_port:=", + trajectory_port, + " ", ] ) robot_description = {"robot_description": robot_description_content} @@ -263,6 +299,7 @@ def launch_setup(context, *args, **kwargs): executable="urscript_interface", parameters=[{"robot_ip": robot_ip}], output="screen", + condition=UnlessCondition(use_mock_hardware), ) controller_stopper_node = Node( @@ -303,31 +340,31 @@ def launch_setup(context, *args, **kwargs): ) # Spawn controllers - def controller_spawner(name, active=True): + def controller_spawner(controllers, active=True): inactive_flags = ["--inactive"] if not active else [] return Node( package="controller_manager", executable="spawner", arguments=[ - name, "--controller-manager", "/controller_manager", "--controller-manager-timeout", controller_spawner_timeout, ] - + inactive_flags, + + inactive_flags + + controllers, ) - controller_spawner_names = [ + controllers_active = [ "joint_state_broadcaster", "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controller_spawner_inactive_names = ["forward_position_controller"] + controllers_inactive = ["forward_position_controller"] - controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [ - controller_spawner(name, active=False) for name in controller_spawner_inactive_names + controller_spawners = [controller_spawner(controllers_active)] + [ + controller_spawner(controllers_inactive, active=False) ] # There may be other controllers of the joints, but this is the initially-started one @@ -381,7 +418,17 @@ def generate_launch_description(): DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], ) ) declared_arguments.append( @@ -498,7 +545,9 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "launch_dashboard_client", default_value="true", description="Launch Dashboard Client?" + "launch_dashboard_client", + default_value="true", + description="Launch Dashboard Client?", ) ) declared_arguments.append( @@ -583,8 +632,28 @@ def generate_launch_description(): DeclareLaunchArgument( "script_command_port", default_value="50004", - description="Port that will be opened to forward script commands from the driver to the robot", + 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)]) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 82a58bf04..64fce7894 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.4.1 + 2.4.3 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Denis Stogl @@ -26,6 +26,7 @@ ament_cmake ament_cmake_python + backward_ros controller_manager controller_manager_msgs geometry_msgs diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2434e5248..8ff61080e 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -59,7 +59,7 @@ URPositionHardwareInterface::~URPositionHardwareInterface() { // If the controller manager is shutdown via Ctrl + C the on_deactivate methods won't be called. // We therefore need to make sure to actually deactivate the communication - on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); } hardware_interface::CallbackReturn @@ -302,7 +302,7 @@ std::vector URPositionHardwareInterface::e } hardware_interface::CallbackReturn -URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) +URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Starting ...please wait..."); @@ -463,13 +463,22 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous } hardware_interface::CallbackReturn -URPositionHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& previous_state) +URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Activating HW interface"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn +URPositionHardwareInterface::on_cleanup(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping ...please wait..."); - async_thread_shutdown_ = true; - async_thread_->join(); - async_thread_.reset(); + if (async_thread_) { + async_thread_shutdown_ = true; + async_thread_->join(); + async_thread_.reset(); + } ur_driver_.reset(); diff --git a/ur_robot_driver/test/dashboard_client.py b/ur_robot_driver/test/dashboard_client.py index cea1b5881..d55d27828 100755 --- a/ur_robot_driver/test/dashboard_client.py +++ b/ur_robot_driver/test/dashboard_client.py @@ -26,84 +26,23 @@ # 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 launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription -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.node import Node -from std_srvs.srv import Trigger from ur_dashboard_msgs.msg import RobotMode -from ur_dashboard_msgs.srv import ( - GetLoadedProgram, - GetProgramState, - GetRobotMode, - IsProgramRunning, - Load, -) -TIMEOUT_WAIT_SERVICE = 10 -# 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_SERVICE_INITIAL = 120 +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(): - 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"], - ) - ) - - ur_type = LaunchConfiguration("ur_type") - - dashboard_client = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("ur_robot_driver"), - "launch", - "ur_dashboard_client.launch.py", - ] - ) - ), - launch_arguments={ - "robot_ip": "192.168.56.101", - }.items(), - ) - ursim = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [ - FindPackagePrefix("ur_client_library"), - "lib", - "ur_client_library", - "start_ursim.sh", - ] - ), - " ", - "-m ", - ur_type, - ], - name="start_ursim", - output="screen", - ) - - return LaunchDescription(declared_arguments + [ReadyToTest(), dashboard_client, ursim]) + return generate_dashboard_test_description() class DashboardClientTest(unittest.TestCase): @@ -121,38 +60,7 @@ def tearDownClass(cls): rclpy.shutdown() def init_robot(self): - # We wait longer for the first client, as the robot is still starting up - power_on_client = waitForService( - self.node, "/dashboard_client/power_on", Trigger, timeout=TIMEOUT_WAIT_SERVICE_INITIAL - ) - - # Connect to all other expected services - dashboard_interfaces = { - "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, - } - self.dashboard_clients = { - srv_name: waitForService(self.node, f"/dashboard_client/{srv_name}", srv_type) - for (srv_name, srv_type) in dashboard_interfaces.items() - } - - # Add first client to dict - self.dashboard_clients["power_on"] = power_on_client - - # - # Test functions - # + self._dashboard_interface = DashboardInterface(self.node) def test_switch_on(self): """Test power on a robot.""" @@ -161,59 +69,34 @@ def test_switch_on(self): mode = RobotMode.DISCONNECTED while mode != RobotMode.POWER_OFF and time.time() < end_time: time.sleep(0.1) - result = self.call_dashboard_service("get_robot_mode", GetRobotMode.Request()) + result = self._dashboard_interface.get_robot_mode() self.assertTrue(result.success) mode = result.robot_mode.mode # Power on robot - self.assertTrue(self.call_dashboard_service("power_on", Trigger.Request()).success) + 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.call_dashboard_service("get_robot_mode", GetRobotMode.Request()) + 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.call_dashboard_service("brake_release", Trigger.Request()).success) + 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.call_dashboard_service("get_robot_mode", GetRobotMode.Request()) + result = self._dashboard_interface.get_robot_mode() self.assertTrue(result.success) mode = result.robot_mode.mode self.assertEqual(mode, RobotMode.RUNNING) - - # - # Utility functions - # - - def call_dashboard_service(self, srv_name, request): - self.node.get_logger().info( - f"Calling dashboard service '{srv_name}' with request {request}" - ) - future = self.dashboard_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - self.node.get_logger().info(f"Received result {future.result()}") - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - -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 diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index effc93e32..96c859947 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -26,8 +26,9 @@ # 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 @@ -37,31 +38,20 @@ from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory from controller_manager_msgs.srv import SwitchController -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 rclpy.node import Node from sensor_msgs.msg import JointState -from std_srvs.srv import Trigger from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from ur_dashboard_msgs.msg import RobotMode -from ur_dashboard_msgs.srv import GetRobotMode from ur_msgs.msg import IOStates -from ur_msgs.srv import SetIO -TIMEOUT_WAIT_SERVICE = 10 -TIMEOUT_WAIT_SERVICE_INITIAL = 60 -TIMEOUT_WAIT_ACTION = 10 +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ActionInterface, + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) + TIMEOUT_EXECUTE_TRAJECTORY = 30 ROBOT_JOINTS = [ @@ -80,70 +70,7 @@ [(""), ("my_ur_")], ) def generate_test_description(tf_prefix): - 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"], - ) - ) - - ur_type = LaunchConfiguration("ur_type") - - robot_driver = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] - ) - ), - launch_arguments={ - "robot_ip": "192.168.56.101", - "ur_type": ur_type, - "launch_rviz": "false", - "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), - "initial_joint_controller": "scaled_joint_trajectory_controller", - "headless_mode": "true", - "launch_dashboard_client": "false", - "start_joint_controller": "false", - "tf_prefix": tf_prefix, - }.items(), - ) - ursim = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [ - FindPackagePrefix("ur_client_library"), - "lib", - "ur_client_library", - "start_ursim.sh", - ] - ), - " ", - "-m ", - ur_type, - ], - name="start_ursim", - output="screen", - ) - 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( - declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] - ) + return generate_driver_test_description(tf_prefix=tf_prefix) class RobotDriverTest(unittest.TestCase): @@ -162,70 +89,32 @@ def tearDownClass(cls): rclpy.shutdown() def init_robot(self): - # Wait longer for the first service clients: - # - The robot has to start up - # - The controller_manager has to start - # - The controllers need to load and activate - service_interfaces_initial = { - "/dashboard_client/power_on": Trigger, - "/controller_manager/switch_controller": SwitchController, - "/io_and_status_controller/set_io": SetIO, - } - self.service_clients = { - srv_name: waitForService( - self.node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE_INITIAL - ) - for (srv_name, srv_type) in service_interfaces_initial.items() - } - - # Connect to the rest of the required interfaces - service_interfaces = { - "/dashboard_client/brake_release": Trigger, - "/dashboard_client/stop": Trigger, - "/dashboard_client/get_robot_mode": GetRobotMode, - "/controller_manager/switch_controller": SwitchController, - "/io_and_status_controller/resend_robot_program": Trigger, - } - self.service_clients.update( - { - srv_name: waitForService(self.node, srv_name, srv_type) - for (srv_name, srv_type) in service_interfaces.items() - } - ) + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) - action_interfaces = { - "/scaled_joint_trajectory_controller/follow_joint_trajectory": FollowJointTrajectory - } - self.action_clients = { - action_name: waitForAction(self.node, action_name, action_type) - for (action_name, action_type) in action_interfaces.items() - } + self._scaled_follow_joint_trajectory = ActionInterface( + self.node, + "/scaled_joint_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) def setUp(self): - # Start robot - empty_req = Trigger.Request() - self.call_service("/dashboard_client/power_on", empty_req) - self.call_service("/dashboard_client/brake_release", empty_req) - time.sleep(1) - robot_mode_resp = self.call_service( - "/dashboard_client/get_robot_mode", GetRobotMode.Request() - ) - self.assertEqual(robot_mode_resp.robot_mode.mode, RobotMode.RUNNING) - self.call_service("/dashboard_client/stop", empty_req) + self._dashboard_interface.start_robot() time.sleep(1) - self.call_service("/io_and_status_controller/resend_robot_program", empty_req) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) # # Test functions # def test_start_scaled_jtc_controller(self): - req = SwitchController.Request() - req.strictness = SwitchController.Request.BEST_EFFORT - req.activate_controllers = ["scaled_joint_trajectory_controller"] - result = self.call_service("/controller_manager/switch_controller", req) - - self.assertEqual(result.ok, True) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) def test_set_io(self): """Test to set an IO and check whether it has been set.""" @@ -246,13 +135,8 @@ def io_msg_cb(msg): # Set pin 0 to 1.0 test_pin = 0 - set_io_req = SetIO.Request() - set_io_req.fun = 1 - set_io_req.pin = test_pin - set_io_req.state = 1.0 - - self.node.get_logger().info(f"Setting pin {test_pin} to {set_io_req.state}") - self.call_service("/io_and_status_controller/set_io", set_io_req) + 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 @@ -262,12 +146,11 @@ def io_msg_cb(msg): if io_msg is not None: pin_state = io_msg.digital_out_states[test_pin].state - self.assertEqual(pin_state, set_io_req.state) + self.assertEqual(pin_state, 1.0) # Set pin 0 to 0.0 - set_io_req.state = 0.0 - self.node.get_logger().info(f"Setting pin {test_pin} to {set_io_req.state}") - self.call_service("/io_and_status_controller/set_io", set_io_req) + 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 @@ -276,7 +159,7 @@ def io_msg_cb(msg): if io_msg is not None: pin_state = io_msg.digital_out_states[test_pin].state - self.assertEqual(pin_state, set_io_req.state) + self.assertEqual(pin_state, 0.0) # Clean up io subscription self.node.destroy_subscription(io_states_sub) @@ -299,21 +182,15 @@ def test_trajectory(self, tf_prefix): ) # Sending trajectory goal - self.node.get_logger().info("Sending simple goal") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - FollowJointTrajectory.Goal(trajectory=trajectory), - ) - self.assertEqual(goal_response.accepted, True) + 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.get_result( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - goal_response, - TIMEOUT_EXECUTE_TRAJECTORY, + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) - self.node.get_logger().info("Received result SUCCESSFUL") def test_illegal_trajectory(self, tf_prefix): """ @@ -336,15 +213,13 @@ def test_illegal_trajectory(self, tf_prefix): ) # Send illegal goal - self.node.get_logger().info("Sending illegal goal") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - FollowJointTrajectory.Goal(trajectory=trajectory), + logging.info("Sending illegal goal") + goal_handle = self._scaled_follow_joint_trajectory.send_goal( + trajectory=trajectory, ) # Verify the failure is correctly detected - self.assertEqual(goal_response.accepted, False) - self.node.get_logger().info("Goal response REJECTED") + self.assertFalse(goal_handle.accepted) def test_trajectory_scaled(self, tf_prefix): """Test robot movement.""" @@ -362,30 +237,17 @@ def test_trajectory_scaled(self, tf_prefix): ], ) - goal = FollowJointTrajectory.Goal(trajectory=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) - # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message - # see https://github.com/ros-controls/ros2_controllers/issues/249 - # self.node.get_logger().info("Sending scaled goal without time restrictions") - self.node.get_logger().info("Sending goal for robot to follow") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", goal + # Verify execution + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, + TIMEOUT_EXECUTE_TRAJECTORY, ) - - 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.assertIn( - result.error_code, - (FollowJointTrajectory.Result.SUCCESSFUL,), - ) - - self.node.get_logger().info("Received result") + 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.""" @@ -416,46 +278,38 @@ def js_cb(msg): joint_state_sub = self.node.create_subscription(JointState, "/joint_states", js_cb, 1) joint_state_sub # prevent warning about unused variable - goal = FollowJointTrajectory.Goal(trajectory=trajectory) + # 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) - self.node.get_logger().info("Sending goal for robot to follow") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", goal + # 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) - self.assertEqual(goal_response.accepted, True) + state_when_aborted = last_joint_state - if goal_response.accepted: - result = self.get_result( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - goal_response, - TIMEOUT_EXECUTE_TRAJECTORY, - ) - self.assertIn( - result.error_code, - (FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED,), - ) - self.node.get_logger().info("Received result") - - # self.node.get_logger().info(f"Joint state before sleep {last_joint_state.position}") - 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 - self.node.get_logger().info(f"before: {state_when_aborted.position.tolist()}") - self.node.get_logger().info(f"after: {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) - ] - ) + # 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 @@ -471,60 +325,3 @@ def js_cb(msg): # 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") - - # - # Utility functions - # - - def call_service(self, srv_name, request): - self.node.get_logger().info(f"Calling service '{srv_name}' with request {request}") - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - self.node.get_logger().info(f"Received result {future.result()}") - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - def call_action(self, action_name, goal): - self.node.get_logger().info(f"Sending goal to action server '{action_name}'") - future = self.action_clients[action_name].send_goal_async(goal) - 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, action_name, goal_response, timeout): - self.node.get_logger().info( - f"Waiting for result for action server '{action_name}' (timeout: {timeout} seconds)" - ) - future_res = self.action_clients[action_name]._get_result_async(goal_response) - rclpy.spin_until_future_complete(self.node, future_res, timeout_sec=timeout) - - if future_res.result() is not None: - self.node.get_logger().info(f"Received result {future_res.result().result}") - return future_res.result().result - else: - raise Exception(f"Exception while calling action: {future_res.exception()}") - - -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 diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py new file mode 100644 index 000000000..5b6033cea --- /dev/null +++ b/ur_robot_driver/test/test_common.py @@ -0,0 +1,345 @@ +# 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 +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 + +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 + + +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}, + 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 + + +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") + + robot_driver = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] + ) + ), + 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": "false", + "start_joint_controller": "false", + "tf_prefix": tf_prefix, + }.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] + ) diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index edc1aaf0d..724ad3abe 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -26,114 +26,31 @@ # 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 pytest +import os +import sys import time import unittest +import pytest import rclpy import rclpy.node -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 std_srvs.srv import Trigger from std_msgs.msg import String as StringMsg -from ur_dashboard_msgs.msg import RobotMode -from ur_dashboard_msgs.srv import ( - GetLoadedProgram, - GetProgramState, - GetRobotMode, - IsProgramRunning, - Load, -) from ur_msgs.msg import IOStates -from controller_manager_msgs.srv import ListControllers +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" -TIMEOUT_WAIT_SERVICE = 10 -# 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_SERVICE_INITIAL = 120 @pytest.mark.launch_test def generate_test_description(): - 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"], - ) - ) - - ur_type = LaunchConfiguration("ur_type") - - robot_driver = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] - ) - ), - launch_arguments={ - "robot_ip": "192.168.56.101", - "ur_type": ur_type, - "launch_rviz": "false", - "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), - "initial_joint_controller": "scaled_joint_trajectory_controller", - "headless_mode": "true", - "launch_dashboard_client": "false", - "start_joint_controller": "false", - }.items(), - ) - - ursim = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [ - FindPackagePrefix("ur_client_library"), - "lib", - "ur_client_library", - "start_ursim.sh", - ] - ), - " ", - "-m ", - ur_type, - ], - name="start_ursim", - output="screen", - ) - - 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( - declared_arguments + [ReadyToTest(), wait_dashboard_server, driver_starter, ursim] - ) + return generate_driver_test_description() class URScriptInterfaceTest(unittest.TestCase): @@ -151,70 +68,20 @@ def tearDownClass(cls): rclpy.shutdown() def init_robot(self): - # We wait longer for the first client, as the robot is still starting up - power_on_client = waitForService( - self.node, "/dashboard_client/power_on", Trigger, timeout=TIMEOUT_WAIT_SERVICE_INITIAL - ) - - # Connect to all other expected services - dashboard_interfaces = { - "/dashboard_client/power_off": Trigger, - "/dashboard_client/brake_release": Trigger, - "/dashboard_client/unlock_protective_stop": Trigger, - "/dashboard_client/restart_safety": Trigger, - "/dashboard_client/get_robot_mode": GetRobotMode, - "/dashboard_client/load_installation": Load, - "/dashboard_client/load_program": Load, - "/dashboard_client/close_popup": Trigger, - "/dashboard_client/get_loaded_program": GetLoadedProgram, - "/dashboard_client/program_state": GetProgramState, - "/dashboard_client/program_running": IsProgramRunning, - "/dashboard_client/play": Trigger, - "/dashboard_client/stop": Trigger, - } - self.service_clients = { - srv_name: waitForService(self.node, f"{srv_name}", srv_type) - for (srv_name, srv_type) in dashboard_interfaces.items() - } - - self.service_clients["/controller_manager/list_controllers"] = waitForService( - self.node, - "/controller_manager/list_controllers", - ListControllers, - timeout=TIMEOUT_WAIT_SERVICE_INITIAL, - ) - - # Add first client to dict - self.service_clients["/dashboard_client/power_on"] = power_on_client + 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): - # Start robot - empty_req = Trigger.Request() - self.call_service("/dashboard_client/power_on", empty_req) - self.call_service("/dashboard_client/brake_release", empty_req) - + self._dashboard_interface.start_robot() time.sleep(1) - robot_mode_resp = self.call_service( - "/dashboard_client/get_robot_mode", GetRobotMode.Request() - ) - self.assertEqual(robot_mode_resp.robot_mode.mode, RobotMode.RUNNING) - self.call_service("/dashboard_client/stop", empty_req) - time.sleep(1) - - io_controller_running = False + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) - while not io_controller_running: - time.sleep(1) - response = self.call_service( - "/controller_manager/list_controllers", ListControllers.Request() - ) - for controller in response.controller: - if controller.name == "io_and_status_controller": - io_controller_running = controller.state == "active" + 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.""" @@ -272,22 +139,3 @@ def check_pin_states(self, pins, states): pin_states[i] = self.io_msg.digital_out_states[pin_id].state self.assertIsNotNone(self.io_msg, "Did not receive an IO state in requested time.") self.assertEqual(pin_states, states) - - def call_service(self, srv_name, request): - self.node.get_logger().info(f"Calling service '{srv_name}' with request {request}") - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - self.node.get_logger().info(f"Received result {future.result()}") - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - -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 diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro new file mode 100644 index 000000000..308071cc1 --- /dev/null +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem + ${mock_sensor_commands} + 0.0 + + + ur_robot_driver/URPositionHardwareInterface + ${robot_ip} + ${script_filename} + ${output_recipe_filename} + ${input_recipe_filename} + ${headless_mode} + ${reverse_port} + ${script_sender_port} + ${reverse_ip} + ${script_command_port} + ${trajectory_port} + ${tf_prefix} + ${non_blocking_read} + 2000 + 0.03 + ${use_tool_communication} + ${kinematics_hash} + ${tool_voltage} + ${tool_parity} + ${tool_baud_rate} + ${tool_stop_bits} + ${tool_rx_idle_chars} + ${tool_tx_idle_chars} + ${tool_device_name} + ${tool_tcp_port} + ${keep_alive_count} + + + + + + + + + + + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur_robot_driver/urdf/ur.urdf.xacro b/ur_robot_driver/urdf/ur.urdf.xacro new file mode 100644 index 000000000..d72a573e4 --- /dev/null +++ b/ur_robot_driver/urdf/ur.urdf.xacro @@ -0,0 +1,106 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +