Skip to content

Commit 69826ea

Browse files
norronorro
committed
Launch test for monitor
Launches two nodes and mode manager, tests mode monitor output when changing modes in the system. Signed-off-by: Nordmann Arne (CR/ADT3) <arne.nordmann@de.bosch.com>
1 parent 7db381a commit 69826ea

5 files changed

Lines changed: 247 additions & 1 deletion

File tree

‎system_modes/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -174,7 +174,8 @@ if(BUILD_TESTING)
174174
set(launch_tests
175175
"two_lifecycle_nodes" # Mode Manager with Lifecycle Nodes
176176
"two_mixed_nodes" # Mode Manager with Lifecycle and non-Lifecycle Nodes
177-
"two_independent_hierarchies") # Mode Manager for two independent hierarchies of nodes
177+
"two_independent_hierarchies" # Mode Manager for two independent hierarchies of nodes
178+
"manager_and_monitor") # Mode Manager and Mode Monitor
178179

179180
# Launch Test: Mode Manager with Lifecycle Nodes
180181
foreach(test_name ${launch_tests})

‎system_modes/test/launchtest/manager_and_monitor.launch.py.in‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
import os
2+
3+
import unittest
4+
5+
import ament_index_python
6+
import launch
7+
import launch_ros
8+
import launch_testing.actions
9+
import launch_testing_ros
10+
11+
from launch import LaunchDescription
12+
from launch.actions import ExecuteProcess
13+
14+
15+
def generate_test_description():
16+
os.environ['OSPL_VERBOSITY'] = '8'
17+
os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}'
18+
19+
modelfile = '@MODELFILE@'
20+
21+
mode_manager = launch.actions.IncludeLaunchDescription(
22+
launch.launch_description_sources.PythonLaunchDescriptionSource(
23+
ament_index_python.packages.get_package_share_directory(
24+
'system_modes') + '/launch/mode_manager.launch.py'),
25+
launch_arguments={'modelfile': modelfile}.items())
26+
27+
test_nodes = ExecuteProcess(
28+
cmd=[
29+
"@PYTHON_EXECUTABLE@",
30+
"@TEST_NODES@"
31+
],
32+
name='test_nodes',
33+
emulate_tty=True)
34+
35+
mode_monitor = ExecuteProcess(
36+
cmd=[
37+
"ros2",
38+
"run",
39+
"system_modes",
40+
"mode_monitor",
41+
"--ros-args",
42+
"-p",
43+
"modelfile:=" + modelfile,
44+
"-p",
45+
"debug:=True",
46+
"-p",
47+
"verbose:=True",
48+
"-p",
49+
"rate:=200"],
50+
name='mode_monitor',
51+
emulate_tty=True,
52+
output='screen')
53+
54+
launch_description = LaunchDescription()
55+
launch_description.add_action(mode_manager)
56+
launch_description.add_action(mode_monitor)
57+
launch_description.add_action(test_nodes)
58+
launch_description.add_action(launch_testing.actions.ReadyToTest())
59+
60+
return launch_description, locals()
61+
62+
class TestModeManagement(unittest.TestCase):
63+
64+
def test_processes_output(self, proc_output, mode_monitor):
65+
"""Check manager and nodes logging output for expected strings."""
66+
67+
from launch_testing.tools.output import get_default_filtered_prefixes
68+
output_filter = launch_testing_ros.tools.basic_output_filter(
69+
filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'],
70+
filtered_rmw_implementation='@RMW_IMPLEMENTATION@'
71+
)
72+
proc_output.assertWaitFor(
73+
expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"),
74+
process=mode_monitor,
75+
output_filter=output_filter,
76+
timeout=15,
77+
stream='stdout')
78+
79+
import time
80+
time.sleep(1)

