Adds a simple launch test for the watchdog

* Talker, heartbeat, and watchdog are launched * When heartbeat and watchdog are in sync, heartbeat and talker are shut down * Proper reaction by watchdog (going nactive) is monitored ros-safety/safety_working_group#28 ros-safety/safety_working_group#30 Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]>
boschresearch · Apr 14, 2021 · 85a6200 · 85a6200
1 parent 89eeb08
commit 85a6200
Show file tree

Hide file tree

Showing 6 changed files with 214 additions and 4 deletions.
diff --git a/sw_watchdog/CMakeLists.txt b/sw_watchdog/CMakeLists.txt
@@ -59,6 +59,21 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  # launch tests
+  find_package(launch_testing_ament_cmake REQUIRED)
+  file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/test_watchdog_shutdown_heartbeat.py" SHUTDOWN_SCRIPT)
+  file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/test_watchdog_expected_output" EXPECTED_OUTPUT)
+  configure_file(
+      "test/test_watchdog.launch.py.in"
+      "test/test_watchdog.launch.py"
+      @ONLY)
+  add_launch_test(
+    "${CMAKE_CURRENT_BINARY_DIR}/test/test_watchdog.launch.py"
+    TARGET "test_watchdog"
+    TIMEOUT 30
+    ENV
+  )
 endif()
 
 install(DIRECTORY

diff --git a/sw_watchdog/package.xml b/sw_watchdog/package.xml
@@ -6,6 +6,7 @@
   <description>Library of software watchdogs implemented as ROS 2 lifecycle nodes</description>
   <maintainer email="[email protected]">Philipp Robbel</maintainer>
   <license>Apache License 2.0</license>
+  <author email="[email protected]">Arne Nordmann</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -14,14 +15,19 @@
   <build_depend>sw_watchdog_msgs</build_depend>
 
   <exec_depend>rclcpp_lifecycle</exec_depend>
+  <exec_depend>launch</exec_depend>
   <exec_depend>lifecycle_msgs</exec_depend>
   <exec_depend>ros2run</exec_depend>
   <exec_depend>sw_watchdog_msgs</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-  <test_depend>ros_testing</test_depend>
-  <!--<exec_depend>rosidl_default_runtime</exec_depend>-->
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>lifecycle_msgs</test_depend>
+  <test_depend>rclpy</test_depend>
+  <test_depend>ros2component</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/sw_watchdog/test/test_watchdog.launch.py.in b/sw_watchdog/test/test_watchdog.launch.py.in
@@ -0,0 +1,123 @@
+# Copyright (c) 2021 Robert Bosch GmbH
+#
+# 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 os
+
+import unittest
+
+import ament_index_python
+import launch
+import launch_ros
+import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing.util
+import launch_testing_ros
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch.actions import LogInfo
+from launch.actions import RegisterEventHandler
+from launch.actions import Shutdown
+from launch.events import matches_action
+from launch.events.process import ShutdownProcess
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import LifecycleNode
+from launch_ros.descriptions import ComposableNode
+from launch_ros.event_handlers import OnStateTransition
+
+import lifecycle_msgs.msg
+
+
+def generate_test_description():
+    os.environ['OSPL_VERBOSITY'] = '8'
+    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}'
+
+    container = ComposableNodeContainer(
+            name='my_container',
+            namespace='my_namespace',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='demo_nodes_cpp',
+                    plugin='demo_nodes_cpp::Talker',
+                    name='talker'),
+                ComposableNode(
+                    package='sw_watchdog',
+                    plugin='sw_watchdog::SimpleHeartbeat',
+                    name='heartbeat',
+                    parameters=[{'period': 200}],
+                    extra_arguments=[{'use_intra_process_comms': True}]),
+            ],
+            output='screen')
+
+    watchdog_node = LifecycleNode(
+        package='sw_watchdog',
+        executable='simple_watchdog',
+        namespace='',
+        name='simple_watchdog',
+        output='screen',
+        arguments=['220', '--publish', '--activate'])
+
+    watchdog_inactive_handler = RegisterEventHandler(
+        OnStateTransition(
+            target_lifecycle_node=watchdog_node,
+            goal_state='inactive',
+            entities=[LogInfo(msg='Watchdog transitioned to `INACTIVE` state.')],
+        )
+    )
+
+    shutdown_heartbeat_when_watchdog_active = ExecuteProcess(
+        cmd=[
+            "@PYTHON_EXECUTABLE@",
+            "@SHUTDOWN_SCRIPT@"
+            ],
+        name='shutdown_heartbeat_when_watchdog_active',
+        emulate_tty=True)
+
+    launch_description = LaunchDescription()
+    launch_description.add_action(container)
+    launch_description.add_action(watchdog_node)
+    launch_description.add_action(watchdog_inactive_handler)
+    launch_description.add_action(shutdown_heartbeat_when_watchdog_active)
+    launch_description.add_action(launch_testing.util.KeepAliveProc())
+    launch_description.add_action(launch_testing.actions.ReadyToTest())
+
+    return launch_description, locals()
+
+class TestModeManagement(unittest.TestCase):
+
+    def test_processes_output(self, proc_output, watchdog_node):
+        """Check manager and nodes logging output for expected strings."""
+
+        from launch_testing.tools.output import get_default_filtered_prefixes
+        output_filter = launch_testing_ros.tools.basic_output_filter(
+            filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'],
+            filtered_rmw_implementation='@rmw_implementation@'
+        )
+        proc_output.assertWaitFor(
+            expected_output=launch_testing.tools.expected_output_from_file(path='@EXPECTED_OUTPUT@'),
+            process=watchdog_node,
+            output_filter=output_filter,
+            timeout=15
+        )
+
+        import time
+        time.sleep(1)
+
+@launch_testing.post_shutdown_test()
+class TestModeManagementShutdown(unittest.TestCase):
+
+    def test_last_process_exit_code(self, proc_info, watchdog_node):
+        launch_testing.asserts.assertExitCodes(proc_info, process=watchdog_node)
diff --git a/sw_watchdog/test/test_watchdog_expected_output.regex b/sw_watchdog/test/test_watchdog_expected_output.regex
@@ -0,0 +1,3 @@
+Watchdog raised, heartbeat sent at
+Publishing lease expiry.*missed count
+on_deactivate.*called
diff --git a/sw_watchdog/test/test_watchdog_shutdown_heartbeat.py b/sw_watchdog/test/test_watchdog_shutdown_heartbeat.py
@@ -0,0 +1,65 @@
+# Copyright (c) 2021 Robert Bosch GmbH
+#
+# 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 time
+
+from lifecycle_msgs.srv import GetState
+import rclpy
+from ros2component.api import get_components_in_container, unload_component_from_container
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = rclpy.create_node('shutdown_heartbeat')
+    cli = node.create_client(GetState, 'simple_watchdog/get_state')
+
+    while not cli.wait_for_service(timeout_sec=1.0):
+        print('simple watchdog not yet available, waiting...')
+
+    active = False
+    req = GetState.Request()
+    while not active:
+        future = cli.call_async(req)
+        rclpy.spin_until_future_complete(node, future)
+        if future.result() is not None:
+            time.sleep(1)
+            node.get_logger().info(
+                'Watchdog active, so now shutting down talker and heartbeat to test watchdog.')
+            active = True
+
+            container = 'my_namespace/my_container'
+            comps = get_components_in_container(node=node, remote_container_node_name=container)
+            print(comps)
+            for uid, error, reason in unload_component_from_container(
+                                          node=node,
+                                          remote_container_node_name=container,
+                                          component_uids=[1, 2]):
+                if error:
+                    return 'Failed to unload component {} from `{}` container node\n    {}'.format(
+                        uid, container, reason.capitalize()
+                    )
+                print('Unloaded component {} from `{}` container node'.format(
+                    uid, container
+                ))
+        else:
+            node.get_logger().error('Exception while calling service: %r' % future.exception())
+            break
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/sw_watchdog_msgs/package.xml b/sw_watchdog_msgs/package.xml
@@ -16,8 +16,6 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-  <test_depend>ros_testing</test_depend>
-  <!--<exec_depend>rosidl_default_runtime</exec_depend>-->
 
   <member_of_group>rosidl_interface_packages</member_of_group>