diff --git a/system_modes/test/launchtest/manager_and_monitor.py b/system_modes/test/launchtest/manager_and_monitor.py index 9c14f89..99eb8cd 100644 --- a/system_modes/test/launchtest/manager_and_monitor.py +++ b/system_modes/test/launchtest/manager_and_monitor.py @@ -1,3 +1,5 @@ +from time import sleep + from lifecycle_msgs.srv import ChangeState from rcl_interfaces.msg import SetParametersResult @@ -85,19 +87,15 @@ def main(args=None): lc.configure_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.activate_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.change_mode('CC') - executor.spin() finally: executor.shutdown() diff --git a/system_modes/test/launchtest/modes_observer.launch.py.in b/system_modes/test/launchtest/modes_observer.launch.py.in index 47e49d1..3a9b386 100644 --- a/system_modes/test/launchtest/modes_observer.launch.py.in +++ b/system_modes/test/launchtest/modes_observer.launch.py.in @@ -18,15 +18,11 @@ def generate_test_description(): modelfile = '@MODELFILE@' - modes_observer = ExecuteProcess( - cmd=[ - "ros2", - "run", - "system_modes", - "modes_observer_test_node"], - name='modes_observer_test_node', - emulate_tty=True, - output='screen') + modes_observer = launch_ros.actions.Node( + package='system_modes', + executable='modes_observer_test_node', + emulate_tty=True, + output='screen') mode_manager = launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( @@ -39,9 +35,7 @@ def generate_test_description(): "@PYTHON_EXECUTABLE@", "@TEST_NODES@" ], - name='test_nodes', - emulate_tty=True, - output='screen') + name='test_nodes',) launch_description = LaunchDescription() launch_description.add_action(modes_observer) diff --git a/system_modes/test/launchtest/modes_observer.py b/system_modes/test/launchtest/modes_observer.py index eb8aac2..a2171c9 100644 --- a/system_modes/test/launchtest/modes_observer.py +++ b/system_modes/test/launchtest/modes_observer.py @@ -117,23 +117,15 @@ def main(args=None): lc.configure_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.activate_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.change_mode('CC') - executor.spin() finally: executor.shutdown() diff --git a/system_modes/test/launchtest/redundant_mode_changes.py b/system_modes/test/launchtest/redundant_mode_changes.py index f1e1f04..95dfa80 100644 --- a/system_modes/test/launchtest/redundant_mode_changes.py +++ b/system_modes/test/launchtest/redundant_mode_changes.py @@ -1,7 +1,10 @@ +from time import sleep + from lifecycle_msgs.srv import ChangeState from rcl_interfaces.msg import SetParametersResult import rclpy + from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.parameter import Parameter @@ -102,33 +105,28 @@ def main(args=None): lc.configure_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) lc.activate_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.change_A_mode('AA') executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - lc.change_A_mode('AA') # redundant, should be ignored - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - lc.change_A_mode('BB') - executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) + sleep(3) # give the system some time to converge + + lc.change_A_mode('AA') # this is the tested aspect: call redundant, should be ignored executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge + lc.change_A_mode('BB') executor.spin() finally: executor.shutdown() diff --git a/system_modes/test/launchtest/redundant_mode_changes_expected_output.regex b/system_modes/test/launchtest/redundant_mode_changes_expected_output.regex index 3419036..6b4d991 100644 --- a/system_modes/test/launchtest/redundant_mode_changes_expected_output.regex +++ b/system_modes/test/launchtest/redundant_mode_changes_expected_output.regex @@ -1,4 +1,3 @@ -Transition [AB]:configure -Transition [AB]:activate +Transition [AB] Parameter callback #0 A:foo:0.2 Parameter callback #1 A:foo:0.3 diff --git a/system_modes/test/launchtest/two_independent_hierarchies.py b/system_modes/test/launchtest/two_independent_hierarchies.py index 82ef6a3..f293929 100644 --- a/system_modes/test/launchtest/two_independent_hierarchies.py +++ b/system_modes/test/launchtest/two_independent_hierarchies.py @@ -1,3 +1,5 @@ +from time import sleep + from lifecycle_msgs.srv import ChangeState from rcl_interfaces.msg import SetParametersResult @@ -89,27 +91,26 @@ def main(args=None): try: lc.configure_system() executor.spin_once(timeout_sec=1) - lc2.configure_system() - executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) + lc2.configure_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.activate_system() executor.spin_once(timeout_sec=1) - lc2.activate_system() - executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) + lc2.activate_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.change_mode('CC') executor.spin_once(timeout_sec=1) - lc2.change_mode('DD') executor.spin_once(timeout_sec=1) - + lc2.change_mode('DD') executor.spin() finally: executor.shutdown() diff --git a/system_modes/test/launchtest/two_lifecycle_nodes.py b/system_modes/test/launchtest/two_lifecycle_nodes.py index 9c14f89..99eb8cd 100644 --- a/system_modes/test/launchtest/two_lifecycle_nodes.py +++ b/system_modes/test/launchtest/two_lifecycle_nodes.py @@ -1,3 +1,5 @@ +from time import sleep + from lifecycle_msgs.srv import ChangeState from rcl_interfaces.msg import SetParametersResult @@ -85,19 +87,15 @@ def main(args=None): lc.configure_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.activate_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.change_mode('CC') - executor.spin() finally: executor.shutdown() diff --git a/system_modes/test/launchtest/two_mixed_nodes.py b/system_modes/test/launchtest/two_mixed_nodes.py index 66f8e70..6f83674 100644 --- a/system_modes/test/launchtest/two_mixed_nodes.py +++ b/system_modes/test/launchtest/two_mixed_nodes.py @@ -1,3 +1,5 @@ +from time import sleep + from lifecycle_msgs.srv import ChangeState from rcl_interfaces.msg import SetParametersResult @@ -103,19 +105,15 @@ def main(args=None): lc.configure_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.activate_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) - executor.spin_once(timeout_sec=1) + sleep(2) # give the system some time to converge lc.change_mode('CC') - executor.spin() finally: executor.shutdown()