From c1ba8bc140f00eca491bd2b27ada0759f8a894ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 31 Jan 2024 20:57:23 +0100 Subject: [PATCH] Fixed compilation and tests. --- controller_manager/CMakeLists.txt | 1 - .../controller_manager/controller_manager.hpp | 3 +- .../test/test_spawner_unspawner.cpp | 80 ------------------- .../test/test_resource_manager.hpp | 2 +- 4 files changed, 2 insertions(+), 84 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0d1b88a55a..376cef3362 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -150,7 +150,6 @@ if(BUILD_TESTING) ) target_link_libraries(test_controller_manager_urdf_passing controller_manager - test_controller ros2_control_test_assets::ros2_control_test_assets ) ament_target_dependencies(test_controller_manager_urdf_passing diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index c0a22fe5bd..f5ee6854a1 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -322,7 +322,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); - void init_controller_manager(); + void subscribe_to_robot_description_topic(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -546,7 +546,6 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; - rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; struct SwitchParams { diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 353f6738ab..1ecc3164b8 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -31,7 +31,6 @@ using ::testing::Return; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture { -public: void SetUp() override { ControllerManagerFixture::SetUp(); @@ -275,82 +274,3 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } - -class TestLoadControllerWithoutRobotDescription -: public ControllerManagerFixture -{ -public: - TestLoadControllerWithoutRobotDescription() - : ControllerManagerFixture("") - { - } - - 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(); } - - rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; - -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(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out) -{ - cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - - ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1) - << "could not spawn controller because not robot description and not services for controller " - "manager are active"; -} - -TEST_F( - TestLoadControllerWithoutRobotDescription, - controller_starting_with_later_load_of_robot_description) -{ - cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - - // Delay sending robot description - robot_description_sending_timer_ = cm_->create_wall_timer( - std::chrono::milliseconds(1500), [&]() { pass_robot_description_to_cm_and_rm(); }); - - ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1) - << "could not activate control because not robot description"; - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); - { - auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); - ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - } -} diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index 46a487f2ef..c9bd17490c 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -44,7 +44,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager public: friend ResourceManagerTest; - FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_and_manual_validation); FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle);