fix. No component -> ret value is ok

ros-controls · Feb 15, 2024 · b21c84c · b21c84c
1 parent ffdfadb
commit b21c84c
Showing 1 changed file with 12 additions and 8 deletions.
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -1154,7 +1154,8 @@ bool ResourceManager::prepare_command_mode_switch(
   auto call_prepare_mode_switch =
     [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components)
   {
-    bool ret = false;
+    size_t count = 0;
+    bool ret = true;
     for (auto & component : components)
     {
       if (
@@ -1165,24 +1166,25 @@ bool ResourceManager::prepare_command_mode_switch(
           return_type::OK ==
           component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
         {
-          ret = true;
+          ++count;
         }
         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;
         }
       }
     }
-    return ret;
+    return ret || (count > 0);
   };
 
   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
@@ -1198,7 +1200,8 @@ bool ResourceManager::perform_command_mode_switch(
 
   auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces](auto & components)
   {
-    bool ret = false;
+    size_t count = 0;
+    bool ret = true;
     for (auto & component : components)
     {
       if (
@@ -1209,23 +1212,24 @@ bool ResourceManager::perform_command_mode_switch(
           return_type::OK ==
           component.perform_command_mode_switch(start_interfaces, stop_interfaces))
         {
-          ret = true;
+          ++count;
         }
         else
         {
           RCUTILS_LOG_ERROR_NAMED(
             "resource_manager", "Component '%s' could not perform switch",
             component.get_name().c_str());
+          ret = false;
         }
       }
     }
-    return ret;
+    return ret || (count > 0);
   };
 
   const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_);
   const bool systems_result = call_perform_mode_switch(resource_storage_->systems_);
 
-  return actuators_result || systems_result;
+  return actuators_result && systems_result;
 }
 
 // CM API: Called in "callback/slow"-thread