Sort controllers while configuring instead of while activating (#1107)

controller_manager/src/controller_manager.cpp

@@ -148,9 +148,6 @@ std::vector<std::string> get_following_controller_names(
 
     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)
   {
@@ -209,13 +206,11 @@ std::vector<std::string> get_preceding_controller_names(
   }
   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)
+      auto split_pos = itf.find_first_of('/');
+      if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name))
       {
         preceding_controllers.push_back(ctrl.info.name);
         auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers);
@@ -768,10 +763,35 @@ controller_interface::return_type ControllerManager::configure_controller(
       return controller_interface::return_type::ERROR;
     }
     resource_manager_->import_controller_reference_interfaces(controller_name, interfaces);
+  }
+
+  // Now let's reorder the controllers
+  // lock controllers
+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);
+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);
+
+  // Copy all controllers from the 'from' list to the 'to' list
+  to = from;
+
+  // Reordering the controllers
+  std::sort(
+    to.begin(), to.end(),
+    std::bind(
+      &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2,
+      to));
 
-    // TODO(destogl): check and resort controllers in the vector
+  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
+  rt_controllers_wrapper_.get_unused_list(guard).clear();
+
   return controller_interface::return_type::OK;
 }
 
@@ -1233,19 +1253,6 @@ controller_interface::return_type ControllerManager::switch_controller(
     }
   }
 
-  // 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
@@ -2435,13 +2442,6 @@ 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())
@@ -2513,7 +2513,11 @@ bool ControllerManager::controller_sorting(
     // 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); });
+      [ctrl_b](auto itf)
+      {
+        auto index = itf.find_first_of('/');
+        return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name));
+      });
     if (state_it != state_itfs.end())
     {
       return false;

controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

@@ -32,33 +32,13 @@ TestChainableController::TestChainableController()
 controller_interface::InterfaceConfiguration
 TestChainableController::command_interface_configuration() const
 {
-  if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
-  {
-    return cmd_iface_cfg_;
-  }
-  else
-  {
-    throw std::runtime_error(
-      "Can not get command interface configuration until the controller is configured.");
-  }
+  return cmd_iface_cfg_;
 }
 
 controller_interface::InterfaceConfiguration
 TestChainableController::state_interface_configuration() const
 {
-  if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
-  {
-    return state_iface_cfg_;
-  }
-  else
-  {
-    throw std::runtime_error(
-      "Can not get state interface configuration until the controller is configured.");
-  }
+  return state_iface_cfg_;
 }
 
 controller_interface::return_type TestChainableController::update_reference_from_subscribers(

controller_manager/test/test_controller/test_controller.cpp

@@ -31,32 +31,12 @@ TestController::TestController()
 
 controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
 {
-  if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
-  {
-    return cmd_iface_cfg_;
-  }
-  else
-  {
-    throw std::runtime_error(
-      "Can not get command interface configuration until the controller is configured.");
-  }
+  return cmd_iface_cfg_;
 }
 
 controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
 {
-  if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
-  {
-    return state_iface_cfg_;
-  }
-  else
-  {
-    throw std::runtime_error(
-      "Can not get state interface configuration until the controller is configured.");
-  }
+  return state_iface_cfg_;
 }
 
 controller_interface::return_type TestController::update(

controller_manager/test/test_controller_manager.cpp

@@ -104,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
 
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
-  cm_->configure_controller(TEST_CONTROLLER2_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+    cm_->configure_controller(TEST_CONTROLLER2_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
 
   test_controller->get_node()->set_parameter({"update_rate", 4});
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
   rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(ctrl_update_rate));
   test_controller->get_node()->set_parameter(update_rate_parameter);
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -389,7 +398,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
 
   test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));

controller_manager/test/test_controller_manager_srvs.cpp

@@ -261,27 +261,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
   // get controller list after configure
   result = call_service_and_wait(*client, request, srv_executor);
   ASSERT_EQ(2u, result->controller.size());
+  // At this stage, the controllers are already reordered
   // check chainable controller
   ASSERT_EQ(result->controller[0].state, "inactive");
   ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u);
-  ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u);
+  ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u);
   ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u);
