fix command mode switch logic

ros-controls · Feb 13, 2024 · f1d7387 · f1d7387
1 parent 3e0a023
commit f1d7387
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -1154,7 +1154,7 @@ bool ResourceManager::prepare_command_mode_switch(
   auto call_prepare_mode_switch =
     [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components)
   {
-    bool ret = true;
+    bool ret = false;
     for (auto & component : components)
     {
       if (
@@ -1164,12 +1164,15 @@ bool ResourceManager::prepare_command_mode_switch(
         if (
           return_type::OK !=
           component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+        {
+          ret = true;
+        }
+        else
         {
           RCUTILS_LOG_ERROR_NAMED(
             "resource_manager",
             "Component '%s' did not accept command interfaces combination: \n%s",
             component.get_name().c_str(), interfaces_to_string().c_str());
-          ret = false;
         }
       }
     }
@@ -1179,7 +1182,7 @@ bool ResourceManager::prepare_command_mode_switch(
   const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_);
   const bool systems_result = call_prepare_mode_switch(resource_storage_->systems_);
 
-  return actuators_result && systems_result;
+  return actuators_result || systems_result;
 }
 
 // CM API: Called in "update"-thread