diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e8ccc7b1f..f00e58d14b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1154,6 +1154,7 @@ bool ResourceManager::prepare_command_mode_switch( auto call_prepare_mode_switch = [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components) { + size_t count = 0; bool ret = true; for (auto & component : components) { @@ -1162,8 +1163,12 @@ bool ResourceManager::prepare_command_mode_switch( component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { if ( - return_type::OK != + return_type::OK == component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + { + ++count; + } + else { RCUTILS_LOG_ERROR_NAMED( "resource_manager", @@ -1173,7 +1178,7 @@ bool ResourceManager::prepare_command_mode_switch( } } } - return ret; + return ret || (count > 0); }; const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_); @@ -1195,6 +1200,7 @@ bool ResourceManager::perform_command_mode_switch( auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces](auto & components) { + size_t count = 0; bool ret = true; for (auto & component : components) { @@ -1203,8 +1209,12 @@ bool ResourceManager::perform_command_mode_switch( component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { if ( - return_type::OK != + return_type::OK == component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + { + ++count; + } + else { RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Component '%s' could not perform switch", @@ -1213,7 +1223,7 @@ bool ResourceManager::perform_command_mode_switch( } } } - return ret; + return ret || (count > 0); }; const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_);