Skip to content

Commit 8ef3b27

Browse files
norronorro
authored
Mode manager prevents redundant mode changes (#67)
Before executing a requested mode change, i.e. applying the according changes, it is now checking whether the mode is already active. If so, no action is taken, but mode change service request is still returned with success. * Mode manager prevents redundant mode changes * Testing redundant mode change #60 Signed-off-by: Nordmann Arne (CR/ADT3) <arne.nordmann@de.bosch.com>
1 parent 02b4b3e commit 8ef3b27

10 files changed

Lines changed: 297 additions & 9 deletions

‎system_modes/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -185,7 +185,8 @@ if(BUILD_TESTING)
185185
"two_lifecycle_nodes" # Mode Manager with Lifecycle Nodes
186186
"two_mixed_nodes" # Mode Manager with Lifecycle and non-Lifecycle Nodes
187187
"two_independent_hierarchies" # Mode Manager for two independent hierarchies of nodes
188-
"manager_and_monitor") # Mode Manager and Mode Monitor
188+
"manager_and_monitor" # Mode Manager and Mode Monitor
189+
"redundant_mode_changes") # Ignore redundant mode changes
189190

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

‎system_modes/src/system_modes/mode_manager.cpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -432,6 +432,21 @@ ModeManager::change_mode(
432432
return false;
433433
}
434434

435+
// Mode change redundant?
436+
try {
437+
auto current = mode_inference_->get_or_infer(node_name);
438+
if (current.mode.compare(mode_name) == 0) {
439+
RCLCPP_ERROR(
440+
this->get_logger(),
441+
"Redundant mode change of %s to %s - ignored.",
442+
node_name.c_str(),
443+
mode_name.c_str());
444+
return true;
445+
}
446+
} catch (...) {
447+
// Okay, if we don't know anything about this part, yet
448+
}
449+
435450
// Publish info about this request
436451
auto info = std::make_shared<ModeEvent>();
437452
info->goal_mode.label = mode_name;

‎system_modes/test/launchtest/manager_and_monitor.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
from rcl_interfaces.msg import SetParametersResult
33

44
import rclpy
5-
from rclpy.executors import MultiThreadedExecutor
5+
from rclpy.executors import SingleThreadedExecutor
66
from rclpy.node import Node
77
from rclpy.parameter import Parameter
88

@@ -72,7 +72,7 @@ def change_mode(self, mode):
7272
def main(args=None):
7373
rclpy.init(args=args)
7474
try:
75-
executor = MultiThreadedExecutor()
75+
executor = SingleThreadedExecutor()
7676
node_a = FakeLifecycleNode('A')
7777
node_b = FakeLifecycleNode('B')
7878

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

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

