From 8a23238ff3676d6368d97e10df74852071760bb1 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Mon, 14 Oct 2024 11:59:35 +0200
Subject: [PATCH] Added a fail test case when the fallback controllers need non
 existing state interface to activate

---
 .../test/test_controller_manager.cpp          | 89 +++++++++++++++++++
 1 file changed, 89 insertions(+)

diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index c94fcde89a..9eb381d946 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -739,3 +739,92 @@ TEST_F(
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
     test_controller_2->get_lifecycle_state().id());
 }
+
+TEST_F(
+  TestControllerManagerFallbackControllers,
+  test_fallback_controllers_failed_activation_on_missing_state_interface)
+{
+  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+  cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  cmd_itfs_cfg.names = {"joint1/position"};
+  auto test_controller_1 = std::make_shared<test_controller::TestController>();
+  test_controller_1->set_command_interface_configuration(cmd_itfs_cfg);
+  auto test_controller_2 = std::make_shared<test_controller::TestController>();
+  controller_interface::InterfaceConfiguration state_itfs_cfg;
+  state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  state_itfs_cfg.names = {"non_existing_state_interface/position"};
+  test_controller_2->set_command_interface_configuration(cmd_itfs_cfg);
+  test_controller_2->set_state_interface_configuration(state_itfs_cfg);
+  const std::string test_controller_1_name = "test_controller_1";
+  const std::string test_controller_2_name = "test_controller_2";
+
+  {
+    controller_manager::ControllerSpec controller_spec;
+    controller_spec.c = test_controller_1;
+    controller_spec.info.name = test_controller_1_name;
+    controller_spec.info.type = "test_controller::TestController";
+    controller_spec.info.fallback_controllers_names = {test_controller_2_name};
+    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    ControllerManagerRunner cm_runner(this);
+    cm_->add_controller(controller_spec);  // add controller_1
+
+    controller_spec.c = test_controller_2;
+    controller_spec.info.name = test_controller_2_name;
+    controller_spec.info.type = "test_controller::TestController";
+    controller_spec.info.fallback_controllers_names = {};
+    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    cm_->add_controller(controller_spec);  // add controller_2
+  }
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller_1.use_count());
+  EXPECT_EQ(2, test_controller_2.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_controller_1->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller_2->get_lifecycle_state().id());
+
+  // configure controllers
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller_1_name);
+    cm_->configure_controller(test_controller_2_name);
+  }
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller_1->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller_2->get_lifecycle_state().id());
+
+  // Start controller, will take effect at the end of the update function
+  std::vector<std::string> start_controllers = {test_controller_1_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::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,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+  }
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller_1->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller_2->get_lifecycle_state().id());
+}