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

@@ -461,3 +461,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();
+ }
+}