update controller_sorting logic to deal with chainable controller tha…

…t export states

controller_manager/src/controller_manager.cpp

@@ -224,6 +224,30 @@ std::vector<std::string> get_following_controller_names(
  }
  }
  }
+ for (const auto & ctrl : controllers)
+ {
+ // If the controller is not configured, then continue
+ if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) continue;
+ auto cmd_itfs = ctrl.c->state_interface_configuration().names;
+ for (const auto & itf : cmd_itfs)
+ {
+ if (itf.find(controller_name) != std::string::npos)
+ {
+ following_controllers.push_back(ctrl.info.name);
+ auto ctrl_names = get_following_controller_names(ctrl.info.name, controllers);
+ for (const std::string & controller : ctrl_names)
+ {
+ if (
+ std::find(following_controllers.begin(), following_controllers.end(), controller) ==
+ following_controllers.end())
+ {
+ // Only add to the list if it doesn't exist
+ following_controllers.push_back(controller);
+ }
+ }
+ }
+ }
+ }
  return following_controllers;
 }
 
@@ -256,11 +280,13 @@ std::vector<std::string> get_preceding_controller_names(
  "Required controller : '%s' is not found in the controller list ", controller_name.c_str());
  return preceding_controllers;
  }
+ // If the controller is not configured, return empty
+ if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c)))
+ return preceding_controllers;
  for (const auto & ctrl : controllers)
  {
- // If the controller is not configured, return empty
- if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c)))
- return preceding_controllers;
+ // If the controller is not configured, then continue
+ if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) continue;
  auto cmd_itfs = ctrl.c->command_interface_configuration().names;
  for (const auto & itf : cmd_itfs)
  {
@@ -281,6 +307,31 @@ std::vector<std::string> get_preceding_controller_names(
  }
  }
  }
+ const auto cmd_itfs = controller_it->c->state_interface_configuration().names;
+ for (const auto & itf : cmd_itfs)
+ {
+ controller_manager::ControllersListIterator ctrl_it;
+ if (is_interface_a_chained_interface(itf, controllers, ctrl_it))
+ {
+ RCLCPP_DEBUG(
+ rclcpp::get_logger("ControllerManager::utils"),
+ "The interface is a internal state interface of controller : %s",
+ ctrl_it->info.name.c_str());
+ preceding_controllers.push_back(ctrl_it->info.name);
+ const std::vector<std::string> ctrl_names =
+ get_preceding_controller_names(ctrl_it->info.name, controllers);
+ for (const std::string & controller : ctrl_names)
+ {
+ if (
+ std::find(preceding_controllers.begin(), preceding_controllers.end(), controller) ==
+ preceding_controllers.end())
+ {
+ // Only add to the list if it doesn't exist
+ preceding_controllers.push_back(controller);
+ }
+ }
+ }
+ }
  return preceding_controllers;
 }
 } // namespace
@@ -2563,10 +2614,11 @@ bool ControllerManager::controller_sorting(
 
  const std::vector<std::string> cmd_itfs = ctrl_a.c->command_interface_configuration().names;
  const std::vector<std::string> state_itfs = ctrl_a.c->state_interface_configuration().names;
- if (cmd_itfs.empty() || !ctrl_a.c->is_chainable())
+ if (cmd_itfs.empty() && !ctrl_a.c->is_chainable())
  {
  // The case of the controllers that don't have any command interfaces. For instance,
- // joint_state_broadcaster
+ // joint_state_broadcaster. It is also needed to make sure that it is not chainable, because the
+ // controllers that export their internal state are handled later
  return true;
  }
  else