Converted io_test and switch_on_test to ROS2 (#124)

* Converted io_test and switch_on_test to ROS2 and configured pipeline to start ursim

Author: urmahp

.github/workflows/ci-build-binary.yml

@@ -20,7 +20,13 @@ jobs:
           - {ROS_DISTRO: foxy, ROS_REPO: testing}
     env:
       UPSTREAM_WORKSPACE: Universal_Robots_ROS2_Driver.repos
+      DOCKER_RUN_OPTS: --network static_test_net
+      BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
+      IMMEDIATE_TEST_OUTPUT: true
     steps:
       - uses: actions/checkout@v1
+      - name: start ursim
+        run: |
+          .github/dockerursim/build_and_run_docker_ursim.sh
       - uses: 'ros-industrial/industrial_ci@master'
         env: ${{matrix.env}}

.github/workflows/ci-build-coverage.yml

@@ -19,6 +19,10 @@ jobs:
       - uses: ros-tooling/[email protected]
         with:
           required-ros-distributions: foxy
+      - uses: actions/checkout@v1
+      - name: start ursim
+        run: |
+          .github/dockerursim/build_and_run_docker_ursim.sh
       - uses: ros-tooling/[email protected]
         with:
           target-ros2-distro: foxy

ur_robot_driver/CMakeLists.txt

@@ -107,3 +107,12 @@ ament_export_dependencies(
 )
 
 ament_package()
+
+if(BUILD_TESTING)
+  find_package(ur_controllers REQUIRED)
+  find_package(ur_description REQUIRED)
+  find_package(ur_bringup REQUIRED)
+  find_package(ur_msgs REQUIRED)
+  find_package(launch_testing_ament_cmake)
+  add_launch_test(test/integration_test.py)
+endif()

ur_robot_driver/package.xml

@@ -30,6 +30,11 @@
   <!-- TODO: rosdep cannot find ur_client_library -->
   <depend>ur_client_library</depend>
   <depend>ur_dashboard_msgs</depend>
+  <depend>launch_testing_ament_cmake</depend>
+  <depend>ur_msgs</depend>
+  <depend>ur_description</depend>
+  <depend>ur_bringup</depend>
+  <depend>ur_controllers</depend>
 
   <exec_depend>controller_manager</exec_depend>
 

ur_robot_driver/test/integration_test.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python
+# Copyright 2019, FZI Forschungszentrum Informatik
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import unittest
+import os
+import time
+import pytest
+
+import launch_testing
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+import rclpy
+from rclpy.node import Node
+
+from ur_msgs.srv import SetIO
+from ur_msgs.msg import IOStates
+from ur_dashboard_msgs.srv import GetRobotMode
+from ur_dashboard_msgs.msg import RobotMode
+from std_srvs.srv import Trigger
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            default_value="192.168.56.101",
+            description="IP address by which the robot can be reached.",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_type", default_value="ur5e", description="Type/series of used UR robot."
+        )
+    )
+
+    ur_type = LaunchConfiguration("ur_type")
+    robot_ip = LaunchConfiguration("robot_ip")
+    dir_path = os.path.dirname(os.path.realpath(__file__))
+
+    launch_file = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([dir_path, "/../../ur_bringup/launch/ur_control.launch.py"]),
+        launch_arguments={"robot_ip": robot_ip, "ur_type": ur_type, "launch_rviz": "false"}.items(),
+    )
+
+    return LaunchDescription(
+        declared_arguments + [launch_testing.actions.ReadyToTest(), launch_file]
+    )
+
+
+class IOTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = Node("ur_robot_driver_integrations_test")
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        # Initialize clients
+        self.set_io_client = self.node.create_client(SetIO, "/io_and_status_controller/set_io")
+        if self.set_io_client.wait_for_service(10) is False:
+            raise Exception(
+                "Could not reach set IO service, make sure that the driver is actually running"
+            )
+
+        self.power_on_client = self.node.create_client(Trigger, "/dashboard_client/power_on")
+        if self.power_on_client.wait_for_service(10) is False:
+            raise Exception(
+                "Could not reach power on service, make sure that the driver is actually running"
+            )
+
+        self.brake_release_client = self.node.create_client(
+            Trigger, "/dashboard_client/brake_release"
+        )
+        if self.brake_release_client.wait_for_service(10) is False:
+            raise Exception(
+                "Could not reach brake release service, make sure that the driver is actually running"
+            )
+
+        self.get_robot_mode_client = self.node.create_client(
+            GetRobotMode, "/dashboard_client/get_robot_mode"
+        )
+        if self.get_robot_mode_client.wait_for_service(10) is False:
+            raise Exception(
+                "Could not reach get robot mode service, make sure that the driver is actually running"
+            )
+
+    def test_switch_on(self):
+        """Test power on a robot."""
+        empty_req = Trigger.Request()
+        get_robot_mode_req = GetRobotMode.Request()
+
+        self.power_on_client.call_async(empty_req)
+        end_time = time.time() + 10
+        mode = RobotMode.DISCONNECTED
+        while mode not in (RobotMode.IDLE, RobotMode.RUNNING) and time.time() < end_time:
+            future = self.get_robot_mode_client.call_async(get_robot_mode_req)
+            while future.done() is False:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+            mode = future.result().robot_mode.mode
+
+        self.assertIn(mode, (RobotMode.IDLE, RobotMode.RUNNING))
+
+        self.brake_release_client.call_async(empty_req)
+        end_time = time.time() + 10
+        while mode != RobotMode.RUNNING and time.time() < end_time:
+            future = self.get_robot_mode_client.call_async(get_robot_mode_req)
+            while future.done() is False:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+            mode = future.result().robot_mode.mode
+
+        self.assertEqual(mode, RobotMode.RUNNING)
+
+    def test_set_io(self):
+        """Test to set an IO and check whether it has been set."""
+        self.io_msg = None
+        io_states_sub = self.node.create_subscription(
+            IOStates,
+            "/io_and_status_controller/io_states",
+            self.io_msg_cb,
+            rclpy.qos.qos_profile_system_default,
+        )
+
+        pin = 0
+        set_io_req = SetIO.Request()
+        set_io_req.fun = 1
+        set_io_req.pin = pin
+        set_io_req.state = 1.0
+
+        self.set_io_client.call_async(set_io_req)
+        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 self.io_msg is not None:
+                pin_state = self.io_msg.digital_out_states[pin].state
+
+        self.assertEqual(pin_state, 1)
+
+        set_io_req.state = 0.0
+        self.set_io_client.call_async(set_io_req)
+
+        end_time = time.time() + 5
+        while pin_state and time.time() < end_time:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            if self.io_msg is not None:
+                pin_state = self.io_msg.digital_out_states[pin].state
+
+        self.assertEqual(pin_state, 0)
+
+        self.node.destroy_subscription(io_states_sub)
+
+    def io_msg_cb(self, msg):
+        self.io_msg = msg