diff --git a/system_modes/CMakeLists.txt b/system_modes/CMakeLists.txt index d93f860..63b7ed5 100644 --- a/system_modes/CMakeLists.txt +++ b/system_modes/CMakeLists.txt @@ -185,7 +185,8 @@ if(BUILD_TESTING) "two_lifecycle_nodes" # Mode Manager with Lifecycle Nodes "two_mixed_nodes" # Mode Manager with Lifecycle and non-Lifecycle Nodes "two_independent_hierarchies" # Mode Manager for two independent hierarchies of nodes - "manager_and_monitor") # Mode Manager and Mode Monitor + "manager_and_monitor" # Mode Manager and Mode Monitor + "redundant_mode_changes") # Ignore redundant mode changes # Launch Test: Mode Manager with Lifecycle Nodes foreach(test_name ${launch_tests}) diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index 35dbfb2..cf5d76b 100644 --- a/system_modes/src/system_modes/mode_manager.cpp +++ b/system_modes/src/system_modes/mode_manager.cpp @@ -432,6 +432,21 @@ ModeManager::change_mode( return false; } + // Mode change redundant? + try { + auto current = mode_inference_->get_or_infer(node_name); + if (current.mode.compare(mode_name) == 0) { + RCLCPP_ERROR( + this->get_logger(), + "Redundant mode change of %s to %s - ignored.", + node_name.c_str(), + mode_name.c_str()); + return true; + } + } catch (...) { + // Okay, if we don't know anything about this part, yet + } + // Publish info about this request auto info = std::make_shared(); info->goal_mode.label = mode_name; diff --git a/system_modes/test/launchtest/manager_and_monitor.py b/system_modes/test/launchtest/manager_and_monitor.py index 8f08964..9c14f89 100644 --- a/system_modes/test/launchtest/manager_and_monitor.py +++ b/system_modes/test/launchtest/manager_and_monitor.py @@ -2,7 +2,7 @@ from rcl_interfaces.msg import SetParametersResult import rclpy -from rclpy.executors import MultiThreadedExecutor +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.parameter import Parameter @@ -72,7 +72,7 @@ def change_mode(self, mode): def main(args=None): rclpy.init(args=args) try: - executor = MultiThreadedExecutor() + executor = SingleThreadedExecutor() node_a = FakeLifecycleNode('A') node_b = FakeLifecycleNode('B') diff --git a/system_modes/test/launchtest/redundant_mode_changes.launch.py.in b/system_modes/test/launchtest/redundant_mode_changes.launch.py.in new file mode 100644 index 0000000..53cf8d9 --- /dev/null +++ b/system_modes/test/launchtest/redundant_mode_changes.launch.py.in @@ -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) diff --git a/system_modes/test/launchtest/redundant_mode_changes.py b/system_modes/test/launchtest/redundant_mode_changes.py new file mode 100644 index 0000000..f1e1f04 --- /dev/null +++ b/system_modes/test/launchtest/redundant_mode_changes.py @@ -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() diff --git a/system_modes/test/launchtest/redundant_mode_changes_expected_output.regex b/system_modes/test/launchtest/redundant_mode_changes_expected_output.regex new file mode 100644 index 0000000..3419036 --- /dev/null +++ b/system_modes/test/launchtest/redundant_mode_changes_expected_output.regex @@ -0,0 +1,4 @@ +Transition [AB]:configure +Transition [AB]:activate +Parameter callback #0 A:foo:0.2 +Parameter callback #1 A:foo:0.3 diff --git a/system_modes/test/launchtest/redundant_mode_changes_modes.yaml b/system_modes/test/launchtest/redundant_mode_changes_modes.yaml new file mode 100644 index 0000000..a768954 --- /dev/null +++ b/system_modes/test/launchtest/redundant_mode_changes_modes.yaml @@ -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 diff --git a/system_modes/test/launchtest/two_independent_hierarchies.py b/system_modes/test/launchtest/two_independent_hierarchies.py index a55e123..82ef6a3 100644 --- a/system_modes/test/launchtest/two_independent_hierarchies.py +++ b/system_modes/test/launchtest/two_independent_hierarchies.py @@ -2,7 +2,7 @@ from rcl_interfaces.msg import SetParametersResult import rclpy -from rclpy.executors import MultiThreadedExecutor +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.parameter import Parameter @@ -72,7 +72,7 @@ def change_mode(self, mode): def main(args=None): rclpy.init(args=args) try: - executor = MultiThreadedExecutor() + executor = SingleThreadedExecutor() node_a = FakeLifecycleNode('A') node_b = FakeLifecycleNode('B') node_c = FakeLifecycleNode('C') diff --git a/system_modes/test/launchtest/two_lifecycle_nodes.py b/system_modes/test/launchtest/two_lifecycle_nodes.py index 8f08964..9c14f89 100644 --- a/system_modes/test/launchtest/two_lifecycle_nodes.py +++ b/system_modes/test/launchtest/two_lifecycle_nodes.py @@ -2,7 +2,7 @@ from rcl_interfaces.msg import SetParametersResult import rclpy -from rclpy.executors import MultiThreadedExecutor +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.parameter import Parameter @@ -72,7 +72,7 @@ def change_mode(self, mode): def main(args=None): rclpy.init(args=args) try: - executor = MultiThreadedExecutor() + executor = SingleThreadedExecutor() node_a = FakeLifecycleNode('A') node_b = FakeLifecycleNode('B') diff --git a/system_modes/test/launchtest/two_mixed_nodes.py b/system_modes/test/launchtest/two_mixed_nodes.py index 8a74c81..66f8e70 100644 --- a/system_modes/test/launchtest/two_mixed_nodes.py +++ b/system_modes/test/launchtest/two_mixed_nodes.py @@ -2,7 +2,7 @@ from rcl_interfaces.msg import SetParametersResult import rclpy -from rclpy.executors import MultiThreadedExecutor +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.parameter import Parameter @@ -90,7 +90,7 @@ def change_mode(self, mode): def main(args=None): rclpy.init(args=args) try: - executor = MultiThreadedExecutor() + executor = SingleThreadedExecutor() node_a = FakeLifecycleNode('A') node_b = NormalNode('B')