‎system_modes/test/launchtest/redundant_mode_changes.py‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
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+
num_param_callbacks = 0
14+
15+
def __init__(self, name):
16+
super().__init__(name)
17+
18+
self.declare_parameter('foo')
19+
self.declare_parameter('bar')
20+
self.add_on_set_parameters_callback(self.parameter_callback)
21+
22+
# State change service
23+
self.srv = self.create_service(
24+
ChangeState,
25+
self.get_name() + '/change_state',
26+
self.change_state_callback)
27+
28+
def parameter_callback(self, params):
29+
for p in params:
30+
if p.name == 'bar' and p.type_ == Parameter.Type.STRING:
31+
self.get_logger().info('Parameter callback #%d %s:%s:%s'
32+
% (self.num_param_callbacks,
33+
self.get_name(),
34+
p.name, p.value))
35+
if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE:
36+
self.get_logger().info('Parameter callback #%d %s:%s:%s'
37+
% (self.num_param_callbacks,
38+
self.get_name(),
39+
p.name, p.value))
40+
self.num_param_callbacks = self.num_param_callbacks + 1
41+
return SetParametersResult(successful=True)
42+
43+
def change_state_callback(self, request, response):
44+
response.success = True
45+
self.get_logger().info('Transition %s:%s' % (self.get_name(), request.transition.label))
46+
47+
return response
48+
49+
50+
class LifecycleClient(Node):
51+
52+
def __init__(self):
53+
super().__init__('system_modes_test_client')
54+
55+
self.clis = self.create_client(ChangeState, '/sys/change_state')
56+
while not self.clis.wait_for_service(timeout_sec=1.0):
57+
self.get_logger().info('service not available, waiting again...')
58+
self.reqs = ChangeState.Request()
59+
60+
self.clim = self.create_client(ChangeMode, '/sys/change_mode')
61+
while not self.clim.wait_for_service(timeout_sec=1.0):
62+
self.get_logger().info('service not available, waiting again...')
63+
self.reqm = ChangeMode.Request()
64+
65+
self.climn = self.create_client(ChangeMode, '/A/change_mode')
66+
while not self.clim.wait_for_service(timeout_sec=1.0):
67+
self.get_logger().info('service not available, waiting again...')
68+
self.reqmn = ChangeMode.Request()
69+
70+
def configure_system(self):
71+
self.reqs.transition.id = 1
72+
self.reqs.transition.label = 'configure'
73+
self.future = self.clis.call_async(self.reqs)
74+
75+
def activate_system(self):
76+
self.reqs.transition.id = 3
77+
self.reqs.transition.label = 'activate'
78+
self.future = self.clis.call_async(self.reqs)
79+
80+
def change_mode(self, mode):
81+
self.reqm.mode_name = mode
82+
self.future = self.clim.call_async(self.reqm)
83+
84+
def change_A_mode(self, mode):
85+
self.reqmn.mode_name = mode
86+
self.future = self.climn.call_async(self.reqmn)
87+
88+
89+
def main(args=None):
90+
rclpy.init(args=args)
91+
try:
92+
executor = MultiThreadedExecutor()
93+
node_a = FakeLifecycleNode('A')
94+
node_b = FakeLifecycleNode('B')
95+
96+
executor.add_node(node_a)
97+
executor.add_node(node_b)
98+
99+
lc = LifecycleClient()
100+
101+
try:
102+
lc.configure_system()
103+
executor.spin_once(timeout_sec=1)
104+
executor.spin_once(timeout_sec=1)
105+
executor.spin_once(timeout_sec=1)
106+
executor.spin_once(timeout_sec=1)
107+
108+
lc.activate_system()
109+
executor.spin_once(timeout_sec=1)
110+
executor.spin_once(timeout_sec=1)
111+
executor.spin_once(timeout_sec=1)
112+
executor.spin_once(timeout_sec=1)
113+
executor.spin_once(timeout_sec=1)
114+
executor.spin_once(timeout_sec=1)
115+
116+
lc.change_A_mode('AA')
117+
executor.spin_once(timeout_sec=1)
118+
executor.spin_once(timeout_sec=1)
119+
executor.spin_once(timeout_sec=1)
120+
executor.spin_once(timeout_sec=1)
121+
lc.change_A_mode('AA') # redundant, should be ignored
122+
executor.spin_once(timeout_sec=1)
123+
executor.spin_once(timeout_sec=1)
124+
executor.spin_once(timeout_sec=1)
125+
executor.spin_once(timeout_sec=1)
126+
lc.change_A_mode('BB')
127+
executor.spin_once(timeout_sec=1)
128+
executor.spin_once(timeout_sec=1)
129+
executor.spin_once(timeout_sec=1)
130+
executor.spin_once(timeout_sec=1)
131+
132+
executor.spin()
133+
finally:
134+
executor.shutdown()
135+
node_a.destroy_node()
136+
node_b.destroy_node()
137+
finally:
138+
rclpy.shutdown()
139+
return 0
140+
141+
142+
if __name__ == '__main__':
143+
main()

‎system_modes/test/launchtest/redundant_mode_changes_expected_output.regex‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
Transition [AB]:configure
2+
Transition [AB]:activate
3+
Parameter callback #0 A:foo:0.2
4+
Parameter callback #1 A:foo:0.3

‎system_modes/test/launchtest/redundant_mode_changes_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.2
31+
BB:
32+
ros__parameters:
33+
foo: 0.3
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

‎system_modes/test/launchtest/two_independent_hierarchies.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
from rcl_interfaces.msg import SetParametersResult
33

44
import rclpy
5-
from rclpy.executors import MultiThreadedExecutor
5+
from rclpy.executors import SingleThreadedExecutor
66
from rclpy.node import Node
77
from rclpy.parameter import Parameter
88

@@ -72,7 +72,7 @@ def change_mode(self, mode):
7272
def main(args=None):
7373
rclpy.init(args=args)
7474
try:
75-
executor = MultiThreadedExecutor()
75+
executor = SingleThreadedExecutor()
7676
node_a = FakeLifecycleNode('A')
7777
node_b = FakeLifecycleNode('B')
7878
node_c = FakeLifecycleNode('C')

‎system_modes/test/launchtest/two_lifecycle_nodes.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
from rcl_interfaces.msg import SetParametersResult
33

44
import rclpy
5-
from rclpy.executors import MultiThreadedExecutor
5+
from rclpy.executors import SingleThreadedExecutor
66
from rclpy.node import Node
77
from rclpy.parameter import Parameter
88

@@ -72,7 +72,7 @@ def change_mode(self, mode):
7272
def main(args=None):
7373
rclpy.init(args=args)
7474
try:
75-
executor = MultiThreadedExecutor()
75+
executor = SingleThreadedExecutor()
7676
node_a = FakeLifecycleNode('A')
7777
node_b = FakeLifecycleNode('B')
7878

‎system_modes/test/launchtest/two_mixed_nodes.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
from rcl_interfaces.msg import SetParametersResult
33

44
import rclpy
5-
from rclpy.executors import MultiThreadedExecutor
5+
from rclpy.executors import SingleThreadedExecutor
66
from rclpy.node import Node
77
from rclpy.parameter import Parameter
88

@@ -90,7 +90,7 @@ def change_mode(self, mode):
9090
def main(args=None):
9191
rclpy.init(args=args)
9292
try:
93-
executor = MultiThreadedExecutor()
93+
executor = SingleThreadedExecutor()
9494
node_a = FakeLifecycleNode('A')
9595
node_b = NormalNode('B')
9696

0 commit comments

Comments
 (0)