Fix the reloading controller with failed activation (#2544)

Author: saikishor

controller_manager/src/controller_manager.cpp

@@ -1216,6 +1216,8 @@ controller_interface::return_type ControllerManager::configure_controller(
       return controller_interface::return_type::ERROR;
     }
   }
+  // For cases, when the controller ends up in the unconfigured state from any other state
+  cleanup_controller_exported_interfaces(*found_it);
 
   try
   {
@@ -4403,7 +4405,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
 
 void ControllerManager::cleanup_controller_exported_interfaces(const ControllerSpec & controller)
 {
-  if (is_controller_inactive(controller.c) && controller.c->is_chainable())
+  if (!is_controller_active(controller.c) && controller.c->is_chainable())
   {
     RCLCPP_DEBUG(
       get_logger(), "Removing controller '%s' exported interfaces from resource manager.",

controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

@@ -172,7 +172,7 @@ CallbackReturn TestChainableController::on_activate(
     (*msg)->data = reference_interfaces_;
   }
 
-  return CallbackReturn::SUCCESS;
+  return fail_on_activate ? CallbackReturn::ERROR : CallbackReturn::SUCCESS;
 }
 
 CallbackReturn TestChainableController::on_cleanup(

controller_manager/test/test_chainable_controller/test_chainable_controller.hpp

@@ -80,6 +80,7 @@ class TestChainableController : public controller_interface::ChainableController
   std::vector<double> get_state_interface_data() const;
 
   size_t internal_counter;
+  bool fail_on_activate = false;
   controller_interface::InterfaceConfiguration cmd_iface_cfg_;
   controller_interface::InterfaceConfiguration state_iface_cfg_;
   std::vector<std::string> reference_interface_names_;

controller_manager/test/test_controller_manager.cpp

@@ -2169,3 +2169,102 @@ TEST_F(
     EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
   }
 }
+
+class TestControllerManagerChainableControllerFailedActivation
+: public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+};
+
+TEST_F(
+  TestControllerManagerChainableControllerFailedActivation,
+  test_chainable_controllers_failed_activation_and_then_reconfiguring_it)
+{
+  const std::string test_controller_name = "test_chainable_controller_2";
+
+  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+  controller_interface::InterfaceConfiguration itfs_cfg;
+  cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  // controller 4
+  auto test_chainable_controller =
+    std::make_shared<test_chainable_controller::TestChainableController>();
+  cmd_itfs_cfg.names = {"joint1/position"};
+  itfs_cfg.names = {"joint2/velocity"};
+  test_chainable_controller->set_command_interface_configuration(cmd_itfs_cfg);
+  test_chainable_controller->set_state_interface_configuration(itfs_cfg);
+  test_chainable_controller->set_reference_interface_names({"modified_joint1/position"});
+  test_chainable_controller->set_exported_state_interface_names({"modified_joint2/velocity"});
+  test_chainable_controller->fail_on_activate = true;
+
+  cm_->add_controller(
+    test_chainable_controller, test_chainable_controller::TEST_CONTROLLER_NAME,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_chainable_controller.use_count());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_chainable_controller->get_lifecycle_state().id());
+
+  // configure controllers
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME));
+  }
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_chainable_controller->get_lifecycle_state().id());
+
+  std::vector<std::string> start_controllers = {test_chainable_controller::TEST_CONTROLLER_NAME};
+  std::vector<std::string> stop_controllers = {};
+  {
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+
+    ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+    {
+      ControllerManagerRunner cm_runner(this);
+      EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
+    }
+  }
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_chainable_controller->get_lifecycle_state().id());
+  // Now, after reconfiguring it, it should work
+  test_chainable_controller->fail_on_activate = false;
+
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME));
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_chainable_controller->get_lifecycle_state().id());
+
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_chainable_controller->get_lifecycle_state().id());
+  }
+}

hardware_interface/src/resource_manager.cpp

@@ -1652,6 +1652,12 @@ void ResourceManager::make_controller_exported_state_interfaces_unavailable(
 void ResourceManager::remove_controller_exported_state_interfaces(
   const std::string & controller_name)
 {
+  if (
+    resource_storage_->controllers_exported_state_interfaces_map_.find(controller_name) ==
+    resource_storage_->controllers_exported_state_interfaces_map_.end())
+  {
+    return;
+  }
   auto interface_names =
     resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name);
   resource_storage_->controllers_exported_state_interfaces_map_.erase(controller_name);
@@ -1714,6 +1720,12 @@ void ResourceManager::make_controller_reference_interfaces_unavailable(
 // CM API: Called in "callback/slow"-thread
 void ResourceManager::remove_controller_reference_interfaces(const std::string & controller_name)
 {
+  if (
+    resource_storage_->controllers_reference_interfaces_map_.find(controller_name) ==
+    resource_storage_->controllers_reference_interfaces_map_.end())
+  {
+    return;
+  }
   auto interface_names =
     resource_storage_->controllers_reference_interfaces_map_.at(controller_name);
   resource_storage_->controllers_reference_interfaces_map_.erase(controller_name);