Added test to test with the namespaced controller manager

controller_manager/test/controller_manager_test_common.hpp

@@ -61,14 +61,15 @@ class ControllerManagerFixture : public ::testing::Test
 {
 public:
   explicit ControllerManagerFixture(
-    const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
+    const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
+    const std::string & cm_namespace = "")
   : robot_description_(robot_description)
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     cm_ = std::make_shared<CtrlMgr>(
       std::make_unique<hardware_interface::ResourceManager>(
         rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
-      executor_, TEST_CM_NAME);
+      executor_, TEST_CM_NAME, cm_namespace);
     // We want to be able to not pass robot description immediately
     if (!robot_description_.empty())
     {

controller_manager/test/test_spawner_unspawner.cpp

@@ -445,3 +445,140 @@ TEST_F(
     ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   }
 }
+
+class TestLoadControllerWithNamespacedCM
+: public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+public:
+  TestLoadControllerWithNamespacedCM()
+  : ControllerManagerFixture<controller_manager::ControllerManager>(
+      ros2_control_test_assets::minimal_robot_urdf, "foo_namespace")
+  {
+  }
+
+  void SetUp() override
+  {
+    ControllerManagerFixture::SetUp();
+
+    update_timer_ = cm_->create_wall_timer(
+      std::chrono::milliseconds(10),
+      [&]()
+      {
+        cm_->read(time_, PERIOD);
+        cm_->update(time_, PERIOD);
+        cm_->write(time_, PERIOD);
+      });
+
+    update_executor_ =
+      std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
+
+    update_executor_->add_node(cm_);
+    update_executor_spin_future_ =
+      std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+
+    // This sleep is needed to prevent a too fast test from ending before the
+    // executor has began to spin, which causes it to hang
+    std::this_thread::sleep_for(50ms);
+  }
+
+  void TearDown() override { update_executor_->cancel(); }
+
+protected:
+  rclcpp::TimerBase::SharedPtr update_timer_;
+
+  // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+  std::shared_ptr<rclcpp::Executor> update_executor_;
+  std::future<void> update_executor_spin_future_;
+};
+
+TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param)
+{
+  cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager -n foo_namespace"), 0);
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 256)
+    << "Should fail without defining the namespace";
+
+  auto validate_loaded_controllers = [&]()
+  {
+    auto loaded_controllers = cm_->get_loaded_controllers();
+    for (size_t i = 0; i < loaded_controllers.size(); i++)
+    {
+      auto ctrl = loaded_controllers[i];
+      const std::string controller_name = "ctrl_" + std::to_string(i + 1);
+
+      RCLCPP_ERROR(rclcpp::get_logger("test_controller_manager"), "%s", controller_name.c_str());
+      auto it = std::find_if(
+        loaded_controllers.begin(), loaded_controllers.end(),
+        [&](const auto & controller) { return controller.info.name == controller_name; });
+      ASSERT_TRUE(it != loaded_controllers.end());
+      ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+      ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+    }
+  };
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+  {
+    validate_loaded_controllers();
+  }
+
+  // Try to spawn again multiple but one of them is already active, it should fail because already
+  // active
+  EXPECT_NE(call_spawner("ctrl_1 ctrl_3 -c /foo_namespace/test_controller_manager"), 0)
+    << "Cannot configure from active";
+
+  std::vector<std::string> start_controllers = {};
+  std::vector<std::string> stop_controllers = {"ctrl_1"};
+  cm_->switch_controller(
+    start_controllers, stop_controllers,
+    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
+
+  // We should be able to reconfigure and activate a configured controller
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+  {
+    validate_loaded_controllers();
+  }
+
+  stop_controllers = {"ctrl_1", "ctrl_2"};
+  cm_->switch_controller(
+    start_controllers, stop_controllers,
+    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
+  for (auto ctrl : stop_controllers)
+  {
+    cm_->unload_controller(ctrl);
+    cm_->load_controller(ctrl);
+  }
+
+  // We should be able to reconfigure and activate am unconfigured loaded controller
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager -n foo_namespace"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+  {
+    validate_loaded_controllers();
+  }
+
+  // Unload and reload
+  EXPECT_EQ(call_unspawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been unloaded";
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager -n foo_namespace"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded";
+  {
+    validate_loaded_controllers();
+  }
+
+  // Now unload everything and load them as a group in a single operation
+  EXPECT_EQ(call_unspawner("ctrl_1 ctrl_2 ctrl_3 -c /foo_namespace/test_controller_manager"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded";
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group -n foo_namespace"),
+    0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded";
+  {
+    validate_loaded_controllers();
+  }
+}