‎system_modes/test/launchtest/manager_and_monitor.py‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
from lifecycle_msgs.srv import ChangeState
2+
from rcl_interfaces.msg import SetParametersResult
3+
4+
import rclpy
5+
from rclpy.executors import MultiThreadedExecutor
6+
from rclpy.node import Node
7+
from rclpy.parameter import Parameter
8+
9+
from system_modes.srv import ChangeMode
10+
11+
12+
class FakeLifecycleNode(Node):
13+
14+
def __init__(self, name):
15+
super().__init__(name)
16+
17+
self.declare_parameter('foo')
18+
self.declare_parameter('bar')
19+
self.add_on_set_parameters_callback(self.parameter_callback)
20+
21+
# State change service
22+
self.srv = self.create_service(
23+
ChangeState,
24+
self.get_name() + '/change_state',
25+
self.change_state_callback)
26+
27+
def parameter_callback(self, params):
28+
for p in params:
29+
if p.name == 'bar' and p.type_ == Parameter.Type.STRING:
30+
self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value))
31+
if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE:
32+
self.get_logger().info('Parameter %s:%s:%s' % (self.get_name(), p.name, p.value))
33+
return SetParametersResult(successful=True)
34+
35+
def change_state_callback(self, request, response):
36+
response.success = True
37+
self.get_logger().info('Transition %s:%s' % (self.get_name(), request.transition.label))
38+
39+
return response
40+
41+
42+
class LifecycleClient(Node):
43+
44+
def __init__(self):
45+
super().__init__('system_modes_test_client')
46+
47+
self.clis = self.create_client(ChangeState, '/sys/change_state')
48+
while not self.clis.wait_for_service(timeout_sec=1.0):
49+
self.get_logger().info('service not available, waiting again...')
50+
self.reqs = ChangeState.Request()
51+
52+
self.clim = self.create_client(ChangeMode, '/sys/change_mode')
53+
while not self.clim.wait_for_service(timeout_sec=1.0):
54+
self.get_logger().info('service not available, waiting again...')
55+
self.reqm = ChangeMode.Request()
56+
57+
def configure_system(self):
58+
self.reqs.transition.id = 1
59+
self.reqs.transition.label = 'configure'
60+
self.future = self.clis.call_async(self.reqs)
61+
62+
def activate_system(self):
63+
self.reqs.transition.id = 3
64+
self.reqs.transition.label = 'activate'
65+
self.future = self.clis.call_async(self.reqs)
66+
67+
def change_mode(self, mode):
68+
self.reqm.mode_name = mode
69+
self.future = self.clim.call_async(self.reqm)
70+
71+
72+
def main(args=None):
73+
rclpy.init(args=args)
74+
try:
75+
executor = MultiThreadedExecutor()
76+
node_a = FakeLifecycleNode('A')
77+
node_b = FakeLifecycleNode('B')
78+
79+
executor.add_node(node_a)
80+
executor.add_node(node_b)
81+
82+
lc = LifecycleClient()
83+
84+
try:
85+
lc.configure_system()
86+
executor.spin_once(timeout_sec=1)
87+
executor.spin_once(timeout_sec=1)
88+
executor.spin_once(timeout_sec=1)
89+
executor.spin_once(timeout_sec=1)
90+
91+
lc.activate_system()
92+
executor.spin_once(timeout_sec=1)
93+
executor.spin_once(timeout_sec=1)
94+
executor.spin_once(timeout_sec=1)
95+
executor.spin_once(timeout_sec=1)
96+
97+
lc.change_mode('CC')
98+
99+
executor.spin()
100+
finally:
101+
executor.shutdown()
102+
node_a.destroy_node()
103+
node_b.destroy_node()
104+
finally:
105+
rclpy.shutdown()
106+
return 0
107+
108+
109+
if __name__ == '__main__':
110+
main()

‎system_modes/test/launchtest/manager_and_monitor_expected_output.regex‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
System.Modes.Monitor
2+
active
3+
CC\(\*\)
4+
BB\(\*\).*BB<foo:0.200000>
5+
FF\(\*\).*<bar:THREE, foo:0.900000>

‎system_modes/test/launchtest/manager_and_monitor_modes.yaml‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
# system modes example
2+
---
3+
4+
sys:
5+
ros__parameters:
6+
type: system
7+
parts:
8+
A
9+
B
10+
modes:
11+
__DEFAULT__:
12+
A: inactive
13+
B: active
14+
DD:
15+
A: active.AA
16+
B: active.EE
17+
CC:
18+
A: active.BB
19+
B: active.FF
20+
21+
A:
22+
ros__parameters:
23+
type: node
24+
modes:
25+
__DEFAULT__:
26+
ros__parameters:
27+
foo: 0.1
28+
AA:
29+
ros__parameters:
30+
foo: 0.1
31+
BB:
32+
ros__parameters:
33+
foo: 0.2
34+
35+
B:
36+
ros__parameters:
37+
type: node
38+
modes:
39+
__DEFAULT__:
40+
ros__parameters:
41+
foo: 0.1
42+
bar: ONE
43+
EE:
44+
ros__parameters:
45+
foo: 0.2
46+
bar: TWO
47+
FF:
48+
ros__parameters:
49+
foo: 0.9
50+
bar: THREE

0 commit comments

Comments
 (0)