Mode manager prevents redundant mode changes

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})

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<ModeEvent>();
   info->goal_mode.label = mode_name;

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')
 

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)

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()

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

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

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')

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')
 

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')