Deactivate the controllers when they return error similar to the hard…

controller_manager/doc/userdoc.rst

@@ -228,3 +228,9 @@ Restarting hardware
 If hardware gets restarted then you should go through its lifecycle again.
 This can be simply achieved by returning ``ERROR`` from ``write`` and ``read`` methods of interface implementation.
 **NOT IMPLEMENTED YET - PLEASE STOP/RESTART ALL CONTROLLERS MANUALLY FOR NOW** The controller manager detects that and stops all the controllers that are commanding that hardware and restarts broadcasters that are listening to its states.
+
+Hardware and Controller Errors
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces.
+Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available.

controller_manager/src/controller_manager.cpp

@@ -2023,6 +2023,7 @@ controller_interface::return_type ControllerManager::update(
  ++update_loop_counter_;
  update_loop_counter_ %= update_rate_;
 
+ std::vector<std::string> failed_controllers_list;
  for (const auto & loaded_controller : rt_controller_list)
  {
  // TODO(v-lopez) we could cache this information
@@ -2061,11 +2062,25 @@ controller_interface::return_type ControllerManager::update(
 
  if (controller_ret != controller_interface::return_type::OK)
  {
+ failed_controllers_list.push_back(loaded_controller.info.name);
  ret = controller_ret;
  }
  }
  }
  }
+ if (!failed_controllers_list.empty())
+ {
+ std::string failed_controllers;
+ for (const auto & controller : failed_controllers_list)
+ {
+ failed_controllers += controller + " ";
+ }
+ RCLCPP_ERROR(
+ get_logger(), "Deactivating controllers: %s, as their update resulted in an error",
+ failed_controllers.c_str());
+
+ deactivate_controllers(rt_controller_list, failed_controllers_list);
+ }
 
  // there are controllers to (de)activate
  if (switch_params_.do_switch)