If the controller activation fails then release the claimed interfaces

ros-controls · Apr 29, 2024 · 77e932e · 77e932e
1 parent 17bb7a2
commit 77e932e
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -1508,10 +1508,13 @@ void ControllerManager::activate_controllers(
     {
       RCLCPP_ERROR(
         get_logger(),
-        "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
+        "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d). Releasing "
+        "interfaces!",
         controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
         hardware_interface::lifecycle_state_names::ACTIVE,
         lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      controller->release_interfaces();
+      return;
     }
 
     // if it is a chainable controller, make the reference interfaces available on activation