-  ASSERT_EQ(result->controller[0].is_chainable, true);
+  ASSERT_EQ(result->controller[0].is_chainable, false);
   ASSERT_EQ(result->controller[0].is_chained, false);
-  ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u);
-  ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]);
-  ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]);
+  ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u);
+  ASSERT_EQ(result->controller[0].chain_connections.size(), 1u);
 
-  ASSERT_EQ(result->controller[0].chain_connections.size(), 0u);
   // check test controller
   ASSERT_EQ(result->controller[1].state, "inactive");
   ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u);
-  ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u);
+  ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u);
   ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u);
-  ASSERT_EQ(result->controller[1].is_chainable, false);
+  ASSERT_EQ(result->controller[1].is_chainable, true);
   ASSERT_EQ(result->controller[1].is_chained, false);
-  ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u);
-  ASSERT_EQ(result->controller[1].chain_connections.size(), 1u);
+  ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u);
+  ASSERT_EQ(result->controller[1].chain_connections.size(), 0u);
+  ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]);
+  ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]);
   // activate controllers
   cm_->switch_controller(
     {test_chainable_controller::TEST_CONTROLLER_NAME}, {},
@@ -603,22 +604,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)
   result = call_service_and_wait(*client, request, srv_executor);
   ASSERT_EQ(6u, result->controller.size());
 
-  // activate controllers
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_1}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
-     TEST_CHAINED_CONTROLLER_4},
-    {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
-    rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {test_controller::TEST_CONTROLLER_NAME}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  // get controller list after activate
-  result = call_service_and_wait(*client, request, srv_executor);
-
-  ASSERT_EQ(6u, result->controller.size());
   // reordered controllers
   ASSERT_EQ(result->controller[0].name, "test_controller_name");
   ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5);
@@ -776,25 +761,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers)
   result = call_service_and_wait(*client, request, srv_executor);
   ASSERT_EQ(8u, result->controller.size());
 
-  // activate controllers
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_1}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_3}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
-     TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_7},
-    {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
-    rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {test_controller::TEST_CONTROLLER_NAME}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  // get controller list after activate
-  result = call_service_and_wait(*client, request, srv_executor);
-
-  ASSERT_EQ(8u, result->controller.size());
   // reordered controllers
   ASSERT_EQ(result->controller[0].name, "test_controller_name");
   ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7);
@@ -1009,29 +975,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers)
   result = call_service_and_wait(*client, request, srv_executor);
   ASSERT_EQ(10u, result->controller.size());
 
-  // activate controllers
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_1}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_4}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_7}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
-     TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_8},
-    {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
-    rclcpp::Duration(0, 0));
-  cm_->switch_controller(
-    {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {},
-    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  // get controller list after activate
-  result = call_service_and_wait(*client, request, srv_executor);
-
-  ASSERT_EQ(10u, result->controller.size());
-
   auto get_ctrl_pos = [result](const std::string & controller_name) -> int
   {
     auto it = std::find_if(

controller_manager/test/test_load_controller.cpp

@@ -162,7 +162,10 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller)
       lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());
 
     // Activate configured controller
-    cm_->configure_controller(controller_name1);
+    {
+      ControllerManagerRunner cm_runner(this);
+      cm_->configure_controller(controller_name1);
+    }
     start_test_controller(test_param.strictness);
     ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id());
   }
@@ -246,7 +249,10 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure
   test_controller->cleanup_calls = &cleanup_calls;
   // Configure from inactive state
   test_controller->simulate_cleanup_failure = false;
-  EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK);
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK);
+  }
   ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id());
   EXPECT_EQ(1u, cleanup_calls);
 }
@@ -421,7 +427,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers)
   ASSERT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id());
 
-  cm_->configure_controller(controller_name2);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(controller_name2);
+  }
 
   // Stop controller 1
   RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1");