Fix flaky ness of opennav_docking test_docking_server (#4878) (#4879)

Call publish() (odom -> base_link tf) at startup to kick things off and
spin 10 times(1 second) TF, so that it has a chance to propogate to the
docking_server so that it will accept an action request.

Previously it was only spinning once, hoping the timer would fire and
call publish fast enough for it to propogate to the docking_server
so that it is able to accept the first 'dock_robot' action request

Signed-off-by: Mike Wake <macwake@gmail.com>

Author: ewak

nav2_docking/opennav_docking/test/test_docking_server.py

@@ -19,23 +19,37 @@
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import TransformStamped, Twist, TwistStamped
 from launch import LaunchDescription
+# from launch.actions import SetEnvironmentVariable
 from launch_ros.actions import Node
 import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing.markers
+import launch_testing.util
 from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot
 import pytest
 import rclpy
-from rclpy.action import ActionClient, ActionServer
+from rclpy.action.client import ActionClient
+from rclpy.action.server import ActionServer
 from sensor_msgs.msg import BatteryState
 from tf2_ros import TransformBroadcaster
 
 
 # This test can be run standalone with:
 # python3 -u -m pytest test_docking_server.py -s
 
+# If python3-flaky is installed, you can run the test multiple times to
+# try to identify flaky ness.
+# python3 -u -m pytest --force-flaky --min-passes 3 --max-runs 5 -s -v test_docking_server.py
+
 @pytest.mark.rostest
+# @pytest.mark.flaky
+# @pytest.mark.flaky(max_runs=5, min_passes=3)
 def generate_test_description():
 
     return LaunchDescription([
+        # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
         Node(
             package='opennav_docking',
             executable='opennav_docking',
@@ -74,21 +88,26 @@ class TestDockingServer(unittest.TestCase):
     @classmethod
     def setUpClass(cls):
         rclpy.init()
-        # Latest odom -> base_link
-        cls.x = 0.0
-        cls.y = 0.0
-        cls.theta = 0.0
-        # Track charge state
-        cls.is_charging = False
-        # Latest command velocity
-        cls.command = Twist()
-        cls.node = rclpy.create_node('test_docking_server')
 
     @classmethod
     def tearDownClass(cls):
-        cls.node.destroy_node()
         rclpy.shutdown()
 
+    def setUp(self):
+        # Create a ROS node for tests
+        # Latest odom -> base_link
+        self.x = 0.0
+        self.y = 0.0
+        self.theta = 0.0
+        # Track charge state
+        self.is_charging = False
+        # Latest command velocity
+        self.command = Twist()
+        self.node = rclpy.create_node('test_docking_server')
+
+    def tearDown(self):
+        self.node.destroy_node()
+
     def command_velocity_callback(self, msg):
         self.node.get_logger().info('Command: %f %f' % (msg.twist.linear.x, msg.twist.angular.z))
         self.command = msg.twist
@@ -175,24 +194,32 @@ def test_docking_server(self):
             'navigate_to_pose',
             self.nav_execute_callback)
 
-        # Spin once so that TF is published
-        rclpy.spin_once(self.node, timeout_sec=0.1)
+        # Publish transform
+        self.publish()
+
+        # Run for 1 seconds to allow tf to propogate
+        for _ in range(10):
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            time.sleep(0.1)
 
         # Test docking action
         self.action_result = []
-        self.dock_action_client.wait_for_server(timeout_sec=5.0)
+        assert self.dock_action_client.wait_for_server(timeout_sec=5.0), \
+               'dock_robot service not available'
+
         goal = DockRobot.Goal()
         goal.use_dock_id = True
         goal.dock_id = 'test_dock'
         future = self.dock_action_client.send_goal_async(
             goal, feedback_callback=self.action_feedback_callback)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future_original = self.goal_handle.get_result_async()
 
         # Run for 2 seconds
-        for i in range(20):
+        for _ in range(20):
             rclpy.spin_once(self.node, timeout_sec=0.1)
             time.sleep(0.1)
 
@@ -201,7 +228,8 @@ def test_docking_server(self):
             goal, feedback_callback=self.action_feedback_callback)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
@@ -216,7 +244,7 @@ def test_docking_server(self):
         self.node.get_logger().info('Goal preempted')
 
         # Run for 0.5 seconds
-        for i in range(5):
+        for _ in range(5):
             rclpy.spin_once(self.node, timeout_sec=0.1)
             time.sleep(0.1)
 
@@ -230,7 +258,8 @@ def test_docking_server(self):
             goal, feedback_callback=self.action_feedback_callback)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
@@ -247,10 +276,19 @@ def test_docking_server(self):
         future = self.undock_action_client.send_goal_async(goal)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
 
         self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED)
         self.assertTrue(self.action_result[3].result.success)
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+
+    def test_exit_code(self, proc_info):
+        # Check that all processes in the launch exit with code 0
+        launch_testing.asserts.assertExitCodes(proc_info)