Controller sorting and proper execution in a chain (Fixes ros-control…

…s#853) (ros-controls#1063)

* Added test for the reordering controllers case

* Perform controller sorting at the end of switch_controller

* fix the list_chained_controllers_srv test case for the new controller sorting

* move the logic to a separate function

* Apply suggestions from code review

Co-authored-by: Christoph Fröhlich <[email protected]>

* remove obsolete TODO comments in the controller chaining tests

* Add a test case to sort 6 controllers in chained mode

* Added a method to retrieve the following controller names given a controller name

* Update the controller_sorting method to support progressive chaining ability

* Added test case to sort chained controllers with branching

* Added a method to retrieve the preceding controller names given a controller name

* Added logic to controller_sorting to support sorting branched controller chains

* Added some documentation to the newly added functions

* Add debug print of reordered controllers list once they are sorted

* Add the condition to skip the non-configured controllers

* remove logging for every command interface

* Improve the complex chain test case checking

* added a test case to sort independent chains

* Added fixes for the independent chains sorting

* better randomization in independent chains testing

* Fix minor logic issues

* Add 3rd chain for better complex testing

* Apply suggestions from code review - Denis

Co-authored-by: Dr. Denis <[email protected]>

* address pull request review comments

---------

Co-authored-by: Christoph Fröhlich <[email protected]>
Co-authored-by: Dr. Denis <[email protected]>
Co-authored-by: Bence Magyar <[email protected]>

controller_manager/include/controller_manager/controller_manager.hpp

@@ -389,6 +389,29 @@ class ControllerManager : public rclcpp::Node
  const std::vector<ControllerSpec> & controllers, int strictness,
  const ControllersListIterator controller_it);
 
+ /// A method to be used in the std::sort method to sort the controllers to be able to
+ /// execute them in a proper order
+ /**
+ * Compares the controllers ctrl_a and ctrl_b and then returns which comes first in the sequence
+ *
+ * @note The following conditions needs to be handled while ordering the controller list
+ * 1. The controllers that do not use any state or command interfaces are updated first
+ * 2. The controllers that use only the state system interfaces only are updated next
+ * 3. The controllers that use any of an another controller's reference interface are updated
+ * before the preceding controller
+ * 4. The controllers that use the controller's estimated interfaces are updated after the
+ * preceding controller
+ * 5. The controllers that only use the hardware command interfaces are updated last
+ * 6. All inactive controllers go at the end of the list
+ *
+ * \param[in] controllers list of controllers to compare their names to interface's prefix.
+ *
+ * @return true, if ctrl_a needs to execute first, else false
+ */
+ bool controller_sorting(
+ const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
+ const std::vector<controller_manager::ControllerSpec> & controllers);
+
  void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
  diagnostic_updater::Updater diagnostics_updater_;
 

controller_manager/src/controller_manager.cpp

@@ -118,6 +118,123 @@ bool command_interface_is_reference_interface_of_controller(
  return true;
 }
 
