-
Notifications
You must be signed in to change notification settings - Fork 9
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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>
- Loading branch information
Showing
10 changed files
with
297 additions
and
9 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
75 changes: 75 additions & 0 deletions
75
system_modes/test/launchtest/redundant_mode_changes.launch.py.in
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,75 @@ | ||
import os | ||
|
||
import unittest | ||
|
||
import ament_index_python | ||
import launch | ||
import launch_ros | ||
import launch_testing | ||
import launch_testing.actions | ||
import launch_testing.asserts | ||
import launch_testing.util | ||
import launch_testing_ros | ||
|
||
from launch import LaunchDescription | ||
from launch.actions import ExecuteProcess | ||
from launch.events import matches_action | ||
from launch.events.process import ShutdownProcess | ||
|
||
import lifecycle_msgs.msg | ||
|
||
|
||
def generate_test_description(): | ||
os.environ['OSPL_VERBOSITY'] = '8' | ||
os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' | ||
|
||
modelfile = '@MODELFILE@' | ||
|
||
mode_manager = launch.actions.IncludeLaunchDescription( | ||
launch.launch_description_sources.PythonLaunchDescriptionSource( | ||
ament_index_python.packages.get_package_share_directory( | ||
'system_modes') + '/launch/mode_manager.launch.py'), | ||
launch_arguments={'modelfile': modelfile}.items()) | ||
|
||
test_nodes = ExecuteProcess( | ||
cmd=[ | ||
"@PYTHON_EXECUTABLE@", | ||
"@TEST_NODES@" | ||
], | ||
name='test_two_lifecycle_nodes', | ||
emulate_tty=True, | ||
output='screen') | ||
|
||
launch_description = LaunchDescription() | ||
launch_description.add_action(mode_manager) | ||
launch_description.add_action(test_nodes) | ||
launch_description.add_action(launch_testing.util.KeepAliveProc()) | ||
launch_description.add_action(launch_testing.actions.ReadyToTest()) | ||
|
||
return launch_description, locals() | ||
|
||
class TestModeManagement(unittest.TestCase): | ||
|
||
def test_processes_output(self, proc_output, test_nodes): | ||
"""Check manager and nodes logging output for expected strings.""" | ||
|
||
from launch_testing.tools.output import get_default_filtered_prefixes | ||
output_filter = launch_testing_ros.tools.basic_output_filter( | ||
filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], | ||
filtered_rmw_implementation='@rmw_implementation@' | ||
) | ||
proc_output.assertWaitFor( | ||
expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), | ||
process=test_nodes, | ||
output_filter=output_filter, | ||
timeout=15 | ||
) | ||
|
||
import time | ||
time.sleep(1) | ||
|
||
@launch_testing.post_shutdown_test() | ||
class TestModeManagementShutdown(unittest.TestCase): | ||
|
||
def test_last_process_exit_code(self, proc_info, test_nodes): | ||
launch_testing.asserts.assertExitCodes(proc_info, process=test_nodes) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,143 @@ | ||
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 | ||
|
||
from system_modes.srv import ChangeMode | ||
|
||
|
||
class FakeLifecycleNode(Node): | ||
num_param_callbacks = 0 | ||
|
||
def __init__(self, name): | ||
super().__init__(name) | ||
|
||
self.declare_parameter('foo') | ||
self.declare_parameter('bar') | ||
self.add_on_set_parameters_callback(self.parameter_callback) | ||
|
||
# State change service | ||
self.srv = self.create_service( | ||
ChangeState, | ||
self.get_name() + '/change_state', | ||
self.change_state_callback) | ||
|
||
def parameter_callback(self, params): | ||
for p in params: | ||
if p.name == 'bar' and p.type_ == Parameter.Type.STRING: | ||
self.get_logger().info('Parameter callback #%d %s:%s:%s' | ||
% (self.num_param_callbacks, | ||
self.get_name(), | ||
p.name, p.value)) | ||
if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE: | ||
self.get_logger().info('Parameter callback #%d %s:%s:%s' | ||
% (self.num_param_callbacks, | ||
self.get_name(), | ||
p.name, p.value)) | ||
self.num_param_callbacks = self.num_param_callbacks + 1 | ||
return SetParametersResult(successful=True) | ||
|
||
def change_state_callback(self, request, response): | ||
response.success = True | ||
self.get_logger().info('Transition %s:%s' % (self.get_name(), request.transition.label)) | ||
|
||
return response | ||
|
||
|
||
class LifecycleClient(Node): | ||
|
||
def __init__(self): | ||
super().__init__('system_modes_test_client') | ||
|
||
self.clis = self.create_client(ChangeState, '/sys/change_state') | ||
while not self.clis.wait_for_service(timeout_sec=1.0): | ||
self.get_logger().info('service not available, waiting again...') | ||
self.reqs = ChangeState.Request() | ||
|
||
self.clim = self.create_client(ChangeMode, '/sys/change_mode') | ||
while not self.clim.wait_for_service(timeout_sec=1.0): | ||
self.get_logger().info('service not available, waiting again...') | ||
self.reqm = ChangeMode.Request() | ||
|
||
self.climn = self.create_client(ChangeMode, '/A/change_mode') | ||
while not self.clim.wait_for_service(timeout_sec=1.0): | ||
self.get_logger().info('service not available, waiting again...') | ||
self.reqmn = ChangeMode.Request() | ||
|
||
def configure_system(self): | ||
self.reqs.transition.id = 1 | ||
self.reqs.transition.label = 'configure' | ||
self.future = self.clis.call_async(self.reqs) | ||
|
||
def activate_system(self): | ||
self.reqs.transition.id = 3 | ||
self.reqs.transition.label = 'activate' | ||
self.future = self.clis.call_async(self.reqs) | ||
|
||
def change_mode(self, mode): | ||
self.reqm.mode_name = mode | ||
self.future = self.clim.call_async(self.reqm) | ||
|
||
def change_A_mode(self, mode): | ||
self.reqmn.mode_name = mode | ||
self.future = self.climn.call_async(self.reqmn) | ||
|
||
|
||
def main(args=None): | ||
rclpy.init(args=args) | ||
try: | ||
executor = MultiThreadedExecutor() | ||
node_a = FakeLifecycleNode('A') | ||
node_b = FakeLifecycleNode('B') | ||
|
||
executor.add_node(node_a) | ||
executor.add_node(node_b) | ||
|
||
lc = LifecycleClient() | ||
|
||
try: | ||
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) | ||
|
||
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) | ||
|
||
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) | ||
executor.spin_once(timeout_sec=1) | ||
executor.spin_once(timeout_sec=1) | ||
|
||
executor.spin() | ||
finally: | ||
executor.shutdown() | ||
node_a.destroy_node() | ||
node_b.destroy_node() | ||
finally: | ||
rclpy.shutdown() | ||
return 0 | ||
|
||
|
||
if __name__ == '__main__': | ||
main() |
4 changes: 4 additions & 0 deletions
4
system_modes/test/launchtest/redundant_mode_changes_expected_output.regex
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,4 @@ | ||
Transition [AB]:configure | ||
Transition [AB]:activate | ||
Parameter callback #0 A:foo:0.2 | ||
Parameter callback #1 A:foo:0.3 |
50 changes: 50 additions & 0 deletions
50
system_modes/test/launchtest/redundant_mode_changes_modes.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,50 @@ | ||
# system modes example | ||
--- | ||
|
||
sys: | ||
ros__parameters: | ||
type: system | ||
parts: | ||
A | ||
B | ||
modes: | ||
__DEFAULT__: | ||
A: inactive | ||
B: active | ||
DD: | ||
A: active.AA | ||
B: active.EE | ||
CC: | ||
A: active.BB | ||
B: active.FF | ||
|
||
A: | ||
ros__parameters: | ||
type: node | ||
modes: | ||
__DEFAULT__: | ||
ros__parameters: | ||
foo: 0.1 | ||
AA: | ||
ros__parameters: | ||
foo: 0.2 | ||
BB: | ||
ros__parameters: | ||
foo: 0.3 | ||
|
||
B: | ||
ros__parameters: | ||
type: node | ||
modes: | ||
__DEFAULT__: | ||
ros__parameters: | ||
foo: 0.1 | ||
bar: ONE | ||
EE: | ||
ros__parameters: | ||
foo: 0.2 | ||
bar: TWO | ||
FF: | ||
ros__parameters: | ||
foo: 0.9 | ||
bar: THREE |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters