Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 4 additions & 6 deletions system_modes/test/launchtest/manager_and_monitor.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
from time import sleep

from lifecycle_msgs.srv import ChangeState
from rcl_interfaces.msg import SetParametersResult

Expand Down Expand Up @@ -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()
Expand Down
18 changes: 6 additions & 12 deletions system_modes/test/launchtest/modes_observer.launch.py.in
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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)
Expand Down
12 changes: 2 additions & 10 deletions system_modes/test/launchtest/modes_observer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
22 changes: 10 additions & 12 deletions system_modes/test/launchtest/redundant_mode_changes.py
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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()
Expand Down
Original file line number Diff line number Diff line change
@@ -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
13 changes: 7 additions & 6 deletions system_modes/test/launchtest/two_independent_hierarchies.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
from time import sleep

from lifecycle_msgs.srv import ChangeState
from rcl_interfaces.msg import SetParametersResult

Expand Down Expand Up @@ -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()
Expand Down
10 changes: 4 additions & 6 deletions system_modes/test/launchtest/two_lifecycle_nodes.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
from time import sleep

from lifecycle_msgs.srv import ChangeState
from rcl_interfaces.msg import SetParametersResult

Expand Down Expand Up @@ -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()
Expand Down
10 changes: 4 additions & 6 deletions system_modes/test/launchtest/two_mixed_nodes.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
from time import sleep

from lifecycle_msgs.srv import ChangeState
from rcl_interfaces.msg import SetParametersResult

Expand Down Expand Up @@ -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()
Expand Down