From 485b25fc40a09c35938ff6b012d9ae8c5f0fcaca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 13 Dec 2023 16:41:50 +0100 Subject: [PATCH] Make reference interfaces of chainable controllers available in inactive state. --- controller_manager/src/controller_manager.cpp | 11 +++++++++++ ...llers_chaining_with_controller_manager.cpp | 19 +++++++------------ hardware_interface/src/resource_manager.cpp | 4 ++-- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2e703f4219..51f49331f7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -643,6 +643,11 @@ controller_interface::return_type ControllerManager::unload_controller( RCLCPP_DEBUG(get_logger(), "Cleanup controller"); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? + // Controller will not be removed so make its reference interface available + if (controller.c->is_chainable()) + { + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } controller.c->get_node()->cleanup(); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -799,6 +804,12 @@ controller_interface::return_type ControllerManager::configure_controller( // clear unused list rt_controllers_wrapper_.get_unused_list(guard).clear(); + // Controller will not be in INACTIVE state so make its reference interface available + if (controller->is_chainable()) + { + resource_manager_->make_controller_reference_interfaces_available(controller_name); + } + return controller_interface::return_type::OK; } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 0ebddd85e0..74533c2835 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -211,7 +211,7 @@ class TestControllerChainingWithControllerManager for (const auto & interface : {"pid_left_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -223,7 +223,7 @@ class TestControllerChainingWithControllerManager for (const auto & interface : {"pid_right_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -236,7 +236,7 @@ class TestControllerChainingWithControllerManager "diff_drive_controller/rot_z"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -250,7 +250,7 @@ class TestControllerChainingWithControllerManager "diff_drive_controller/rot_z"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -290,6 +290,7 @@ class TestControllerChainingWithControllerManager size_t expected_internal_counter, const controller_interface::return_type expected_return, bool deactivated, bool claimed_interfaces_from_hw = false) { + (void)claimed_interfaces_from_hw; for (const auto & interface : claimed_command_itfs) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); @@ -297,18 +298,12 @@ class TestControllerChainingWithControllerManager if ((expected_return == controller_interface::return_type::OK) != deactivated) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface)); } else { - if (claimed_interfaces_from_hw) - { - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); - } - else - { - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); - } + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 542d4eff1f..8fe5cb3841 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1108,7 +1108,7 @@ bool ResourceManager::prepare_command_mode_switch( if (!command_interface_exists(interface)) { all_exist = false; - ss_not_existing << " " << interface << std::endl; + ss_not_existing << " " << interface << std::endl; } } return all_exist; @@ -1133,7 +1133,7 @@ bool ResourceManager::prepare_command_mode_switch( if (!command_interface_is_available(interface)) { all_available = false; - ss_not_available << " " << interface << std::endl; + ss_not_available << " " << interface << std::endl; } } return all_available;