diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 72326e8c6c..d5a60e6aac 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/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(); cm_ = std::make_shared( std::make_unique( 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()) { diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a7c2432959..23bbacff14 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/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 +{ +public: + TestLoadControllerWithNamespacedCM() + : ControllerManagerFixture( + 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::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 update_executor_; + std::future 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 start_controllers = {}; + std::vector 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(); + } +}