+/**
+ * A method to retrieve the names of all it's following controllers given a controller name
+ * For instance, for the following case
+ * A -> B -> C -> D
+ * When called with B, returns C and D
+ * NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported from
+ * the controller B (or) the controller B is utilizing the expected interfaces exported from the
+ * controller A
+ *
+ * @param controller_name - Name of the controller for checking the tree
+ * \param[in] controllers list of controllers to compare their names to interface's prefix.
+ * @return list of controllers that are following the given controller in a chain. If none, return
+ * empty.
+ */
+std::vector<std::string> get_following_controller_names(
+ const std::string controller_name,
+ const std::vector<controller_manager::ControllerSpec> & controllers)
+{
+ std::vector<std::string> following_controllers;
+ auto controller_it = std::find_if(
+ controllers.begin(), controllers.end(),
+ std::bind(controller_name_compare, std::placeholders::_1, controller_name));
+ if (controller_it == controllers.end())
+ {
+ RCLCPP_DEBUG(
+ rclcpp::get_logger("ControllerManager::utils"),
+ "Required controller : '%s' is not found in the controller list ", controller_name.c_str());
+
+ return following_controllers;
+ }
+ // If the controller is not configured, return empty
+ if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c)))
+ return following_controllers;
+ const auto cmd_itfs = controller_it->c->command_interface_configuration().names;
+ for (const auto & itf : cmd_itfs)
+ {
+ controller_manager::ControllersListIterator ctrl_it;
+ if (command_interface_is_reference_interface_of_controller(itf, controllers, ctrl_it))
+ {
+ RCLCPP_DEBUG(
+ rclcpp::get_logger("ControllerManager::utils"),
+ "The interface is a reference interface of controller : %s", ctrl_it->info.name.c_str());
+ following_controllers.push_back(ctrl_it->info.name);
+ const std::vector<std::string> ctrl_names =
+ get_following_controller_names(ctrl_it->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;
+}
+
+/**
+ * A method to retrieve the names of all it's preceding controllers given a controller name
+ * For instance, for the following case
+ * A -> B -> C -> D
+ * When called with C, returns A and B
+ * NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported from
+ * the controller B (or) the controller B is utilizing the expected interfaces exported from the
+ * controller A
+ *
+ * @param controller_name - Name of the controller for checking the tree
+ * \param[in] controllers list of controllers to compare their names to interface's prefix.
+ * @return list of controllers that are preceding the given controller in a chain. If none, return
+ * empty.
+ */
+std::vector<std::string> get_preceding_controller_names(
+ const std::string controller_name,
+ const std::vector<controller_manager::ControllerSpec> & controllers)
+{
+ std::vector<std::string> preceding_controllers;
+ auto controller_it = std::find_if(
+ controllers.begin(), controllers.end(),
+ std::bind(controller_name_compare, std::placeholders::_1, controller_name));
+ if (controller_it == controllers.end())
+ {
+ RCLCPP_DEBUG(
+ rclcpp::get_logger("ControllerManager::utils"),
+ "Required controller : '%s' is not found in the controller list ", controller_name.c_str());
+ 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;
+ auto cmd_itfs = ctrl.c->command_interface_configuration().names;
+ for (const auto & itf : cmd_itfs)
+ {
+ if (itf.find(controller_name) != std::string::npos)
+ {
+ preceding_controllers.push_back(ctrl.info.name);
+ auto ctrl_names = get_preceding_controller_names(ctrl.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
 
 namespace controller_manager
@@ -1116,6 +1233,20 @@ controller_interface::return_type ControllerManager::switch_controller(
  controller.info.claimed_interfaces.clear();
  }
  }
+
+ // Reordering the controllers
+ std::sort(
+ to.begin(), to.end(),
+ std::bind(
+ &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2,
+ to));
+
+ RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:");
+ for (const auto & ctrl : to)
+ {
+ RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str());
+ }
+
  // switch lists
  rt_controllers_wrapper_.switch_updated_list(guard);
  // clear unused list
@@ -2280,6 +2411,99 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
  }
  }
  return controller_interface::return_type::OK;
+}
+
+bool ControllerManager::controller_sorting(
+ const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
+ const std::vector<controller_manager::ControllerSpec> & controllers)
+{
+ // If the controllers are not active, then should be at the end of the list
+ if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c))
+ {
+ if (is_controller_active(ctrl_a.c)) return true;
+ return false;
+ }
+
+ 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())
+ {
+ // The case of the controllers that don't have any command interfaces. For instance,
+ // joint_state_broadcaster
+ return true;
+ }
+ else
+ {
+ auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers);
+ if (following_ctrls.empty()) return false;
+ // If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b
+ if (
+ std::find(following_ctrls.begin(), following_ctrls.end(), ctrl_b.info.name) !=
+ following_ctrls.end())
+ return true;
+ else
+ {
+ auto ctrl_a_preceding_ctrls = get_preceding_controller_names(ctrl_a.info.name, controllers);
+ // This is to check that the ctrl_b is in the preceding controllers list of ctrl_a - This
+ // check is useful when there is a chained controller branching, but they belong to same
+ // branch
+ if (
+ std::find(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), ctrl_b.info.name) !=
+ ctrl_a_preceding_ctrls.end())
+ {
+ return false;
+ }
+
+ // This is to handle the cases where, the parsed ctrl_a and ctrl_b are not directly related
+ // but might have a common parent - happens in branched chained controller
+ auto ctrl_b_preceding_ctrls = get_preceding_controller_names(ctrl_b.info.name, controllers);
+ std::sort(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end());
+ std::sort(ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end());
+ std::list<std::string> intersection;
+ std::set_intersection(
+ ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(),
+ ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end(),
+ std::back_inserter(intersection));
+ if (!intersection.empty())
+ {
+ // If there is an intersection, then there is a common parent controller for both ctrl_a and
+ // ctrl_b
+ return true;
+ }
+
+ // If there is no common parent, then they belong to 2 different sets
+ auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers);
+ if (following_ctrls_b.empty()) return true;
+ auto find_first_element = [&](const auto & controllers_list)
+ {
+ auto it = std::find_if(
+ controllers.begin(), controllers.end(),
+ std::bind(controller_name_compare, std::placeholders::_1, controllers_list.back()));
+ if (it != controllers.end())
+ {
+ int dist = std::distance(controllers.begin(), it);
+ return dist;
+ }
+ };
+ const int ctrl_a_chain_first_controller = find_first_element(following_ctrls);
+ const int ctrl_b_chain_first_controller = find_first_element(following_ctrls_b);
+ if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) return true;
+ }
+
+ // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be
+ // infront of ctrl_a
+ // TODO(saikishor): deal with the state interface chaining in the sorting algorithm
+ auto state_it = std::find_if(
+ state_itfs.begin(), state_itfs.end(),
+ [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
+ if (state_it != state_itfs.end())
+ {
+ return false;
+ }
+
+ // The rest of the cases, basically end up at the end of the list
+ return false;
+ }
 };
 
 void ControllerManager::controller_activity_diagnostic_callback(