From f16bb0114c01050501be2e889dccb389a33051ce Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 21 Apr 2023 18:18:24 +0200 Subject: [PATCH 01/61] Add the first implementation to export the controller state interfaces from a chainable controller interface --- .../chainable_controller_interface.hpp | 19 +++- .../controller_interface.hpp | 8 ++ .../controller_interface_base.hpp | 9 ++ .../src/chainable_controller_interface.cpp | 39 +++++++ .../src/controller_interface.cpp | 5 + .../test_chainable_controller_interface.hpp | 13 +++ controller_manager/src/controller_manager.cpp | 13 ++- .../test_chainable_controller.cpp | 6 ++ .../test_chainable_controller.hpp | 3 + .../hardware_interface/resource_manager.hpp | 52 +++++++++ hardware_interface/src/resource_manager.cpp | 102 +++++++++++++++++- 11 files changed, 263 insertions(+), 6 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2bdccefdc5..d1e287c22e 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -55,6 +55,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase CONTROLLER_INTERFACE_PUBLIC bool is_chainable() const final; + CONTROLLER_INTERFACE_PUBLIC + std::vector export_state_interfaces() final; + CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; @@ -65,7 +68,18 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_in_chained_mode() const final; protected: - /// Virtual method that each chainable controller should implement to export its chainable + /// Virtual method that each chainable controller should implement to export its read-only chainable + /// interfaces. + /** + * Each chainable controller implements this methods where all its state(read only) interfaces are + * exported. The method has the same meaning as `export_state_interfaces` method from + * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. + * + * \returns list of StateInterfaces that other controller can use as their outputs. + */ + virtual std::vector on_export_state_interfaces() = 0; + + /// Virtual method that each chainable controller should implement to export its read/write chainable /// interfaces. /** * Each chainable controller implements this methods where all input (command) interfaces are @@ -114,6 +128,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /// Storage of values for state interfaces + std::vector state_interfaces_storage_; + /// Storage of values for reference interfaces std::vector reference_interfaces_; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index a3d006755f..b39c025679 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -42,6 +42,14 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase CONTROLLER_INTERFACE_PUBLIC bool is_chainable() const final; + /** + * Controller has no state interfaces. + * + * \returns empty list. + */ + CONTROLLER_INTERFACE_PUBLIC + std::vector export_state_interfaces() final; + /** * Controller has no reference interfaces. * diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2de57d3f83..2f20c3f166 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -223,6 +223,15 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual std::vector export_reference_interfaces() = 0; + /** + * Export interfaces for a chainable controller that can be used as state interface by other + * controllers. + * + * \returns list of state interfaces for preceding controllers. + */ + CONTROLLER_INTERFACE_PUBLIC + virtual std::vector export_state_interfaces() = 0; + /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 2f7c67741e..7e577999c8 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,6 +44,45 @@ return_type ChainableControllerInterface::update( return ret; } +std::vector +ChainableControllerInterface::export_state_interfaces() +{ + auto state_interfaces = on_export_state_interfaces(); + // check if the "state_interfaces_storage_" variable is resized to number of interfaces + if (state_interfaces_storage_.size() != state_interfaces.size()) + { + // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the + // framework + RCLCPP_FATAL( + get_node()->get_logger(), + "The internal storage for state values 'state_interfaces_storage_' variable has size '%zu', " + "but it is expected to have the size '%zu' equal to the number of exported reference " + "interfaces. No reference interface will be exported. Please correct and recompile " + "the controller with name '%s' and try again.", + state_interfaces_storage_.size(), state_interfaces.size(), get_node()->get_name()); + state_interfaces.clear(); + } + + // check if the names of the state interfaces begin with the controller's name + for (const auto & interface : state_interfaces) + { + if (interface.get_prefix_name() != get_node()->get_name()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "The name of the interface '%s' does not begin with the controller's name. This is " + "mandatory " + " for state interfaces. No state interface will be exported. Please correct and " + "recompile the controller with name '%s' and try again.", + interface.get_name().c_str(), get_node()->get_name()); + state_interfaces.clear(); + break; + } + } + + return state_interfaces; +} + std::vector ChainableControllerInterface::export_reference_interfaces() { diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 392a48ffa4..0f11bba71c 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,6 +28,11 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } +std::vector ControllerInterface::export_state_interfaces() +{ + return {}; +} + std::vector ControllerInterface::export_reference_interfaces() { return {}; diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index 28401f13d5..c4875266d3 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -41,6 +41,8 @@ class TestableChainableControllerInterface { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); + state_interfaces_storage_.reserve(1); + state_interfaces_storage_.push_back(0.0); } controller_interface::CallbackReturn on_init() override @@ -63,6 +65,17 @@ class TestableChainableControllerInterface controller_interface::interface_configuration_type::NONE}; } + // Implementation of ChainableController virtual methods + std::vector on_export_state_interfaces() override + { + std::vector state_interfaces; + + state_interfaces.push_back(hardware_interface::StateInterface( + name_prefix_of_reference_interfaces_, "test_state", &state_interfaces_storage_[0])); + + return state_interfaces; + } + // Implementation of ChainableController virtual methods std::vector on_export_reference_interfaces() override { diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c85b9d3556..99632c69b1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -685,16 +685,21 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto interfaces = controller->export_reference_interfaces(); - if (interfaces.empty()) + auto state_interfaces = controller->export_state_interfaces(); + auto ref_interfaces = controller->export_reference_interfaces(); + if (ref_interfaces.empty() && state_interfaces.empty()) { // TODO(destogl): Add test for this! RCLCPP_ERROR( - get_logger(), "Controller '%s' is chainable, but does not export any reference interfaces.", + get_logger(), + "Controller '%s' is chainable, but does not export any state or reference interfaces.", controller_name.c_str()); return controller_interface::return_type::ERROR; } - resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + if (!ref_interfaces.empty()) + resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); + if (!state_interfaces.empty()) + resource_manager_->import_controller_state_interfaces(controller_name, state_interfaces); } // let's update the list of following and preceding controllers diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index d21957a0b4..738d043d06 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -150,6 +150,12 @@ CallbackReturn TestChainableController::on_cleanup( return CallbackReturn::SUCCESS; } +std::vector +TestChainableController::on_export_state_interfaces() +{ + return {}; +} + std::vector TestChainableController::on_export_reference_interfaces() { diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 5925ed8d11..9843f7cf87 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -60,6 +60,9 @@ class TestChainableController : public controller_interface::ChainableController CONTROLLER_MANAGER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC + std::vector on_export_state_interfaces() override; + CONTROLLER_MANAGER_PUBLIC std::vector on_export_reference_interfaces() override; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 743e548a4c..36f4489d30 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -134,6 +134,58 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + /// Add controllers' state interfaces to resource manager. + /** + * Interface for transferring management of state interfaces to resource manager. + * When chaining controllers, state interfaces are used by the preceding + * controllers. + * Therefore, they should be managed in the same way as state interface of hardware. + * + * \param[in] controller_name name of the controller which state interfaces are imported. + * \param[in] interfaces list of controller's state interfaces as StateInterfaces. + */ + void import_controller_state_interfaces( + const std::string & controller_name, std::vector & interfaces); + + /// Get list of state interface of a controller. + /** + * Returns lists of stored state interfaces names for a controller. + * + * \param[in] controller_name for which list of state interface names is returned. + * \returns list of reference interface names. + */ + std::vector get_controller_state_interface_names( + const std::string & controller_name); + + /// Add controller's state interfaces to available list. + /** + * Adds state interfacess of a controller with given name to the available list. This method should + * be called when a controller gets activated with chained mode turned on. That means, the + * controller's state interfaces can be used by another controllers in chained architectures. + * + * \param[in] controller_name name of the controller which interfaces should become available. + */ + void make_controller_state_interfaces_available(const std::string & controller_name); + + /// Remove controller's state interface to available list. + /** + * Removes interfaces of a controller with given name from the available list. This method should + * be called when a controller gets deactivated and its reference interfaces cannot be used by + * another controller anymore. + * + * \param[in] controller_name name of the controller which interfaces should become unavailable. + */ + void make_controller_state_interfaces_unavailable(const std::string & controller_name); + + /// Remove controllers state interfaces from resource manager. + /** + * Remove state interfaces from resource manager, i.e., resource storage. + * The interfaces will be deleted from all internal maps and lists. + * + * \param[in] controller_name list of interface names that will be deleted from resource manager. + */ + void remove_controller_state_interfaces(const std::string & controller_name); + /// Add controllers' reference interfaces to resource manager. /** * Interface for transferring management of reference interfaces to resource manager. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e8ccc7b1f..1f197d9ef4 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -454,6 +454,47 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + /// Adds exported state interfaces into internal storage. + /** + * Adds state interfaces to the internal storage. State interfaces exported from hardware or + * chainable controllers are moved to the map with name-interface pairs and available list's + * size is increased to reserve storage when interface change theirs status in real-time + * control loop. + * + * \param[interfaces] list of state interface to add into storage. + * \returns list of interface names that are added into internal storage. The output is used to + * avoid additional iterations to cache interface names, e.g., for initializing info structures. + */ + std::vector add_state_interfaces(std::vector & interfaces) + { + std::vector interface_names; + interface_names.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + auto key = interface.get_name(); + state_interface_map_.emplace(std::make_pair(key, std::move(interface))); + interface_names.push_back(key); + } + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + + /// Removes state interfaces from internal storage. + /** + * State interface are removed from the maps with theirs storage and their claimed status. + * + * \param[interface_names] list of state interface names to remove from storage. + */ + void remove_state_interfaces(const std::vector & interface_names) + { + for (const auto & interface : interface_names) + { + state_interface_map_.erase(interface); + } + } + /// Adds exported command interfaces into internal storage. /** * Add command interfaces to the internal storage. Command interfaces exported from hardware or @@ -705,7 +746,8 @@ class ResourceStorage /// Mapping between hardware and controllers that are using it (accessing data from it) std::unordered_map> hardware_used_by_controllers_; - /// Mapping between controllers and list of reference interfaces they are using + /// Mapping between controllers and list of interfaces they are using + std::unordered_map> controllers_state_interfaces_map_; std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -842,6 +884,64 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +// CM API: Called in "callback/slow"-thread +void ResourceManager::import_controller_state_interfaces( + const std::string & controller_name, std::vector & interfaces) +{ + std::lock_guard guard(resource_interfaces_lock_); + auto interface_names = resource_storage_->add_state_interfaces(interfaces); + resource_storage_->controllers_state_interfaces_map_[controller_name] = interface_names; +} + +// CM API: Called in "callback/slow"-thread +std::vector ResourceManager::get_controller_state_interface_names( + const std::string & controller_name) +{ + return resource_storage_->controllers_state_interfaces_map_.at(controller_name); +} + +// CM API: Called in "update"-thread +void ResourceManager::make_controller_state_interfaces_available( + const std::string & controller_name) +{ + auto interface_names = resource_storage_->controllers_state_interfaces_map_.at(controller_name); + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->available_state_interfaces_.insert( + resource_storage_->available_state_interfaces_.end(), interface_names.begin(), + interface_names.end()); +} + +// CM API: Called in "update"-thread +void ResourceManager::make_controller_state_interfaces_unavailable( + const std::string & controller_name) +{ + auto interface_names = resource_storage_->controllers_state_interfaces_map_.at(controller_name); + + std::lock_guard guard(resource_interfaces_lock_); + for (const auto & interface : interface_names) + { + auto found_it = std::find( + resource_storage_->available_state_interfaces_.begin(), + resource_storage_->available_state_interfaces_.end(), interface); + if (found_it != resource_storage_->available_state_interfaces_.end()) + { + resource_storage_->available_state_interfaces_.erase(found_it); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "'%s' state interface removed from available list", interface.c_str()); + } + } +} + +// CM API: Called in "callback/slow"-thread +void ResourceManager::remove_controller_state_interfaces(const std::string & controller_name) +{ + auto interface_names = resource_storage_->controllers_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_state_interfaces_map_.erase(controller_name); + + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->remove_state_interfaces(interface_names); +} + // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) From 494315d4559f20a299c2f78e6258dac8aa3bcf2d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 21 Apr 2023 19:09:50 +0200 Subject: [PATCH 02/61] rename the methods and variable from using state to estimated --- .../chainable_controller_interface.hpp | 10 +++---- .../controller_interface.hpp | 2 +- .../controller_interface_base.hpp | 2 +- .../src/chainable_controller_interface.cpp | 29 +++++++++---------- .../src/controller_interface.cpp | 2 +- .../test_chainable_controller_interface.hpp | 14 ++++----- controller_manager/src/controller_manager.cpp | 11 +++---- .../test_chainable_controller.cpp | 2 +- .../test_chainable_controller.hpp | 2 +- .../hardware_interface/resource_manager.hpp | 24 +++++++-------- hardware_interface/src/resource_manager.cpp | 27 +++++++++-------- 11 files changed, 64 insertions(+), 61 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index d1e287c22e..773b4c6ef9 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -56,7 +56,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_estimated_interfaces() final; CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; @@ -72,12 +72,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// interfaces. /** * Each chainable controller implements this methods where all its state(read only) interfaces are - * exported. The method has the same meaning as `export_state_interfaces` method from + * exported. The method has the same meaning as `export_estimated_interfaces` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * * \returns list of StateInterfaces that other controller can use as their outputs. */ - virtual std::vector on_export_state_interfaces() = 0; + virtual std::vector on_export_estimated_interfaces() = 0; /// Virtual method that each chainable controller should implement to export its read/write chainable /// interfaces. @@ -128,8 +128,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for state interfaces - std::vector state_interfaces_storage_; + /// Storage of values for estimated interfaces + std::vector estimated_interfaces_data_; /// Storage of values for reference interfaces std::vector reference_interfaces_; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index b39c025679..8391ec7b74 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -48,7 +48,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_estimated_interfaces() final; /** * Controller has no reference interfaces. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2f20c3f166..13cc456aab 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -230,7 +230,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_estimated_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 7e577999c8..05de731a73 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -45,42 +45,41 @@ return_type ChainableControllerInterface::update( } std::vector -ChainableControllerInterface::export_state_interfaces() +ChainableControllerInterface::export_estimated_interfaces() { - auto state_interfaces = on_export_state_interfaces(); - // check if the "state_interfaces_storage_" variable is resized to number of interfaces - if (state_interfaces_storage_.size() != state_interfaces.size()) + auto estimated_interfaces = on_export_estimated_interfaces(); + // check if the "estimated_interfaces_data_" variable is resized to number of interfaces + if (estimated_interfaces_data_.size() != estimated_interfaces.size()) { // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the // framework RCLCPP_FATAL( get_node()->get_logger(), - "The internal storage for state values 'state_interfaces_storage_' variable has size '%zu', " - "but it is expected to have the size '%zu' equal to the number of exported reference " + "The internal storage for estimated values 'estimated_interfaces_data_' variable has size " + "'%zu', but it is expected to have the size '%zu' equal to the number of exported reference " "interfaces. No reference interface will be exported. Please correct and recompile " "the controller with name '%s' and try again.", - state_interfaces_storage_.size(), state_interfaces.size(), get_node()->get_name()); - state_interfaces.clear(); + estimated_interfaces_data_.size(), estimated_interfaces.size(), get_node()->get_name()); + estimated_interfaces.clear(); } - // check if the names of the state interfaces begin with the controller's name - for (const auto & interface : state_interfaces) + // check if the names of the controller state interfaces begin with the controller's name + for (const auto & interface : estimated_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { RCLCPP_FATAL( get_node()->get_logger(), "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory " - " for state interfaces. No state interface will be exported. Please correct and " - "recompile the controller with name '%s' and try again.", + "mandatory for estimated interfaces. No estimated interface will be exported. Please " + "correct and recompile the controller with name '%s' and try again.", interface.get_name().c_str(), get_node()->get_name()); - state_interfaces.clear(); + estimated_interfaces.clear(); break; } } - return state_interfaces; + return estimated_interfaces; } std::vector diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 0f11bba71c..88fa1c3ed2 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,7 +28,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_state_interfaces() +std::vector ControllerInterface::export_estimated_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index c4875266d3..9f54c7a012 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -41,8 +41,8 @@ class TestableChainableControllerInterface { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); - state_interfaces_storage_.reserve(1); - state_interfaces_storage_.push_back(0.0); + estimated_interfaces_data_.reserve(1); + estimated_interfaces_data_.push_back(0.0); } controller_interface::CallbackReturn on_init() override @@ -66,14 +66,14 @@ class TestableChainableControllerInterface } // Implementation of ChainableController virtual methods - std::vector on_export_state_interfaces() override + std::vector on_export_estimated_interfaces() override { - std::vector state_interfaces; + std::vector estimated_interfaces; - state_interfaces.push_back(hardware_interface::StateInterface( - name_prefix_of_reference_interfaces_, "test_state", &state_interfaces_storage_[0])); + estimated_interfaces.push_back(hardware_interface::StateInterface( + name_prefix_of_reference_interfaces_, "test_state", &estimated_interfaces_data_[0])); - return state_interfaces; + return estimated_interfaces; } // Implementation of ChainableController virtual methods diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 99632c69b1..6e1b6ec1a6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -685,21 +685,22 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto state_interfaces = controller->export_state_interfaces(); + auto estimated_interfaces = controller->export_estimated_interfaces(); auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && state_interfaces.empty()) + if (ref_interfaces.empty() && estimated_interfaces.empty()) { // TODO(destogl): Add test for this! RCLCPP_ERROR( get_logger(), - "Controller '%s' is chainable, but does not export any state or reference interfaces.", + "Controller '%s' is chainable, but does not export any estimated or reference interfaces.", controller_name.c_str()); return controller_interface::return_type::ERROR; } if (!ref_interfaces.empty()) resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); - if (!state_interfaces.empty()) - resource_manager_->import_controller_state_interfaces(controller_name, state_interfaces); + if (!estimated_interfaces.empty()) + resource_manager_->import_controller_estimated_interfaces( + controller_name, estimated_interfaces); } // let's update the list of following and preceding controllers diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index 738d043d06..da83722a58 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -151,7 +151,7 @@ CallbackReturn TestChainableController::on_cleanup( } std::vector -TestChainableController::on_export_state_interfaces() +TestChainableController::on_export_estimated_interfaces() { return {}; } diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 9843f7cf87..cc2b3cd397 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -61,7 +61,7 @@ class TestChainableController : public controller_interface::ChainableController CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CONTROLLER_MANAGER_PUBLIC - std::vector on_export_state_interfaces() override; + std::vector on_export_estimated_interfaces() override; CONTROLLER_MANAGER_PUBLIC std::vector on_export_reference_interfaces() override; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 36f4489d30..87c7634c67 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -134,9 +134,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; - /// Add controllers' state interfaces to resource manager. + /// Add controllers' estimated interfaces to resource manager. /** - * Interface for transferring management of state interfaces to resource manager. + * Interface for transferring management of estimated interfaces to resource manager. * When chaining controllers, state interfaces are used by the preceding * controllers. * Therefore, they should be managed in the same way as state interface of hardware. @@ -144,30 +144,30 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] controller_name name of the controller which state interfaces are imported. * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ - void import_controller_state_interfaces( + void import_controller_estimated_interfaces( const std::string & controller_name, std::vector & interfaces); - /// Get list of state interface of a controller. + /// Get list of estimated interface of a controller. /** * Returns lists of stored state interfaces names for a controller. * * \param[in] controller_name for which list of state interface names is returned. * \returns list of reference interface names. */ - std::vector get_controller_state_interface_names( + std::vector get_controller_estimated_interface_names( const std::string & controller_name); - /// Add controller's state interfaces to available list. + /// Add controller's estimated interfaces to available list. /** * Adds state interfacess of a controller with given name to the available list. This method should * be called when a controller gets activated with chained mode turned on. That means, the - * controller's state interfaces can be used by another controllers in chained architectures. + * controller's estimated interfaces can be used by another controllers in chained architectures. * * \param[in] controller_name name of the controller which interfaces should become available. */ - void make_controller_state_interfaces_available(const std::string & controller_name); + void make_controller_estimated_interfaces_available(const std::string & controller_name); - /// Remove controller's state interface to available list. + /// Remove controller's estimated interface to available list. /** * Removes interfaces of a controller with given name from the available list. This method should * be called when a controller gets deactivated and its reference interfaces cannot be used by @@ -175,16 +175,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * * \param[in] controller_name name of the controller which interfaces should become unavailable. */ - void make_controller_state_interfaces_unavailable(const std::string & controller_name); + void make_controller_estimated_interfaces_unavailable(const std::string & controller_name); - /// Remove controllers state interfaces from resource manager. + /// Remove controllers estimated interfaces from resource manager. /** * Remove state interfaces from resource manager, i.e., resource storage. * The interfaces will be deleted from all internal maps and lists. * * \param[in] controller_name list of interface names that will be deleted from resource manager. */ - void remove_controller_state_interfaces(const std::string & controller_name); + void remove_controller_estimated_interfaces(const std::string & controller_name); /// Add controllers' reference interfaces to resource manager. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 1f197d9ef4..89c993bf1b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -747,7 +747,7 @@ class ResourceStorage std::unordered_map> hardware_used_by_controllers_; /// Mapping between controllers and list of interfaces they are using - std::unordered_map> controllers_state_interfaces_map_; + std::unordered_map> controllers_estimated_interfaces_map_; std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -885,26 +885,27 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con } // CM API: Called in "callback/slow"-thread -void ResourceManager::import_controller_state_interfaces( +void ResourceManager::import_controller_estimated_interfaces( const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); - resource_storage_->controllers_state_interfaces_map_[controller_name] = interface_names; + resource_storage_->controllers_estimated_interfaces_map_[controller_name] = interface_names; } // CM API: Called in "callback/slow"-thread -std::vector ResourceManager::get_controller_state_interface_names( +std::vector ResourceManager::get_controller_estimated_interface_names( const std::string & controller_name) { - return resource_storage_->controllers_state_interfaces_map_.at(controller_name); + return resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); } // CM API: Called in "update"-thread -void ResourceManager::make_controller_state_interfaces_available( +void ResourceManager::make_controller_estimated_interfaces_available( const std::string & controller_name) { - auto interface_names = resource_storage_->controllers_state_interfaces_map_.at(controller_name); + auto interface_names = + resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->available_state_interfaces_.insert( resource_storage_->available_state_interfaces_.end(), interface_names.begin(), @@ -912,10 +913,11 @@ void ResourceManager::make_controller_state_interfaces_available( } // CM API: Called in "update"-thread -void ResourceManager::make_controller_state_interfaces_unavailable( +void ResourceManager::make_controller_estimated_interfaces_unavailable( const std::string & controller_name) { - auto interface_names = resource_storage_->controllers_state_interfaces_map_.at(controller_name); + auto interface_names = + resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); for (const auto & interface : interface_names) @@ -933,10 +935,11 @@ void ResourceManager::make_controller_state_interfaces_unavailable( } // CM API: Called in "callback/slow"-thread -void ResourceManager::remove_controller_state_interfaces(const std::string & controller_name) +void ResourceManager::remove_controller_estimated_interfaces(const std::string & controller_name) { - auto interface_names = resource_storage_->controllers_state_interfaces_map_.at(controller_name); - resource_storage_->controllers_state_interfaces_map_.erase(controller_name); + auto interface_names = + resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); + resource_storage_->controllers_estimated_interfaces_map_.erase(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->remove_state_interfaces(interface_names); From 3e375ea845eb29b189bb8182110fbe2fc6f13a69 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 21 Apr 2023 23:36:00 +0200 Subject: [PATCH 03/61] update controller interface chainable controller tests for the newly exported readonly interfaces --- .../test_chainable_controller_interface.cpp | 37 ++++++++++++++++++- .../test_chainable_controller_interface.hpp | 18 +++++---- 2 files changed, 46 insertions(+), 9 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 47487e019c..d644931eec 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -31,6 +31,23 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) EXPECT_FALSE(controller.is_in_chained_mode()); } +TEST_F(ChainableControllerInterfaceTest, export_estimated_interfaces) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + auto estimated_interfaces = controller.export_estimated_interfaces(); + + ASSERT_EQ(estimated_interfaces.size(), 1u); + EXPECT_EQ(estimated_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(estimated_interfaces[0].get_interface_name(), "test_state"); + + EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE); +} + TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) { TestableChainableControllerInterface controller; @@ -51,7 +68,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); } -TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) +TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) { TestableChainableControllerInterface controller; @@ -66,9 +83,13 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc controller.reference_interfaces_.clear(); auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); + // expect empty return because storage is not resized + controller.estimated_interfaces_data_.clear(); + auto estimated_interfaces = controller.export_estimated_interfaces(); + ASSERT_TRUE(estimated_interfaces.empty()); } -TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node_name) +TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) { TestableChainableControllerInterface controller; @@ -84,6 +105,9 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node // expect empty return because interface prefix is not equal to the node name auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); + // expect empty return because interface prefix is not equal to the node name + auto estimated_interfaces = controller.export_estimated_interfaces(); + ASSERT_TRUE(estimated_interfaces.empty()); } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -99,11 +123,14 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); + auto estimated_interfaces = controller.export_estimated_interfaces(); + ASSERT_EQ(estimated_interfaces.size(), 1u); EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -116,6 +143,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); + EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -154,6 +182,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); + ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -161,6 +190,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); + ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); // Provoke error from update - return ERROR, but reference interface is updated and not reduced controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); @@ -168,6 +198,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); + ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); controller.reference_interfaces_[0] = 0.0; @@ -181,6 +212,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], -1.0); + ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1); // Provoke error from update - return ERROR, but reference interface is updated directly controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -189,4 +221,5 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); + ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1); } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index 9f54c7a012..298769fe44 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -29,12 +29,14 @@ constexpr double INTERFACE_VALUE = 1989.0; constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; +constexpr double ESTIMATED_INTERFACE_VALUE = 21833.0; +constexpr double ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface { public: - FRIEND_TEST(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size); + FRIEND_TEST(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size); FRIEND_TEST(ChainableControllerInterfaceTest, test_update_logic); TestableChainableControllerInterface() @@ -42,13 +44,13 @@ class TestableChainableControllerInterface reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); estimated_interfaces_data_.reserve(1); - estimated_interfaces_data_.push_back(0.0); + estimated_interfaces_data_.push_back(ESTIMATED_INTERFACE_VALUE); } controller_interface::CallbackReturn on_init() override { // set default value - name_prefix_of_reference_interfaces_ = get_node()->get_name(); + name_prefix_of_interfaces_ = get_node()->get_name(); return controller_interface::CallbackReturn::SUCCESS; } @@ -71,7 +73,7 @@ class TestableChainableControllerInterface std::vector estimated_interfaces; estimated_interfaces.push_back(hardware_interface::StateInterface( - name_prefix_of_reference_interfaces_, "test_state", &estimated_interfaces_data_[0])); + name_prefix_of_interfaces_, "test_state", &estimated_interfaces_data_[0])); return estimated_interfaces; } @@ -82,7 +84,7 @@ class TestableChainableControllerInterface std::vector command_interfaces; command_interfaces.push_back(hardware_interface::CommandInterface( - name_prefix_of_reference_interfaces_, "test_itf", &reference_interfaces_[0])); + name_prefix_of_interfaces_, "test_itf", &reference_interfaces_[0])); return command_interfaces; } @@ -91,6 +93,7 @@ class TestableChainableControllerInterface { if (reference_interfaces_[0] == 0.0) { + estimated_interfaces_data_[0] = ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE; return true; } else @@ -120,13 +123,14 @@ class TestableChainableControllerInterface } reference_interfaces_[0] -= 1; + estimated_interfaces_data_[0] += 1; return controller_interface::return_type::OK; } void set_name_prefix_of_reference_interfaces(const std::string & prefix) { - name_prefix_of_reference_interfaces_ = prefix; + name_prefix_of_interfaces_ = prefix; } void set_new_reference_interface_value(const double ref_itf_value) @@ -134,7 +138,7 @@ class TestableChainableControllerInterface reference_interface_value_ = ref_itf_value; } - std::string name_prefix_of_reference_interfaces_; + std::string name_prefix_of_interfaces_; double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF; }; From 297021325a20e9f2f99cad198219fbc6a3463e7e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 23 Apr 2023 21:45:48 +0200 Subject: [PATCH 04/61] Added some helper methods in the Test classes to set the estimated interfaces information --- .../test_chainable_controller.cpp | 18 +++++++++++++++++- .../test_chainable_controller.hpp | 4 ++++ .../test/test_controller/test_controller.cpp | 10 ++++++++++ .../test/test_controller/test_controller.hpp | 3 +++ 4 files changed, 34 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index da83722a58..80faf34266 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -153,7 +153,15 @@ CallbackReturn TestChainableController::on_cleanup( std::vector TestChainableController::on_export_estimated_interfaces() { - return {}; + std::vector estimated_interfaces; + + for (size_t i = 0; i < estimated_interface_names_.size(); ++i) + { + estimated_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), estimated_interface_names_[i], &estimated_interfaces_data_[i])); + } + + return estimated_interfaces; } std::vector @@ -190,6 +198,14 @@ void TestChainableController::set_reference_interface_names( reference_interfaces_.resize(reference_interface_names.size(), 0.0); } +void TestChainableController::set_estimated_interface_names( + const std::vector & estimated_interface_names) +{ + estimated_interface_names_ = estimated_interface_names; + + estimated_interfaces_data_.resize(estimated_interface_names_.size(), 0.0); +} + } // namespace test_chainable_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index cc2b3cd397..873f89d9f3 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -83,10 +83,14 @@ class TestChainableController : public controller_interface::ChainableController CONTROLLER_MANAGER_PUBLIC void set_reference_interface_names(const std::vector & reference_interface_names); + CONTROLLER_MANAGER_PUBLIC + void set_estimated_interface_names(const std::vector & estimated_interface_names); + size_t internal_counter; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; + std::vector estimated_interface_names_; realtime_tools::RealtimeBuffer> rt_command_ptr_; rclcpp::Subscription::SharedPtr joints_command_subscriber_; diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7585ae36e5..829d6475c1 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -120,6 +120,16 @@ void TestController::set_state_interface_configuration( state_iface_cfg_ = cfg; } +std::vector TestController::get_state_interface_data() const +{ + std::vector state_intr_data; + for (const auto & interface : state_interfaces_) + { + state_intr_data.push_back(interface.get_value()); + } + return state_intr_data; +} + } // namespace test_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 14ad753803..41a13e65c8 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -66,6 +66,9 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); + CONTROLLER_MANAGER_PUBLIC + std::vector get_state_interface_data() const; + const std::string & getRobotDescription() const; unsigned int internal_counter = 0; From e261d15497792a4d2915f8592a4dc37c731875b7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 26 Apr 2023 23:47:04 +0200 Subject: [PATCH 05/61] Added an IMU hardware test component --- hardware_interface/CMakeLists.txt | 1 + .../test_hardware_components.xml | 6 + .../test_imu_sensor.cpp | 143 ++++++++++++++++++ 3 files changed, 150 insertions(+) create mode 100644 hardware_interface/test/test_hardware_components/test_imu_sensor.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 94eaa6a050..ae4f24ee0f 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -83,6 +83,7 @@ if(BUILD_TESTING) add_library(test_hardware_components SHARED test/test_hardware_components/test_single_joint_actuator.cpp test/test_hardware_components/test_force_torque_sensor.cpp + test/test_hardware_components/test_imu_sensor.cpp test/test_hardware_components/test_two_joint_system.cpp test/test_hardware_components/test_system_with_command_modes.cpp ) diff --git a/hardware_interface/test/test_hardware_components/test_hardware_components.xml b/hardware_interface/test/test_hardware_components/test_hardware_components.xml index b297bde668..5d99017287 100644 --- a/hardware_interface/test/test_hardware_components/test_hardware_components.xml +++ b/hardware_interface/test/test_hardware_components/test_hardware_components.xml @@ -12,6 +12,12 @@ + + + Test sensor component emulating an IMU sensor + + + Test system component for a two degree of freedom position joint robot diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp new file mode 100644 index 0000000000..16f236fb72 --- /dev/null +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -0,0 +1,143 @@ +// Copyright 2023 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Sai Kishor Kothakota + */ + +#include +#include +#include +#include +#include + +#include "hardware_interface/sensor_interface.hpp" + +using hardware_interface::return_type; +using hardware_interface::SensorInterface; +using hardware_interface::StateInterface; + +namespace test_hardware_components +{ +class TestIMUSensor : public SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & sensor_info) override + { + if (SensorInterface::on_init(sensor_info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + const auto & state_interfaces = info_.sensors[0].state_interfaces; + if (state_interfaces.size() != 10) + { + return CallbackReturn::ERROR; + } + for (const auto & imu_key : + {"orientation.x", "orientation.y", "orientation.z", "orientation.w", "angular_velocity.x", + "angular_velocity.y", "angular_velocity.z", "linear_acceleration.x", + "linear_acceleration.y", "linear_acceleration.z"}) + { + if ( + std::find_if( + state_interfaces.begin(), state_interfaces.end(), + [&imu_key](const auto & interface_info) + { return interface_info.name == imu_key; }) == state_interfaces.end()) + { + return CallbackReturn::ERROR; + } + } + + fprintf(stderr, "TestIMUSensor configured successfully.\n"); + return CallbackReturn::SUCCESS; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + + const std::string & sensor_name = info_.sensors[0].name; + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.y", &orientation_.y)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.z", &orientation_.z)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.w", &orientation_.w)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "angular_velocity.x", &angular_velocity_.x)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "angular_velocity.y", &angular_velocity_.y)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "angular_velocity.z", &angular_velocity_.z)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor_name, "linear_acceleration.x", &linear_acceleration_.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor_name, "linear_acceleration.y", &linear_acceleration_.y)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor_name, "linear_acceleration.z", &linear_acceleration_.z)); + + return state_interfaces; + } + + return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + // generate a random distribution of the quaternion + std::uniform_real_distribution distribution_1(0.0, 1.0); + const double u1 = distribution_1(generator_); + const double u2 = distribution_1(generator_); + const double u3 = distribution_1(generator_); + orientation_.w = std::sqrt(1. - u1) * std::sin(2 * M_PI * u2); + orientation_.x = std::sqrt(1. - u1) * std::cos(2 * M_PI * u2); + orientation_.y = std::sqrt(u1) * std::sin(2 * M_PI * u3); + orientation_.z = std::sqrt(u1) * std::cos(2 * M_PI * u3); + + // generate random angular velocities and linear accelerations + std::uniform_real_distribution distribution_2(0.0, 0.1); + angular_velocity_.x = distribution_2(generator_); + angular_velocity_.y = distribution_2(generator_); + angular_velocity_.z = distribution_2(generator_); + + linear_acceleration_.x = distribution_2(generator_); + linear_acceleration_.y = distribution_2(generator_); + linear_acceleration_.z = distribution_2(generator_); + return return_type::OK; + } + +private: + struct QuaternionValues + { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double w = 1.0; + }; + struct AxisValues + { + double x = 0.0; + double y = 0.0; + double z = 0.0; + }; + + std::default_random_engine generator_; + QuaternionValues orientation_; + AxisValues angular_velocity_; + AxisValues linear_acceleration_; +}; + +} // namespace test_hardware_components + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(test_hardware_components::TestIMUSensor, hardware_interface::SensorInterface) From 9723f28d05a8b1190d5cc41ce3357c32f00cc20e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 26 Apr 2023 23:53:02 +0200 Subject: [PATCH 06/61] Added IMU sensor hardware to the differential drive URDF description --- .../ros2_control_test_assets/descriptions.hpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index a2f6195c67..79bc943912 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -450,6 +450,23 @@ const auto diffbot_urdf = + + + test_hardware_components/TestIMUSensor + + + + + + + + + + + + + + )"; From 67770560eff0144b5ea0686986f971b5f653c6f6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Apr 2023 21:37:58 +0200 Subject: [PATCH 07/61] Added new estimated interface data testing to chainable controller tests using odom and localizer --- .../test_chainable_controller.cpp | 12 + .../test_chainable_controller.hpp | 1 + ...llers_chaining_with_controller_manager.cpp | 253 ++++++++++++++++-- 3 files changed, 237 insertions(+), 29 deletions(-) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index 80faf34266..691826c65d 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -96,6 +96,18 @@ controller_interface::return_type TestChainableController::update_and_write_comm { command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); } + // If there is a command interface then integrate and set it to the estimated interface data + for (size_t i = 0; i < estimated_interface_names_.size() && i < command_interfaces_.size(); ++i) + { + estimated_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + } + // If there is no command interface and if there is a state interface then just forward the same value as in the state interface + for (size_t i = 0; i < estimated_interface_names_.size() && i < state_interfaces_.size() && + command_interfaces_.empty(); + ++i) + { + estimated_interfaces_data_[i] = state_interfaces_[i].get_value(); + } return controller_interface::return_type::OK; } diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 873f89d9f3..3ffe4b45ed 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -35,6 +35,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface constexpr char TEST_CONTROLLER_NAME[] = "test_chainable_controller_name"; // corresponds to the name listed within the pluginlib xml constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_chainable_controller"; +constexpr double CONTROLLER_DT = 0.001; class TestChainableController : public controller_interface::ChainableControllerInterface { public: 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 eadca39756..77b48142e4 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -125,6 +125,8 @@ class TestControllerChainingWithControllerManager diff_drive_controller = std::make_shared(); diff_drive_controller_two = std::make_shared(); position_tracking_controller = std::make_shared(); + odom_publisher_controller = std::make_shared(); + robot_localization_controller = std::make_shared(); // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { @@ -135,7 +137,7 @@ class TestControllerChainingWithControllerManager pid_left_wheel_controller->set_state_interface_configuration(pid_left_state_ifs_cfg); pid_left_wheel_controller->set_reference_interface_names({"velocity"}); - // configure Left Wheel controller + // configure Right Wheel controller controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = { controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = { @@ -154,11 +156,13 @@ class TestControllerChainingWithControllerManager diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); + diff_drive_controller->set_estimated_interface_names({"odom_x", "odom_y"}); - // configure Diff Drive Two controller (Has same command interfaces ad Diff Drive controller) + // configure Diff Drive Two controller (Has same command interfaces as Diff Drive controller) diff_drive_controller_two->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller_two->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller_two->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); + diff_drive_controller_two->set_estimated_interface_names({"odom_x", "odom_y"}); // configure Position Tracking controller controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { @@ -168,16 +172,28 @@ class TestControllerChainingWithControllerManager // in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel" position_tracking_controller->set_command_interface_configuration( position_tracking_cmd_ifs_cfg); + + // configure Robot Localization controller + controller_interface::InterfaceConfiguration odom_and_loc_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(DIFF_DRIVE_CONTROLLER) + "/odom_x", + std::string(DIFF_DRIVE_CONTROLLER) + "/odom_y"}}; + robot_localization_controller->set_state_interface_configuration(odom_and_loc_state_ifs_cfg); + + // configure Odometry Publisher controller + odom_publisher_controller->set_state_interface_configuration(odom_and_loc_state_ifs_cfg); } void CheckIfControllersAreAddedCorrectly() { - EXPECT_EQ(5u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(7u, cm_->get_loaded_controllers().size()); EXPECT_EQ(2, pid_left_wheel_controller.use_count()); EXPECT_EQ(2, pid_right_wheel_controller.use_count()); EXPECT_EQ(2, diff_drive_controller.use_count()); EXPECT_EQ(2, diff_drive_controller_two.use_count()); EXPECT_EQ(2, position_tracking_controller.use_count()); + EXPECT_EQ(2, robot_localization_controller.use_count()); + EXPECT_EQ(2, odom_publisher_controller.use_count()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, @@ -194,6 +210,12 @@ class TestControllerChainingWithControllerManager EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, position_tracking_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + robot_localization_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + odom_publisher_controller->get_state().id()); } // order or controller configuration is not important therefore we can reuse the same method @@ -201,6 +223,7 @@ class TestControllerChainingWithControllerManager { // Store initial values of command interfaces size_t number_of_cmd_itfs = cm_->resource_manager_->command_interface_keys().size(); + size_t number_of_state_itfs = cm_->resource_manager_->state_interface_keys().size(); // configure chainable controller and check exported interfaces cm_->configure_controller(PID_LEFT_WHEEL); @@ -208,6 +231,7 @@ class TestControllerChainingWithControllerManager pid_left_wheel_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); for (const auto & interface : {"pid_left_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); @@ -220,6 +244,7 @@ class TestControllerChainingWithControllerManager pid_right_wheel_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); for (const auto & interface : {"pid_right_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); @@ -228,9 +253,14 @@ class TestControllerChainingWithControllerManager } cm_->configure_controller(DIFF_DRIVE_CONTROLLER); + for (auto & x : cm_->resource_manager_->available_state_interfaces()) + { + RCLCPP_ERROR_STREAM(cm_->get_logger(), x); + } EXPECT_EQ( diff_drive_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 2); for (const auto & interface : {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}) @@ -239,12 +269,18 @@ class TestControllerChainingWithControllerManager EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } + for (const auto & interface : {"diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->state_interface_is_available(interface)); + } cm_->configure_controller(DIFF_DRIVE_CONTROLLER_TWO); EXPECT_EQ( diff_drive_controller_two->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); for (const auto & interface : {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}) @@ -253,31 +289,68 @@ class TestControllerChainingWithControllerManager EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } + for (const auto & interface : {"diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->state_interface_is_available(interface)); + } cm_->configure_controller(POSITION_TRACKING_CONTROLLER); EXPECT_EQ( position_tracking_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); + + cm_->configure_controller(ROBOT_LOCALIZATION_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); + + cm_->configure_controller(ODOM_PUBLISHER_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); } template < typename T, typename std::enable_if< std::is_convertible::value, T>::type * = nullptr> - void SetToChainedModeAndMakeReferenceInterfacesAvailable( + void SetToChainedModeAndMakeInterfacesAvailable( std::shared_ptr & controller, const std::string & controller_name, + const std::vector & estimated_interfaces, const std::vector & reference_interfaces) { - controller->set_chained_mode(true); - EXPECT_TRUE(controller->is_in_chained_mode()); - // make reference interface command_interfaces available - cm_->resource_manager_->make_controller_reference_interfaces_available(controller_name); - for (const auto & interface : reference_interfaces) + if (!estimated_interfaces.empty() || !reference_interfaces.empty()) { - EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + controller->set_chained_mode(true); + EXPECT_TRUE(controller->is_in_chained_mode()); + if (!reference_interfaces.empty()) + { + // make reference interface command_interfaces available + cm_->resource_manager_->make_controller_reference_interfaces_available(controller_name); + for (const auto & interface : reference_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + } + if (!estimated_interfaces.empty()) + { + // make estimated interface state_interfaces available + cm_->resource_manager_->make_controller_estimated_interfaces_available(controller_name); + for (const auto & interface : estimated_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); + } + } } } @@ -382,10 +455,12 @@ class TestControllerChainingWithControllerManager cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if all controllers are updated - ASSERT_EQ(position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl); - ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 2); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); + ASSERT_EQ(odom_publisher_controller->internal_counter, exp_internal_counter_pos_ctrl); + ASSERT_EQ(robot_localization_controller->internal_counter, exp_internal_counter_pos_ctrl + 2); + ASSERT_EQ(position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); + ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 10); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 8); // check if values are set properly in controllers ASSERT_EQ( @@ -401,12 +476,24 @@ class TestControllerChainingWithControllerManager ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + EXP_ESTIMATED_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); + EXP_ESTIMATED_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); @@ -415,10 +502,17 @@ class TestControllerChainingWithControllerManager pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); } // check data propagation through controllers and if individual controllers are working double chained_ctrl_calculation(double reference, double state) { return reference - state; } + double chained_estimate_calculation(double reference, double state) + { + return (reference - state) * test_chainable_controller::CONTROLLER_DT; + } double hardware_calculation(double command) { return command / 2.0; } // set controllers' names @@ -427,6 +521,8 @@ class TestControllerChainingWithControllerManager static constexpr char DIFF_DRIVE_CONTROLLER[] = "diff_drive_controller"; static constexpr char DIFF_DRIVE_CONTROLLER_TWO[] = "diff_drive_controller_two"; static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller"; + static constexpr char ROBOT_LOCALIZATION_CONTROLLER[] = "robot_localization_controller"; + static constexpr char ODOM_PUBLISHER_CONTROLLER[] = "odometry_publisher_controller"; const std::vector PID_LEFT_WHEEL_REFERENCE_INTERFACES = { "pid_left_wheel_controller/velocity"}; @@ -434,6 +530,8 @@ class TestControllerChainingWithControllerManager "pid_right_wheel_controller/velocity"}; const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; + const std::vector DIFF_DRIVE_ESTIMATED_INTERFACES = { + "diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}; const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"}; const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"}; @@ -447,6 +545,8 @@ class TestControllerChainingWithControllerManager std::shared_ptr pid_right_wheel_controller; std::shared_ptr diff_drive_controller; std::shared_ptr diff_drive_controller_two; + std::shared_ptr robot_localization_controller; + std::shared_ptr odom_publisher_controller; std::shared_ptr position_tracking_controller; testing::WithParamInterface::ParamType test_param; @@ -458,6 +558,8 @@ class TestControllerChainingWithControllerManager double EXP_RIGHT_WHEEL_HW_STATE = 0.0; double EXP_LEFT_WHEEL_REF = 0.0; double EXP_RIGHT_WHEEL_REF = 0.0; + double EXP_ESTIMATED_ODOM_X = 0.0; + double EXP_ESTIMATED_ODOM_Y = 0.0; // Expected behaviors struct used in chaining activation/deactivation tests struct ExpectedBehaviorStruct @@ -492,17 +594,24 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); ConfigureAndCheckControllers(); - SetToChainedModeAndMakeReferenceInterfacesAvailable( - pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); - SetToChainedModeAndMakeReferenceInterfacesAvailable( - pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); - SetToChainedModeAndMakeReferenceInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, {}, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_ESTIMATED_INTERFACES, + DIFF_DRIVE_REFERENCE_INTERFACES); EXPECT_THROW( cm_->resource_manager_->make_controller_reference_interfaces_available( @@ -512,17 +621,29 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + // Before starting check that the internal counters are set to zero + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 0u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 0u); + ASSERT_EQ(diff_drive_controller->internal_counter, 0u); + ASSERT_EQ(diff_drive_controller_two->internal_counter, 0u); + ASSERT_EQ(position_tracking_controller->internal_counter, 0u); + ASSERT_EQ(odom_publisher_controller->internal_counter, 0u); + ASSERT_EQ(robot_localization_controller->internal_counter, 0u); + // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER // (otherwise, interface will be missing) ActivateAndCheckController( pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 1u); ActivateAndCheckController( pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 1u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); // Diff-Drive Controller claims the reference interfaces of PID controllers ActivateAndCheckController( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(diff_drive_controller->internal_counter, 1u); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); @@ -530,6 +651,21 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ActivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(position_tracking_controller->internal_counter, 1u); + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // Robot localization Controller uses estimated interfaces of Diff-Drive Controller + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ASSERT_EQ(robot_localization_controller->internal_counter, 1u); + ASSERT_EQ(position_tracking_controller->internal_counter, 3u); + ASSERT_EQ(diff_drive_controller->internal_counter, 5u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 7u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 9u); + + // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) { @@ -537,9 +673,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } - ASSERT_EQ(diff_drive_controller->internal_counter, 3u); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + ASSERT_EQ(odom_publisher_controller->internal_counter, 1u); + ASSERT_EQ(robot_localization_controller->internal_counter, 3u); + ASSERT_EQ(position_tracking_controller->internal_counter, 5u); + ASSERT_EQ(diff_drive_controller->internal_counter, 7u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); // update controllers std::vector reference = {32.0, 128.0}; @@ -552,25 +691,39 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(position_tracking_controller->internal_counter, 2u); + ASSERT_EQ(position_tracking_controller->internal_counter, 6u); ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through // update 'Diff Drive' Controller diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(diff_drive_controller->internal_counter, 4u); + ASSERT_EQ(diff_drive_controller->internal_counter, 8u); // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + // run the update cycles of the robot localization and odom publisher controller + robot_localization_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXP_ESTIMATED_ODOM_X = + chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 / dt + EXP_ESTIMATED_ODOM_Y = + chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 / dt + ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 12u); pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 10u); // update hardware ('read' is sufficient for test hardware) cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -582,6 +735,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); // 128 - 0 EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); @@ -589,8 +745,13 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); + ASSERT_EQ(robot_localization_controller->internal_counter, 4u); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); // update all controllers at once and see that all have expected values --> also checks the order // of controller execution @@ -624,6 +785,12 @@ TEST_P( cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -662,6 +829,16 @@ TEST_P( EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + // Robot localization Controller uses estimated interfaces of Diff-Drive Controller + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 0u); + // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 0u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_EQ(robot_localization_controller->internal_counter, 0u); + ASSERT_EQ(odom_publisher_controller->internal_counter, 0u); + UpdateAllControllerAndCheck({32.0, 128.0}, 2u); UpdateAllControllerAndCheck({1024.0, 4096.0}, 3u); @@ -713,6 +890,12 @@ TEST_P( cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -802,6 +985,12 @@ TEST_P( cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -864,6 +1053,12 @@ TEST_P( cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); From 483dbfa51d37f3fbf88eb4db69a886b80a62ef4d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 1 May 2023 12:14:38 +0200 Subject: [PATCH 08/61] Add optional imu sensor to the testing of chainable controller --- .../test_chainable_controller.cpp | 11 +++++++++++ .../test_chainable_controller.hpp | 5 +++++ 2 files changed, 16 insertions(+) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index 691826c65d..6e92b13ad6 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -52,6 +52,13 @@ TestChainableController::state_interface_configuration() const get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + auto state_iface_cfg = state_iface_cfg_; + if (imu_sensor_) + { + auto imu_interfaces = imu_sensor_->get_state_interface_names(); + state_iface_cfg.names.insert( + state_iface_cfg.names.end(), imu_interfaces.begin(), imu_interfaces.end()); + } return state_iface_cfg_; } else @@ -218,6 +225,10 @@ void TestChainableController::set_estimated_interface_names( estimated_interfaces_data_.resize(estimated_interface_names_.size(), 0.0); } +void TestChainableController::set_imu_sensor_name(const std::string & name) +{ + if (!name.empty()) imu_sensor_ = std::make_unique(name); +} } // namespace test_chainable_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 3ffe4b45ed..0a9a06fb1a 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -23,6 +23,7 @@ #include "controller_manager/visibility_control.h" #include "rclcpp/subscription.hpp" #include "realtime_tools/realtime_buffer.h" +#include "semantic_components/imu_sensor.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace test_chainable_controller @@ -87,11 +88,15 @@ class TestChainableController : public controller_interface::ChainableController CONTROLLER_MANAGER_PUBLIC void set_estimated_interface_names(const std::vector & estimated_interface_names); + CONTROLLER_MANAGER_PUBLIC + void set_imu_sensor_name(const std::string &name); + size_t internal_counter; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; std::vector estimated_interface_names_; + std::unique_ptr imu_sensor_; realtime_tools::RealtimeBuffer> rt_command_ptr_; rclcpp::Subscription::SharedPtr joints_command_subscriber_; From 6fd8dcea9524a278c1138459f1fb88579b9b7a3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 1 May 2023 12:20:10 +0200 Subject: [PATCH 09/61] rename the method to have more generic meaning --- controller_manager/src/controller_manager.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6e1b6ec1a6..639dba17fe 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -82,9 +82,9 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const return a.info.name == name; } -/// Checks if a command interface belongs to a controller based on its prefix. +/// Checks if an interface belongs to a controller based on its prefix. /** - * A command interface can be provided by a controller in which case is called "reference" + * A State/Command interface can be provided by a controller in which case is called "estimated/reference" * interface. * This means that the @interface_name starts with the name of a controller. * @@ -94,7 +94,7 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const * @interface_name belongs to. * \return true if interface has a controller name as prefix, false otherwise. */ -bool command_interface_is_reference_interface_of_controller( +bool is_interface_a_chained_interface( const std::string interface_name, const std::vector & controllers, controller_manager::ControllersListIterator & following_controller_it) @@ -124,7 +124,7 @@ bool command_interface_is_reference_interface_of_controller( { RCLCPP_DEBUG( rclcpp::get_logger("ControllerManager::utils"), - "Required command interface '%s' with prefix '%s' is not reference interface.", + "Required interface '%s' with prefix '%s' is not a chained interface.", interface_name.c_str(), interface_prefix.c_str()); return false; @@ -2206,8 +2206,7 @@ void ControllerManager::propagate_deactivation_of_chained_mode( { // controller that 'cmd_tf_name' belongs to ControllersListIterator following_ctrl_it; - if (command_interface_is_reference_interface_of_controller( - cmd_itf_name, controllers, following_ctrl_it)) + if (is_interface_a_chained_interface(cmd_itf_name, controllers, following_ctrl_it)) { // currently iterated "controller" is preceding controller --> add following controller // with matching interface name to "from" chained mode list (if not already in it) @@ -2240,8 +2239,7 @@ controller_interface::return_type ControllerManager::check_following_controllers { ControllersListIterator following_ctrl_it; // Check if interface if reference interface and following controller exist. - if (!command_interface_is_reference_interface_of_controller( - cmd_itf_name, controllers, following_ctrl_it)) + if (!is_interface_a_chained_interface(cmd_itf_name, controllers, following_ctrl_it)) { continue; } From df728518ee52935b83d9d32ae518bb7f823adfa1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 22:14:10 +0200 Subject: [PATCH 10/61] Added sensor_fusion_controller to the class TestControllerChainingWithControllerManager --- ...llers_chaining_with_controller_manager.cpp | 55 ++++++++++++++----- 1 file changed, 42 insertions(+), 13 deletions(-) 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 77b48142e4..e760f3b218 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -126,6 +126,7 @@ class TestControllerChainingWithControllerManager diff_drive_controller_two = std::make_shared(); position_tracking_controller = std::make_shared(); odom_publisher_controller = std::make_shared(); + sensor_fusion_controller = std::make_shared(); robot_localization_controller = std::make_shared(); // configure Left Wheel controller @@ -173,25 +174,36 @@ class TestControllerChainingWithControllerManager position_tracking_controller->set_command_interface_configuration( position_tracking_cmd_ifs_cfg); - // configure Robot Localization controller - controller_interface::InterfaceConfiguration odom_and_loc_state_ifs_cfg = { + // configure Odometry Publisher controller + controller_interface::InterfaceConfiguration odom_and_fusion_ifs_cfg = { controller_interface::interface_configuration_type::INDIVIDUAL, {std::string(DIFF_DRIVE_CONTROLLER) + "/odom_x", std::string(DIFF_DRIVE_CONTROLLER) + "/odom_y"}}; - robot_localization_controller->set_state_interface_configuration(odom_and_loc_state_ifs_cfg); + odom_publisher_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); - // configure Odometry Publisher controller - odom_publisher_controller->set_state_interface_configuration(odom_and_loc_state_ifs_cfg); + // configure sensor fusion controller + sensor_fusion_controller->set_imu_sensor_name("base_imu"); + sensor_fusion_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); + sensor_fusion_controller->set_estimated_interface_names({"odom_x", "odom_y", "yaw"}); + + // configure Robot Localization controller + controller_interface::InterfaceConfiguration odom_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(SENSOR_FUSION_CONTROLLER) + "/odom_x", + std::string(SENSOR_FUSION_CONTROLLER) + "/odom_y", + std::string(SENSOR_FUSION_CONTROLLER) + "/yaw"}}; + robot_localization_controller->set_state_interface_configuration(odom_ifs_cfg); } void CheckIfControllersAreAddedCorrectly() { - EXPECT_EQ(7u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(8u, cm_->get_loaded_controllers().size()); EXPECT_EQ(2, pid_left_wheel_controller.use_count()); EXPECT_EQ(2, pid_right_wheel_controller.use_count()); EXPECT_EQ(2, diff_drive_controller.use_count()); EXPECT_EQ(2, diff_drive_controller_two.use_count()); EXPECT_EQ(2, position_tracking_controller.use_count()); + EXPECT_EQ(2, sensor_fusion_controller.use_count()); EXPECT_EQ(2, robot_localization_controller.use_count()); EXPECT_EQ(2, odom_publisher_controller.use_count()); @@ -210,6 +222,9 @@ class TestControllerChainingWithControllerManager EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, position_tracking_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + sensor_fusion_controller->get_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, robot_localization_controller->get_state().id()); @@ -302,19 +317,26 @@ class TestControllerChainingWithControllerManager EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); + cm_->configure_controller(SENSOR_FUSION_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); + cm_->configure_controller(ROBOT_LOCALIZATION_CONTROLLER); EXPECT_EQ( position_tracking_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); - EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); cm_->configure_controller(ODOM_PUBLISHER_CONTROLLER); EXPECT_EQ( position_tracking_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); - EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); } template < @@ -457,10 +479,11 @@ class TestControllerChainingWithControllerManager // check if all controllers are updated ASSERT_EQ(odom_publisher_controller->internal_counter, exp_internal_counter_pos_ctrl); ASSERT_EQ(robot_localization_controller->internal_counter, exp_internal_counter_pos_ctrl + 2); - ASSERT_EQ(position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); - ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 10); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 8); + ASSERT_EQ(sensor_fusion_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); + ASSERT_EQ(position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); + ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 8); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 12); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 10); // check if values are set properly in controllers ASSERT_EQ( @@ -478,7 +501,8 @@ class TestControllerChainingWithControllerManager EXP_ESTIMATED_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); EXP_ESTIMATED_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(sensor_fusion_controller->estimated_interfaces_data_.size(), 3u); + ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); @@ -521,6 +545,7 @@ class TestControllerChainingWithControllerManager static constexpr char DIFF_DRIVE_CONTROLLER[] = "diff_drive_controller"; static constexpr char DIFF_DRIVE_CONTROLLER_TWO[] = "diff_drive_controller_two"; static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller"; + static constexpr char SENSOR_FUSION_CONTROLLER[] = "sensor_fusion_controller"; static constexpr char ROBOT_LOCALIZATION_CONTROLLER[] = "robot_localization_controller"; static constexpr char ODOM_PUBLISHER_CONTROLLER[] = "odometry_publisher_controller"; @@ -532,6 +557,9 @@ class TestControllerChainingWithControllerManager "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; const std::vector DIFF_DRIVE_ESTIMATED_INTERFACES = { "diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}; + const std::vector SENSOR_FUSION_ESIMTATED_INTERFACES = { + "sensor_fusion_controller/odom_x", "sensor_fusion_controller/odom_y", + "sensor_fusion_controller/yaw"}; const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"}; const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"}; @@ -545,6 +573,7 @@ class TestControllerChainingWithControllerManager std::shared_ptr pid_right_wheel_controller; std::shared_ptr diff_drive_controller; std::shared_ptr diff_drive_controller_two; + std::shared_ptr sensor_fusion_controller; std::shared_ptr robot_localization_controller; std::shared_ptr odom_publisher_controller; std::shared_ptr position_tracking_controller; From 27821469cdc3ca5364885eb4a0a3f9ab979a623b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 22:15:49 +0200 Subject: [PATCH 11/61] update the test_chained_controllers test with new sensor_fusion_controller --- ...llers_chaining_with_controller_manager.cpp | 45 ++++++++++++++----- 1 file changed, 33 insertions(+), 12 deletions(-) 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 e760f3b218..74a6316a34 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -626,6 +626,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) cm_->add_controller( odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -641,6 +644,8 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) SetToChainedModeAndMakeInterfacesAvailable( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_ESTIMATED_INTERFACES, DIFF_DRIVE_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, SENSOR_FUSION_ESIMTATED_INTERFACES, {}); EXPECT_THROW( cm_->resource_manager_->make_controller_reference_interfaces_available( @@ -656,6 +661,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(diff_drive_controller->internal_counter, 0u); ASSERT_EQ(diff_drive_controller_two->internal_counter, 0u); ASSERT_EQ(position_tracking_controller->internal_counter, 0u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 0u); ASSERT_EQ(odom_publisher_controller->internal_counter, 0u); ASSERT_EQ(robot_localization_controller->internal_counter, 0u); @@ -685,14 +691,23 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); - // Robot localization Controller uses estimated interfaces of Diff-Drive Controller - ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); - ASSERT_EQ(robot_localization_controller->internal_counter, 1u); + // Sensor Controller uses estimated interfaces of Diff-Drive Controller and IMU + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 1u); ASSERT_EQ(position_tracking_controller->internal_counter, 3u); ASSERT_EQ(diff_drive_controller->internal_counter, 5u); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 7u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 9u); + // Robot localization Controller uses estimated interfaces of Diff-Drive Controller + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ASSERT_EQ(robot_localization_controller->internal_counter, 1u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 3u); + ASSERT_EQ(position_tracking_controller->internal_counter, 5u); + ASSERT_EQ(diff_drive_controller->internal_counter, 7u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); + // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); // 'rot_z' reference interface is not claimed @@ -704,10 +719,11 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) } ASSERT_EQ(odom_publisher_controller->internal_counter, 1u); ASSERT_EQ(robot_localization_controller->internal_counter, 3u); - ASSERT_EQ(position_tracking_controller->internal_counter, 5u); - ASSERT_EQ(diff_drive_controller->internal_counter, 7u); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 5u); + ASSERT_EQ(position_tracking_controller->internal_counter, 7u); + ASSERT_EQ(diff_drive_controller->internal_counter, 9u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 11u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 13u); // update controllers std::vector reference = {32.0, 128.0}; @@ -720,14 +736,14 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(position_tracking_controller->internal_counter, 6u); + ASSERT_EQ(position_tracking_controller->internal_counter, 8u); ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through // update 'Diff Drive' Controller diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(diff_drive_controller->internal_counter, 8u); + ASSERT_EQ(diff_drive_controller->internal_counter, 10u); // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 @@ -735,13 +751,17 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); // run the update cycles of the robot localization and odom publisher controller + sensor_fusion_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u); robot_localization_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(robot_localization_controller->internal_counter, 4u); odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); EXP_ESTIMATED_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 / dt EXP_ESTIMATED_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 / dt - ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); @@ -750,9 +770,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, 12u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 14u); pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, 10u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 12u); // update hardware ('read' is sufficient for test hardware) cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -775,6 +795,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u); ASSERT_EQ(robot_localization_controller->internal_counter, 4u); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); From 889265b5f8e838b7cf40755314f6c74a62d212a1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 22:21:30 +0200 Subject: [PATCH 12/61] Update auto_switch_to_chained_mode with more elaborate tests incl. sensor_fusion_controller --- .../test_chainable_controller_interface.cpp | 5 +- controller_manager/src/controller_manager.cpp | 4 +- ...llers_chaining_with_controller_manager.cpp | 145 +++++++++++++++--- 3 files changed, 133 insertions(+), 21 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index d644931eec..0f3c30bf99 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -36,7 +36,10 @@ TEST_F(ChainableControllerInterfaceTest, export_estimated_interfaces) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto estimated_interfaces = controller.export_estimated_interfaces(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 639dba17fe..9a2e02fac7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -709,7 +709,7 @@ controller_interface::return_type ControllerManager::configure_controller( for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; - if (command_interface_is_reference_interface_of_controller(cmd_itf, controllers, ctrl_it)) + if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) { add_element_to_list( controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); @@ -721,7 +721,7 @@ controller_interface::return_type ControllerManager::configure_controller( for (const auto & state_itf : state_itfs) { controller_manager::ControllersListIterator ctrl_it; - if (command_interface_is_reference_interface_of_controller(state_itf, controllers, ctrl_it)) + if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) { add_element_to_list( controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); 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 74a6316a34..afacfe895d 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -838,6 +838,9 @@ TEST_P( cm_->add_controller( odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -863,6 +866,7 @@ TEST_P( EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); // DiffDrive (preceding) controller is activated --> PID controller in chained mode ActivateAndCheckController( @@ -870,6 +874,7 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); // PositionController is activated --> all following controller in chained mode ActivateAndCheckController( @@ -878,45 +883,149 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + for (const auto & interface : {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y"}) + { + 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)); + } + for (const auto & interface : {"diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); + } + + // Sensor fusion Controller uses estimated interfaces of Diff-Drive Controller and IMU + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + + // Robot localization Controller uses estimated interfaces of sensor fusion Controller + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); - // Robot localization Controller uses estimated interfaces of Diff-Drive Controller - ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 0u); // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller - ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 0u); + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); - ASSERT_EQ(robot_localization_controller->internal_counter, 0u); - ASSERT_EQ(odom_publisher_controller->internal_counter, 0u); + ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + ASSERT_EQ(robot_localization_controller->internal_counter, 3u); + ASSERT_EQ(odom_publisher_controller->internal_counter, 1u); UpdateAllControllerAndCheck({32.0, 128.0}, 2u); UpdateAllControllerAndCheck({1024.0, 4096.0}, 3u); // Test switch 'from chained mode' when controllers are deactivated - // PositionController is deactivated --> DiffDrive controller is not in chained mode anymore + // PositionController is deactivated --> DiffDrive controller still continues in chained mode + // As the DiffDriveController is in chained mode, right now we tend to also deactivate + // the other controllers that rely on the DiffDriveController expected interfaces DeactivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, - POSITION_CONTROLLER_CLAIMED_INTERFACES, 4u, true); + POSITION_CONTROLLER_CLAIMED_INTERFACES, 10u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + // SensorFusionController continues to stay in the chained mode as it is still using the estimated interfaces + ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_state().id()); - // DiffDrive (preceding) controller is activated --> PID controller in chained mode + // Deactivate the robot localization controller and see that the sensor fusion controller is still active but not in the chained mode DeactivateAndCheckController( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 8u, true); + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 8u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); + + // Deactivate the odometry publisher controller + DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 8u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); + + // Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active but not in the chained mode + DeactivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 14u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); + + // Deactivate the diff_drive_controller as all it's following controllers that uses it's interfaces are deactivated + DeactivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 20u, true); EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( - pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u, true); + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 26u, true); DeactivateAndCheckController( - pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u, true); + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 26u, true); EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); } TEST_P( @@ -1249,12 +1358,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add ConfigureAndCheckControllers(); - SetToChainedModeAndMakeReferenceInterfacesAvailable( - pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); - SetToChainedModeAndMakeReferenceInterfacesAvailable( - pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); - SetToChainedModeAndMakeReferenceInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, {}, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, {}, DIFF_DRIVE_REFERENCE_INTERFACES); EXPECT_THROW( cm_->resource_manager_->make_controller_reference_interfaces_available( From 1d182f31d47df824faafdb98f67622cb1351ffa0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 22:26:36 +0200 Subject: [PATCH 13/61] Update the activation_error_handling tests with all newly added controllers --- ...llers_chaining_with_controller_manager.cpp | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) 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 afacfe895d..6035710692 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -1052,6 +1052,9 @@ TEST_P( cm_->add_controller( odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1068,6 +1071,7 @@ TEST_P( EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); // Test Case 1: Trying to activate a preceding controller when following controller // is not activated --> return error (If STRICT); Preceding controller is still inactive. @@ -1088,6 +1092,17 @@ TEST_P( // Check if the controller activated (Should not be activated) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + + ActivateController( + SENSOR_FUSION_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + // Check if the controller activated (Should not be activated) + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); // Test Case 2: Try to activate a preceding controller the same time when trying to // deactivate a following controller (using switch_controller function) @@ -1101,11 +1116,11 @@ TEST_P( ActivateAndCheckController( pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); - // Attempt to activate a preceding controller (diff-drive controller) + // Attempt to activate a preceding controller (diff-drive controller + remaining) // while trying to deactivate a following controller switch_test_controllers( - {DIFF_DRIVE_CONTROLLER}, {PID_RIGHT_WHEEL}, test_param.strictness, - expected.at(test_param.strictness).future_status, + {DIFF_DRIVE_CONTROLLER, ODOM_PUBLISHER_CONTROLLER, SENSOR_FUSION_CONTROLLER}, {PID_RIGHT_WHEEL}, + test_param.strictness, expected.at(test_param.strictness).future_status, expected.at(test_param.strictness).return_type); // Preceding controller should stay deactivated and following controller @@ -1117,6 +1132,11 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); } TEST_P( From 2d103f759817aff2cf5543c568aa2f9c6c4e4a4c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 22:31:49 +0200 Subject: [PATCH 14/61] update tests of the activation and deactivation error handling with new controllers --- ...llers_chaining_with_controller_manager.cpp | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) 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 6035710692..6ab6d459d8 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -1167,6 +1167,9 @@ TEST_P( cm_->add_controller( odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1235,6 +1238,9 @@ TEST_P( cm_->add_controller( odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1251,6 +1257,7 @@ TEST_P( EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); // Activate following controllers ActivateAndCheckController( @@ -1273,11 +1280,28 @@ TEST_P( // Verify preceding controller (diff_drive_controller) is inactive EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); // Attempt to deactivate inactive controller (diff_drive_controller) DeactivateController( DIFF_DRIVE_CONTROLLER, expected.at(test_param.strictness).return_type, std::future_status::ready); + DeactivateController( + SENSOR_FUSION_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + DeactivateController( + ODOM_PUBLISHER_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + DeactivateController( + ROBOT_LOCALIZATION_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); // Check to see preceding controller (diff_drive_controller) is still inactive and // following controllers (pid_left_wheel_controller) (pid_left_wheel_controller) are still active @@ -1287,6 +1311,14 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); // Test Case 6: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state From ad7525343a2024f46b13b39065ab65bb9205cfd7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 22:32:34 +0200 Subject: [PATCH 15/61] Added new deactivation_switching_error_handling to test the controller deactivation behaviour --- ...llers_chaining_with_controller_manager.cpp | 252 +++++++++++++++++- 1 file changed, 250 insertions(+), 2 deletions(-) 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 6ab6d459d8..e93a91ddef 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -446,10 +446,11 @@ class TestControllerChainingWithControllerManager std::shared_ptr & controller, const std::string & controller_name, const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, const bool claimed_interfaces_from_hw = false, - const controller_interface::return_type expected_return = controller_interface::return_type::OK) + const controller_interface::return_type expected_return = controller_interface::return_type::OK, + const std::future_status expected_future_status = std::future_status::timeout) { switch_test_controllers( - {}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return); + {}, {controller_name}, test_param.strictness, expected_future_status, expected_return); check_after_de_activate( controller, claimed_command_itfs, expected_internal_counter, expected_return, true, claimed_interfaces_from_hw); @@ -1385,6 +1386,253 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); } +TEST_P( + TestControllerChainingWithControllerManager, + test_chained_controllers_deactivation_switching_error_handling) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // at beginning controllers are not in chained mode + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + + // Activate the following controller and the preceding controllers + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + + // check once active that they are in chained mode + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + + // Verify that initially all of them are in active state + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_state().id()); + + // There is different error and timeout behavior depending on strictness + static std::unordered_map expected = { + {controller_manager_msgs::srv::SwitchController::Request::STRICT, + {controller_interface::return_type::ERROR, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}}, + {controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + {controller_interface::return_type::OK, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; + + // Test switch 'from chained mode' when controllers are deactivated and possible combination of disabling + // controllers that use reference/estimated interfaces of the other controller. + // This is important to check that deactivation is not trigger irrespective of the type (reference/estimated) + // interface that is shared among the other controllers + + // PositionController is deactivated --> DiffDrive controller still continues in chained mode + // As the DiffDriveController is in chained mode, right now we tend to also deactivate + // the other controllers that rely on the DiffDriveController expected interfaces + DeactivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 2u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + // SensorFusionController continues to stay in the chained mode as it is still using the estimated interfaces + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_state().id()); + + // DiffDrive (preceding) controller is activated --> PID controller in chained mod + // Let's try to deactivate the diff_drive_control, it should fail as there are still other controllers that use it's resources + DeactivateController( + DIFF_DRIVE_CONTROLLER, expected.at(test_param.strictness).return_type, + expected.at(test_param.strictness).future_status); + check_after_de_activate( + diff_drive_controller, DIFF_DRIVE_CLAIMED_INTERFACES, 0u, + controller_interface::return_type::ERROR, true, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_state().id()); + + // Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the robot + // localization controller is still active + DeactivateAndCheckController( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 0u, true, + expected.at(test_param.strictness).return_type, + expected.at(test_param.strictness).future_status); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_state().id()); + + // Deactivate the robot localization controller and see that the sensor fusion controller is still active but not in the chained mode + DeactivateAndCheckController( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 0u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); + + // Deactivate the sensor_fusion controller and this should be successful as there are no other + // controllers using it's interfaces + DeactivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 0u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); + + // Deactivate the odometry publisher controller and now the diff_drive should continue active but not in chained mode + DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 0u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); + + // Deactivate the diff_drive_controller as all it's following controllers that uses it's interfaces are deactivated + DeactivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 0u); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + + // all controllers are deactivated --> chained mode is not changed + DeactivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 0u, true); + DeactivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 0u, true); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); +} + TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) { SetupControllers(); From f0bc18c438b5a127a71acfdae45d351877e7d525 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 22:43:51 +0200 Subject: [PATCH 16/61] Added toggle_references_from_subscribers method to the controller interface base to have control over the references --- .../chainable_controller_interface.hpp | 6 ++++++ .../controller_interface.hpp | 8 +++++++ .../controller_interface_base.hpp | 15 +++++++++++-- .../src/chainable_controller_interface.cpp | 21 ++++++++++++++++++- .../src/controller_interface.cpp | 2 ++ .../test_chainable_controller_interface.cpp | 2 ++ 6 files changed, 51 insertions(+), 3 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 773b4c6ef9..84ab1cdd0f 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -67,6 +67,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase CONTROLLER_INTERFACE_PUBLIC bool is_in_chained_mode() const final; + CONTROLLER_INTERFACE_PUBLIC + virtual bool toggle_references_from_subscribers(bool enable) final; + protected: /// Virtual method that each chainable controller should implement to export its read-only chainable /// interfaces. @@ -137,6 +140,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase private: /// A flag marking if a chainable controller is currently preceded by another controller. bool in_chained_mode_ = false; + + /// A flag marking whether to use references from subscribers or from the interfaces as input commands + bool use_references_from_subscribers_ = false; }; } // namespace controller_interface diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 8391ec7b74..85da8f2c62 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -73,6 +73,14 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase */ CONTROLLER_INTERFACE_PUBLIC bool is_in_chained_mode() const final; + + /** + * Controller is no chainable, so cannot use references from subscribers + * + * @return false + */ + CONTROLLER_INTERFACE_PUBLIC + virtual bool toggle_references_from_subscribers(bool enable) final; }; using ControllerInterfaceSharedPtr = std::shared_ptr; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 13cc456aab..5915eb2b46 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -235,8 +235,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode - * usually involves disabling of subscribers and other external interfaces to avoid potential - * concurrency in input commands. + * usually involves the usage of the controller's reference/estimated interfaces by the other + * controllers * * \returns true if mode is switched successfully and false if not. */ @@ -254,6 +254,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; + /// A method to enable the useage of references from subscribers + /** + * Enable or disable the retrieval of references for a chainable controller. This methods helps + * to toggle between the usage of reference from the subscribers or the external interfaces to + * avoid potential concurrency in input commands + * + * @return true if the toggle is successful and false if not + */ + CONTROLLER_INTERFACE_PUBLIC + virtual bool toggle_references_from_subscribers(bool enable) = 0; + protected: std::vector command_interfaces_; std::vector state_interfaces_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 05de731a73..347921f583 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -30,7 +30,7 @@ return_type ChainableControllerInterface::update( { return_type ret = return_type::ERROR; - if (!is_in_chained_mode()) + if (use_references_from_subscribers_) { ret = update_reference_from_subscribers(time, period); if (ret != return_type::OK) @@ -149,6 +149,25 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; } +bool ChainableControllerInterface::toggle_references_from_subscribers(bool enable) +{ + bool result = false; + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + use_references_from_subscribers_ = enable; + result = true; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Can not toggle the controller's references between subscribers and interfaces because it is " + "no in '%s' state. Current state is '%s'.", + hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str()); + } + return result; +} + bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } } // namespace controller_interface diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 88fa1c3ed2..40a74195a5 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -42,4 +42,6 @@ bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false bool ControllerInterface::is_in_chained_mode() const { return false; } +bool ControllerInterface::toggle_references_from_subscribers(bool /*enable*/) { return false; } + } // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 0f3c30bf99..7373c58f01 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -178,6 +178,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); + EXPECT_TRUE(controller.toggle_references_from_subscribers(true)); EXPECT_FALSE(controller.is_in_chained_mode()); // call update and update it from subscriber because not in chained mode @@ -206,6 +207,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.reference_interfaces_[0] = 0.0; EXPECT_TRUE(controller.set_chained_mode(true)); + EXPECT_TRUE(controller.toggle_references_from_subscribers(false)); EXPECT_TRUE(controller.is_in_chained_mode()); // Provoke error in update from subscribers - return OK because update of subscribers is not used From 006681b0b637c74b4a50513661a066177d14e0fa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:11:55 +0200 Subject: [PATCH 17/61] Added some utility methods in the controller_manager --- controller_manager/src/controller_manager.cpp | 50 ++++++++++++++++++- 1 file changed, 49 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9a2e02fac7..2dad884b26 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -132,6 +132,55 @@ bool is_interface_a_chained_interface( return true; } +bool is_interface_exported_from_controller( + const std::string & interface_name, const std::string & controller_name) +{ + auto split_pos = interface_name.find_first_of('/'); + if (split_pos == std::string::npos) // '/' exist in the string (should be always false) + { + RCLCPP_FATAL( + rclcpp::get_logger("ControllerManager::utils"), + "Character '/', was not find in the interface name '%s'. This should never happen. " + "Stop the controller manager immediately and restart it.", + interface_name.c_str()); + throw std::runtime_error("Mismatched interface name. See the FATAL message above."); + } + + auto interface_prefix = interface_name.substr(0, split_pos); + if (controller_name.compare(interface_prefix) == 0) + return true; + else + return false; +} + +bool is_controller_estimated_interfaces_inuse_by_other_controllers( + const std::string & controller_name, + const std::vector & controllers, + std::vector blacklist) +{ + for (const auto & controller : controllers) + { + auto blacklist_it = std::find(blacklist.begin(), blacklist.end(), controller.info.name); + if (blacklist_it != blacklist.end() || controller_name.compare(controller.info.name) == 0) + { + continue; + } + if (!is_controller_active(controller.c)) + { + continue; + } + auto controller_state_interfaces = controller.c->state_interface_configuration().names; + for (const auto & ctrl_itf_name : controller_state_interfaces) + { + if (is_interface_exported_from_controller(ctrl_itf_name, controller_name)) + { + return true; + } + } + } + return false; +} + template void add_element_to_list(std::vector & list, const T & element) { @@ -168,7 +217,6 @@ void controller_chain_spec_cleanup( } ctrl_chain_spec.erase(controller); } - } // namespace namespace controller_manager From 9c1218d2d374fbcdd0d271bdf0144d7b36dfe6c9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:18:01 +0200 Subject: [PATCH 18/61] import exported interfaces without conditioning --- controller_manager/src/controller_manager.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2dad884b26..39b202cf5a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -744,11 +744,9 @@ controller_interface::return_type ControllerManager::configure_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } - if (!ref_interfaces.empty()) - resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); - if (!estimated_interfaces.empty()) - resource_manager_->import_controller_estimated_interfaces( - controller_name, estimated_interfaces); + resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); + resource_manager_->import_controller_estimated_interfaces( + controller_name, estimated_interfaces); } // let's update the list of following and preceding controllers From 61b337ffaaa6f6f9baf159d91748a5c67940b9f4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:25:40 +0200 Subject: [PATCH 19/61] Added a new method to be able to toggle the controller references between interfaces and subscribers --- .../controller_manager/controller_manager.hpp | 11 ++++++ controller_manager/src/controller_manager.cpp | 38 +++++++++++++++++++ 2 files changed, 49 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 9a709c5f8f..068fd2ffc8 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -390,6 +390,16 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + ///Sets if the parsed vector of controller's reference interfacs are available to use or not + /** + * Sets the availability status of the reference interfaces of the given list of controllers + * @param[in] controllers list + * @param[in] available - To specify they are needed to be available or unavailable for other + * controllers to use + */ + void set_controllers_reference_interfaces_availability( + const std::vector & controllers, bool available); + /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. @@ -543,6 +553,7 @@ class ControllerManager : public rclcpp::Node std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; + std::vector to_use_references_from_subscribers_; std::vector activate_command_interface_request_, deactivate_command_interface_request_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 39b202cf5a..bf9be7ac8a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -836,6 +836,7 @@ void ControllerManager::clear_requests() } to_chained_mode_request_.clear(); from_chained_mode_request_.clear(); + to_use_references_from_subscribers_.clear(); activate_command_interface_request_.clear(); deactivate_command_interface_request_.clear(); } @@ -2611,4 +2612,41 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( return controller_node_options; } +void ControllerManager::set_controllers_reference_interfaces_availability( + const std::vector & controllers, bool available) +{ + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + + for (const auto & request : controllers) + { + auto found_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, request)); + if (found_it == rt_controller_list.end()) + { + RCLCPP_FATAL( + get_logger(), + "Got request to turn %s reference interfaces of controller '%s', but controller is not in " + "the realtime controller list. (This should never happen!)", + (available ? "ON" : "OFF"), request.c_str()); + continue; + } + auto controller = found_it->c; + if (!is_controller_active(*controller)) + { + if (available) + { + controller->toggle_references_from_subscribers(false); + resource_manager_->make_controller_reference_interfaces_available(request); + } + else + { + controller->toggle_references_from_subscribers(true); + resource_manager_->make_controller_reference_interfaces_unavailable(request); + } + } + } +}; + } // namespace controller_manager From 4a9081470b44498690223a03641dc80730467177 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:27:24 +0200 Subject: [PATCH 20/61] activate and deactivate the controller when switching the references between subscribers and interfaces --- controller_manager/src/controller_manager.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index bf9be7ac8a..047e3a1648 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1072,6 +1072,12 @@ controller_interface::return_type ControllerManager::switch_controller( from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); + auto to_use_ref_from_sub_it = std::find( + to_use_references_from_subscribers_.begin(), to_use_references_from_subscribers_.end(), + controller.info.name); + bool in_to_use_ref_from_sub_list = + to_use_ref_from_sub_it != to_use_references_from_subscribers_.end(); + auto deactivate_list_it = std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); bool in_deactivate_list = deactivate_list_it != deactivate_request_.end(); @@ -1080,7 +1086,7 @@ controller_interface::return_type ControllerManager::switch_controller( const bool is_inactive = is_controller_inactive(*controller.c); // restart controllers that need to switch their 'chained mode' - add to (de)activate lists - if (in_to_chained_mode_list || in_from_chained_mode_list) + if (in_to_chained_mode_list || in_from_chained_mode_list || in_to_use_ref_from_sub_list) { if (is_active && !in_deactivate_list) { From 2fa39f5278ef96c8e13b20ee77dcb2a3d41e2fe5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:33:51 +0200 Subject: [PATCH 21/61] in to_chained_mode enable references from interfaces and make all interfaces available, and only release estimated interfaces in from_chained_mode --- controller_manager/src/controller_manager.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 047e3a1648..d4f1147ef2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -832,6 +832,7 @@ void ControllerManager::clear_requests() // state without the controller being in active state for (const auto & controller_name : to_chained_mode_request_) { + resource_manager_->make_controller_estimated_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); @@ -1399,6 +1400,8 @@ void ControllerManager::deactivate_controllers( // if it is a chainable controller, make the reference interfaces unavailable on deactivation if (controller->is_chainable()) { + controller->toggle_references_from_subscribers(true); + resource_manager_->make_controller_estimated_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -1575,6 +1578,10 @@ void ControllerManager::activate_controllers( // if it is a chainable controller, make the reference interfaces available on activation if (controller->is_chainable()) { + // enable references from the controller interfaces + controller->toggle_references_from_subscribers(false); + // make all the exported interfaces of the controller available + resource_manager_->make_controller_estimated_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } @@ -2370,6 +2377,9 @@ controller_interface::return_type ControllerManager::check_following_controllers to_chained_mode_request_.push_back(following_ctrl_it->info.name); // if it is a chainable controller, make the reference interfaces available on preactivation // (This is needed when you activate a couple of chainable controller altogether) + // make all the exported interfaces of the controller available + resource_manager_->make_controller_estimated_interfaces_available( + following_ctrl_it->info.name); resource_manager_->make_controller_reference_interfaces_available( following_ctrl_it->info.name); RCLCPP_DEBUG( From 47d3b3c30af2010b5bf4cbab7121f6a6a5d12391 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:36:10 +0200 Subject: [PATCH 22/61] check all exported interfaces of following actuators when activating the controllers --- controller_manager/src/controller_manager.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d4f1147ef2..8363b15edc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2295,11 +2295,16 @@ controller_interface::return_type ControllerManager::check_following_controllers get_logger(), "Checking following controllers of preceding controller with name '%s'.", controller_it->info.name.c_str()); - for (const auto & cmd_itf_name : controller_it->c->command_interface_configuration().names) + auto controller_interfaces = controller_it->c->command_interface_configuration().names; + auto controller_state_interfaces = controller_it->c->state_interface_configuration().names; + controller_interfaces.insert( + controller_interfaces.end(), controller_state_interfaces.begin(), + controller_state_interfaces.end()); + for (const auto & ctrl_itf_name : controller_interfaces) { ControllersListIterator following_ctrl_it; // Check if interface if reference interface and following controller exist. - if (!is_interface_a_chained_interface(cmd_itf_name, controllers, following_ctrl_it)) + if (!is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { continue; } @@ -2319,9 +2324,9 @@ controller_interface::return_type ControllerManager::check_following_controllers { RCLCPP_WARN( get_logger(), - "No reference interface '%s' exist, since the following controller with name '%s' " - "is not chainable.", - cmd_itf_name.c_str(), following_ctrl_it->info.name.c_str()); + "No estimated/reference interface '%s' exist, since the following controller with name " + "'%s' is not chainable.", + ctrl_itf_name.c_str(), following_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } From da4097b82b6d8f8933149b938051b03787ebcc0d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:39:42 +0200 Subject: [PATCH 23/61] Add the proper to_use_references_from_subscribers_ list generation with propagation of the deactivate controller list --- controller_manager/src/controller_manager.cpp | 49 ++++++++++++++----- 1 file changed, 37 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8363b15edc..3901621e19 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2262,23 +2262,48 @@ void ControllerManager::propagate_deactivation_of_chained_mode( break; } - for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) + const auto ctrl_cmd_itf_names = controller.c->command_interface_configuration().names; + const auto ctrl_state_itf_names = controller.c->state_interface_configuration().names; + auto ctrl_itf_names = ctrl_cmd_itf_names; + ctrl_itf_names.insert( + ctrl_itf_names.end(), ctrl_state_itf_names.begin(), ctrl_state_itf_names.end()); + for (const auto & ctrl_itf_name : ctrl_itf_names) { // controller that 'cmd_tf_name' belongs to ControllersListIterator following_ctrl_it; - if (is_interface_a_chained_interface(cmd_itf_name, controllers, following_ctrl_it)) + if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { - // currently iterated "controller" is preceding controller --> add following controller - // with matching interface name to "from" chained mode list (if not already in it) - if ( - std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name) == from_chained_mode_request_.end()) + // If the preceding controller's estimated interfaces are in use by other controllers, then maintain the chained mode + if (is_controller_estimated_interfaces_inuse_by_other_controllers( + following_ctrl_it->info.name, controllers, deactivate_request_)) { - from_chained_mode_request_.push_back(following_ctrl_it->info.name); - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'from chained mode' request.", - following_ctrl_it->info.name.c_str()); + auto found_it = std::find( + to_use_references_from_subscribers_.begin(), + to_use_references_from_subscribers_.end(), following_ctrl_it->info.name); + if (found_it == to_use_references_from_subscribers_.end()) + { + // In this case we check if the interface is state only and then add to use references_from_subscribers list + to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'use references from subscriber' request.", + following_ctrl_it->info.name.c_str()); + } + } + else + { + // currently iterated "controller" is preceding controller --> add following controller + // with matching interface name to "from" chained mode list (if not already in it) + if ( + std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), + following_ctrl_it->info.name) == from_chained_mode_request_.end()) + { + from_chained_mode_request_.push_back(following_ctrl_it->info.name); + to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'from chained mode' request.", + following_ctrl_it->info.name.c_str()); + } } } } From 295fcce347bc4b79f25e54d45a294877bb341633 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:45:09 +0200 Subject: [PATCH 24/61] Check if all the exported interfaces being utilized by other preceding controllers --- controller_manager/src/controller_manager.cpp | 114 ++++++++++-------- 1 file changed, 63 insertions(+), 51 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3901621e19..87978b7c81 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2461,63 +2461,75 @@ controller_interface::return_type ControllerManager::check_preceeding_controller get_logger(), "Checking preceding controller of following controller with name '%s'.", controller_it->info.name.c_str()); - for (const auto & ref_itf_name : - resource_manager_->get_controller_reference_interface_names(controller_it->info.name)) + const auto ctrl_ref_itfs = + resource_manager_->get_controller_reference_interface_names(controller_it->info.name); + const auto ctrl_estim_itfs = + resource_manager_->get_controller_estimated_interface_names(controller_it->info.name); + for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_estim_itfs}) { - std::vector preceding_controllers_using_ref_itf; - - // TODO(destogl): This data could be cached after configuring controller into a map for faster - // access here - for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); - ++preceding_ctrl_it) + for (const auto & ref_itf_name : controller_interfaces) { - const auto preceding_ctrl_cmd_itfs = - preceding_ctrl_it->c->command_interface_configuration().names; + std::vector preceding_controllers_using_ref_itf; - // if controller is not preceding go the next one - if ( - std::find(preceding_ctrl_cmd_itfs.begin(), preceding_ctrl_cmd_itfs.end(), ref_itf_name) == - preceding_ctrl_cmd_itfs.end()) + // TODO(destogl): This data could be cached after configuring controller into a map for faster + // access here + for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); + ++preceding_ctrl_it) { - continue; - } + const auto preceding_ctrl_cmd_itfs = + preceding_ctrl_it->c->command_interface_configuration().names; + const auto preceding_ctrl_state_itfs = + preceding_ctrl_it->c->state_interface_configuration().names; - // check if preceding controller will be activated - if ( - is_controller_inactive(preceding_ctrl_it->c) && - std::find( - activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != - activate_request_.end()) - { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' will be activated. ", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; - } - // check if preceding controller will not be deactivated - else if ( - is_controller_active(preceding_ctrl_it->c) && - std::find( - deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) == - deactivate_request_.end()) - { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' is active and will not be deactivated.", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; + // if controller is not preceding go the next one + if ( + (std::find( + preceding_ctrl_cmd_itfs.begin(), preceding_ctrl_cmd_itfs.end(), ref_itf_name) == + preceding_ctrl_cmd_itfs.end()) && + (std::find( + preceding_ctrl_state_itfs.begin(), preceding_ctrl_state_itfs.end(), ref_itf_name) == + preceding_ctrl_state_itfs.end())) + { + continue; + } + + // check if preceding controller will be activated + if ( + is_controller_inactive(preceding_ctrl_it->c) && + std::find( + activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != + activate_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' will be activated. ", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + // check if preceding controller will not be deactivated + else if ( + is_controller_active(preceding_ctrl_it->c) && + std::find( + deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) == + deactivate_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' is active and will not be deactivated.", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + // TODO(destogl): this should be discussed how to it the best - just a placeholder for now + // else if ( + // strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) + // { + // // insert to the begin of activate request list to be activated before preceding controller + // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); + // } } - // TODO(destogl): this should be discussed how to it the best - just a placeholder for now - // else if ( - // strictness == - // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) - // { - // // insert to the begin of activate request list to be activated before preceding controller - // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); - // } } } return controller_interface::return_type::OK; From 61b1a8a88a096d76a00b24e838ad6b341d36d594 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 May 2023 23:55:56 +0200 Subject: [PATCH 25/61] Add estimated_interfaces field to the ControllerState msg and fill-in appropriate data --- controller_manager/src/controller_manager.cpp | 9 +++++++++ controller_manager_msgs/msg/ControllerState.msg | 1 + 2 files changed, 10 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 87978b7c81..488db25a4b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1676,13 +1676,22 @@ void ControllerManager::list_controllers_srv_cb( { auto references = resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); + auto estimated_interfaces = + resource_manager_->get_controller_estimated_interface_names(controllers[i].info.name); controller_state.reference_interfaces.reserve(references.size()); + controller_state.estimated_interfaces.reserve(estimated_interfaces.size()); for (const auto & reference : references) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = reference.substr(prefix_name.size() + 1); controller_state.reference_interfaces.push_back(interface_name); } + for (const auto & estimate : estimated_interfaces) + { + const std::string prefix_name = controllers[i].c->get_node()->get_name(); + const std::string interface_name = estimate.substr(prefix_name.size() + 1); + controller_state.reference_interfaces.push_back(interface_name); + } } } response->controller.push_back(controller_state); diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index 3b8e032541..ed0c64dd4c 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -6,5 +6,6 @@ string[] required_command_interfaces # command interfaces required by con string[] required_state_interfaces # state interfaces required by controller bool is_chainable # specifies whether or not controller can export references for a controller chain bool is_chained # specifies whether or not controller's exported references are claimed by another controller +string[] estimated_interfaces # estimated interfaces to be exported (only applicable if is_chainable is true) string[] reference_interfaces # references to be exported (only applicable if is_chainable is true) ChainConnection[] chain_connections # specifies list of controllers and their exported references that the controller is chained to From a016f2d282c177d858d00338fb091f35f6d9dfd3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 May 2023 10:20:43 +0200 Subject: [PATCH 26/61] set the controllers references from interface or subscribers in manage switch method --- controller_manager/src/controller_manager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 488db25a4b..ac44ff3130 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2071,6 +2071,7 @@ void ControllerManager::manage_switch() switch_chained_mode(to_chained_mode_request_, true); switch_chained_mode(from_chained_mode_request_, false); + set_controllers_reference_interfaces_availability(to_use_references_from_subscribers_, false); // activate controllers once the switch is fully complete if (!switch_params_.activate_asap) From e6c813840da46a70d868a6b69077634ad5972bde Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 May 2023 19:11:18 +0200 Subject: [PATCH 27/61] update the activation_switching_error_handling tests in the controller_manager --- ...llers_chaining_with_controller_manager.cpp | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) 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 e93a91ddef..c6839a7312 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -1190,17 +1190,29 @@ TEST_P( pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); ActivateAndCheckController( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); - // Verify that the other preceding controller is deactivated (diff_drive_controller_two) + // Verify that the other preceding controller is deactivated (diff_drive_controller_two) and other depending controllers are active EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller_two->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); // Deactivate the first preceding controller (diff_drive_controller) and // activate the other preceding controller (diff_drive_controller_two) switch_test_controllers( - {DIFF_DRIVE_CONTROLLER_TWO}, {DIFF_DRIVE_CONTROLLER}, test_param.strictness, - std::future_status::timeout, controller_interface::return_type::OK); + {DIFF_DRIVE_CONTROLLER_TWO}, + {DIFF_DRIVE_CONTROLLER, SENSOR_FUSION_CONTROLLER, ROBOT_LOCALIZATION_CONTROLLER, + ODOM_PUBLISHER_CONTROLLER}, + test_param.strictness, std::future_status::timeout, controller_interface::return_type::OK); // Following controllers should stay active EXPECT_EQ( @@ -1213,6 +1225,14 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller_two->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_state().id()); } TEST_P( From a032da148c0e031852303abf4b7a6969a147b24d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 May 2023 21:56:19 +0200 Subject: [PATCH 28/61] check if the activate and deactivate list is empty after all the checks --- controller_manager/src/controller_manager.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ac44ff3130..314543bfd7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1063,6 +1063,14 @@ controller_interface::return_type ControllerManager::switch_controller( } } + // Check after the check if the activate and deactivate list is empty or not + if (activate_request_.empty() && deactivate_request_.empty()) + { + RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); + clear_requests(); + return controller_interface::return_type::OK; + } + for (const auto & controller : controllers) { auto to_chained_mode_list_it = std::find( From db500d32ab24fa2342fe50fed28225003d64e985 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 May 2023 21:57:07 +0200 Subject: [PATCH 29/61] remove the controller from the use_references_from_subscribers_ list when needed by the controller to be active --- controller_manager/src/controller_manager.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 314543bfd7..0fadcc2edf 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2319,7 +2319,9 @@ void ControllerManager::propagate_deactivation_of_chained_mode( from_chained_mode_request_.push_back(following_ctrl_it->info.name); to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'from chained mode' request.", + get_logger(), + "Adding controller '%s' in 'from chained mode' and 'use references from " + "subscriber' request.", following_ctrl_it->info.name.c_str()); } } @@ -2450,6 +2452,18 @@ controller_interface::return_type ControllerManager::check_following_controllers "should stay in chained mode.", following_ctrl_it->info.name.c_str()); } + auto ref_from_sub_it = std::find( + to_use_references_from_subscribers_.begin(), to_use_references_from_subscribers_.end(), + following_ctrl_it->info.name); + if (found_it != to_use_references_from_subscribers_.end()) + { + to_use_references_from_subscribers_.erase(ref_from_sub_it); + RCLCPP_DEBUG( + get_logger(), + "Removing controller '%s' in 'use references from subscriber' request because it " + "should stay in chained mode and accept references from the active controller.", + following_ctrl_it->info.name.c_str()); + } } } return controller_interface::return_type::OK; From fc1cb5b875f82f706434d3ec84482022b87f603a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 May 2023 23:06:48 +0200 Subject: [PATCH 30/61] added some cpplint and clangformatting changes --- .../chainable_controller_interface.hpp | 13 +++--- .../controller_interface.hpp | 2 +- .../controller_interface_base.hpp | 2 +- .../controller_manager/controller_manager.hpp | 2 +- controller_manager/src/controller_manager.cpp | 15 ++++--- .../test_chainable_controller.cpp | 3 +- .../test_chainable_controller.hpp | 2 +- ...llers_chaining_with_controller_manager.cpp | 44 ++++++++++++------- .../hardware_interface/resource_manager.hpp | 4 +- 9 files changed, 51 insertions(+), 36 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 84ab1cdd0f..73d6b4d596 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -68,11 +68,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_in_chained_mode() const final; CONTROLLER_INTERFACE_PUBLIC - virtual bool toggle_references_from_subscribers(bool enable) final; + bool toggle_references_from_subscribers(bool enable) final; protected: - /// Virtual method that each chainable controller should implement to export its read-only chainable - /// interfaces. + /// Virtual method that each chainable controller should implement to export its read-only + /// chainable interfaces. /** * Each chainable controller implements this methods where all its state(read only) interfaces are * exported. The method has the same meaning as `export_estimated_interfaces` method from @@ -82,8 +82,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase */ virtual std::vector on_export_estimated_interfaces() = 0; - /// Virtual method that each chainable controller should implement to export its read/write chainable - /// interfaces. + /// Virtual method that each chainable controller should implement to export its read/write + /// chainable interfaces. /** * Each chainable controller implements this methods where all input (command) interfaces are * exported. The method has the same meaning as `export_command_interface` method from @@ -141,7 +141,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// A flag marking if a chainable controller is currently preceded by another controller. bool in_chained_mode_ = false; - /// A flag marking whether to use references from subscribers or from the interfaces as input commands + /// A flag marking whether to use references from subscribers or from the interfaces as input + /// commands bool use_references_from_subscribers_ = false; }; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 85da8f2c62..8f3e9a08f8 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -80,7 +80,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * @return false */ CONTROLLER_INTERFACE_PUBLIC - virtual bool toggle_references_from_subscribers(bool enable) final; + bool toggle_references_from_subscribers(bool enable) final; }; using ControllerInterfaceSharedPtr = std::shared_ptr; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 5915eb2b46..b6a2b9edea 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -254,7 +254,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; - /// A method to enable the useage of references from subscribers + /// A method to enable the usage of references from subscribers /** * Enable or disable the retrieval of references for a chainable controller. This methods helps * to toggle between the usage of reference from the subscribers or the external interfaces to diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 068fd2ffc8..feb85cb54c 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -390,7 +390,7 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); - ///Sets if the parsed vector of controller's reference interfacs are available to use or not + /// Sets if the parsed vector of controller's reference interfacs are available to use or not /** * Sets the availability status of the reference interfaces of the given list of controllers * @param[in] controllers list diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0fadcc2edf..d1c70e8223 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -84,9 +84,9 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const /// Checks if an interface belongs to a controller based on its prefix. /** - * A State/Command interface can be provided by a controller in which case is called "estimated/reference" - * interface. - * This means that the @interface_name starts with the name of a controller. + * A State/Command interface can be provided by a controller in which case is called + * "estimated/reference" interface. This means that the @interface_name starts with the name of a + * controller. * * \param[in] interface_name to be found in the map. * \param[in] controllers list of controllers to compare their names to interface's prefix. @@ -2291,7 +2291,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode( ControllersListIterator following_ctrl_it; if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { - // If the preceding controller's estimated interfaces are in use by other controllers, then maintain the chained mode + // If the preceding controller's estimated interfaces are in use by other controllers, + // then maintain the chained mode if (is_controller_estimated_interfaces_inuse_by_other_controllers( following_ctrl_it->info.name, controllers, deactivate_request_)) { @@ -2300,7 +2301,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode( to_use_references_from_subscribers_.end(), following_ctrl_it->info.name); if (found_it == to_use_references_from_subscribers_.end()) { - // In this case we check if the interface is state only and then add to use references_from_subscribers list + // In this case we check if the interface is state only and then add to use + // references_from_subscribers list to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'use references from subscriber' request.", @@ -2558,7 +2560,8 @@ controller_interface::return_type ControllerManager::check_preceeding_controller // strictness == // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) // { - // // insert to the begin of activate request list to be activated before preceding controller + // // insert to the begin of activate request list to be activated before preceding + // controller // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); // } } diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index 6e92b13ad6..f9cbd75657 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -108,7 +108,8 @@ controller_interface::return_type TestChainableController::update_and_write_comm { estimated_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; } - // If there is no command interface and if there is a state interface then just forward the same value as in the state interface + // If there is no command interface and if there is a state interface then just forward the same + // value as in the state interface for (size_t i = 0; i < estimated_interface_names_.size() && i < state_interfaces_.size() && command_interfaces_.empty(); ++i) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 0a9a06fb1a..96c961a135 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -89,7 +89,7 @@ class TestChainableController : public controller_interface::ChainableController void set_estimated_interface_names(const std::vector & estimated_interface_names); CONTROLLER_MANAGER_PUBLIC - void set_imu_sensor_name(const std::string &name); + void set_imu_sensor_name(const std::string & name); size_t internal_counter; controller_interface::InterfaceConfiguration cmd_iface_cfg_; 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 c6839a7312..ef0922cab8 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -932,7 +932,8 @@ TEST_P( ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the estimated interfaces + // SensorFusionController continues to stay in the chained mode as it is still using the estimated + // interfaces ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); @@ -942,7 +943,8 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, robot_localization_controller->get_state().id()); - // Deactivate the robot localization controller and see that the sensor fusion controller is still active but not in the chained mode + // Deactivate the robot localization controller and see that the sensor fusion controller is still + // active but not in the chained mode DeactivateAndCheckController( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 8u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); @@ -976,7 +978,8 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, robot_localization_controller->get_state().id()); - // Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active but not in the chained mode + // Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active + // but not in the chained mode DeactivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 14u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); @@ -993,8 +996,9 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, robot_localization_controller->get_state().id()); - // Deactivate the diff_drive_controller as all it's following controllers that uses it's interfaces are deactivated - DeactivateAndCheckController( + // Deactivate the diff_drive_controller as all it's following controllers that uses it's + // interfaces are deactivated + DeactivateAndCheckController( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 20u, true); EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); @@ -1194,7 +1198,8 @@ TEST_P( ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); - // Verify that the other preceding controller is deactivated (diff_drive_controller_two) and other depending controllers are active + // Verify that the other preceding controller is deactivated (diff_drive_controller_two) and other + // depending controllers are active EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller_two->get_state().id()); @@ -1498,10 +1503,10 @@ TEST_P( {controller_interface::return_type::OK, std::future_status::ready, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; - // Test switch 'from chained mode' when controllers are deactivated and possible combination of disabling - // controllers that use reference/estimated interfaces of the other controller. - // This is important to check that deactivation is not trigger irrespective of the type (reference/estimated) - // interface that is shared among the other controllers + // Test switch 'from chained mode' when controllers are deactivated and possible combination of + // disabling controllers that use reference/estimated interfaces of the other controller. This is + // important to check that deactivation is not trigger irrespective of the type + // (reference/estimated) interface that is shared among the other controllers // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate @@ -1515,7 +1520,8 @@ TEST_P( ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the estimated interfaces + // SensorFusionController continues to stay in the chained mode as it is still using the estimated + // interfaces EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); EXPECT_EQ( @@ -1525,7 +1531,8 @@ TEST_P( robot_localization_controller->get_state().id()); // DiffDrive (preceding) controller is activated --> PID controller in chained mod - // Let's try to deactivate the diff_drive_control, it should fail as there are still other controllers that use it's resources + // Let's try to deactivate the diff_drive_control, it should fail as there are still other + // controllers that use it's resources DeactivateController( DIFF_DRIVE_CONTROLLER, expected.at(test_param.strictness).return_type, expected.at(test_param.strictness).future_status); @@ -1546,8 +1553,8 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, robot_localization_controller->get_state().id()); - // Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the robot - // localization controller is still active + // Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the + // robot localization controller is still active DeactivateAndCheckController( sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 0u, true, expected.at(test_param.strictness).return_type, @@ -1566,7 +1573,8 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, robot_localization_controller->get_state().id()); - // Deactivate the robot localization controller and see that the sensor fusion controller is still active but not in the chained mode + // Deactivate the robot localization controller and see that the sensor fusion controller is still + // active but not in the chained mode DeactivateAndCheckController( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 0u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); @@ -1600,7 +1608,8 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, robot_localization_controller->get_state().id()); - // Deactivate the odometry publisher controller and now the diff_drive should continue active but not in chained mode + // Deactivate the odometry publisher controller and now the diff_drive should continue active but + // not in chained mode DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 0u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); @@ -1617,7 +1626,8 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, robot_localization_controller->get_state().id()); - // Deactivate the diff_drive_controller as all it's following controllers that uses it's interfaces are deactivated + // Deactivate the diff_drive_controller as all it's following controllers that uses it's + // interfaces are deactivated DeactivateAndCheckController( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 0u); EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 87c7634c67..a21c742c8b 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -159,8 +159,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /// Add controller's estimated interfaces to available list. /** - * Adds state interfacess of a controller with given name to the available list. This method should - * be called when a controller gets activated with chained mode turned on. That means, the + * Adds state interfacess of a controller with given name to the available list. This method + * should be called when a controller gets activated with chained mode turned on. That means, the * controller's estimated interfaces can be used by another controllers in chained architectures. * * \param[in] controller_name name of the controller which interfaces should become available. From 415cef6e76b54bb0518edc27f454eb22df27ee6a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 29 Jul 2023 17:06:02 +0200 Subject: [PATCH 31/61] renamed estimated to exported state interface --- .../chainable_controller_interface.hpp | 10 +- .../controller_interface.hpp | 2 +- .../controller_interface_base.hpp | 4 +- .../src/chainable_controller_interface.cpp | 26 +++--- .../src/controller_interface.cpp | 2 +- .../test_chainable_controller_interface.cpp | 42 +++++---- .../test_chainable_controller_interface.hpp | 22 ++--- controller_manager/src/controller_manager.cpp | 49 +++++----- .../test_chainable_controller.cpp | 32 ++++--- .../test_chainable_controller.hpp | 6 +- ...llers_chaining_with_controller_manager.cpp | 91 ++++++++++--------- .../msg/ControllerState.msg | 2 +- .../hardware_interface/resource_manager.hpp | 29 +++--- hardware_interface/src/resource_manager.cpp | 26 +++--- 14 files changed, 176 insertions(+), 167 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 73d6b4d596..822f5563da 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -56,7 +56,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_estimated_interfaces() final; + std::vector export_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; @@ -75,12 +75,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// chainable interfaces. /** * Each chainable controller implements this methods where all its state(read only) interfaces are - * exported. The method has the same meaning as `export_estimated_interfaces` method from + * exported. The method has the same meaning as `export_state_interfaces` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * * \returns list of StateInterfaces that other controller can use as their outputs. */ - virtual std::vector on_export_estimated_interfaces() = 0; + virtual std::vector on_export_state_interfaces() = 0; /// Virtual method that each chainable controller should implement to export its read/write /// chainable interfaces. @@ -131,8 +131,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for estimated interfaces - std::vector estimated_interfaces_data_; + /// Storage of values for exported state interfaces + std::vector exported_state_interfaces_data_; /// Storage of values for reference interfaces std::vector reference_interfaces_; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 8f3e9a08f8..cce5dd3f84 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -48,7 +48,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_estimated_interfaces() final; + std::vector export_state_interfaces() final; /** * Controller has no reference interfaces. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index b6a2b9edea..706a40fa46 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -230,12 +230,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_estimated_interfaces() = 0; + virtual std::vector export_state_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode - * usually involves the usage of the controller's reference/estimated interfaces by the other + * usually involves the usage of the controller's reference/state interfaces by the other * controllers * * \returns true if mode is switched successfully and false if not. diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 347921f583..8398f11955 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -45,41 +45,41 @@ return_type ChainableControllerInterface::update( } std::vector -ChainableControllerInterface::export_estimated_interfaces() +ChainableControllerInterface::export_state_interfaces() { - auto estimated_interfaces = on_export_estimated_interfaces(); - // check if the "estimated_interfaces_data_" variable is resized to number of interfaces - if (estimated_interfaces_data_.size() != estimated_interfaces.size()) + auto state_interfaces = on_export_state_interfaces(); + // check if the "state_interfaces_data_" variable is resized to number of interfaces + if (exported_state_interfaces_data_.size() != state_interfaces.size()) { // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the // framework RCLCPP_FATAL( get_node()->get_logger(), - "The internal storage for estimated values 'estimated_interfaces_data_' variable has size " - "'%zu', but it is expected to have the size '%zu' equal to the number of exported reference " - "interfaces. No reference interface will be exported. Please correct and recompile " + "The internal storage for exported state values 'state_interfaces_data_' variable has size " + "'%zu', but it is expected to have the size '%zu' equal to the number of exported state " + "interfaces. No state interface will be exported. Please correct and recompile " "the controller with name '%s' and try again.", - estimated_interfaces_data_.size(), estimated_interfaces.size(), get_node()->get_name()); - estimated_interfaces.clear(); + exported_state_interfaces_data_.size(), state_interfaces.size(), get_node()->get_name()); + state_interfaces.clear(); } // check if the names of the controller state interfaces begin with the controller's name - for (const auto & interface : estimated_interfaces) + for (const auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { RCLCPP_FATAL( get_node()->get_logger(), "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory for estimated interfaces. No estimated interface will be exported. Please " + "mandatory for state interfaces. No state interface will be exported. Please " "correct and recompile the controller with name '%s' and try again.", interface.get_name().c_str(), get_node()->get_name()); - estimated_interfaces.clear(); + state_interfaces.clear(); break; } } - return estimated_interfaces; + return state_interfaces; } std::vector diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 40a74195a5..3d82c4d59d 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,7 +28,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_estimated_interfaces() +std::vector ControllerInterface::export_state_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 7373c58f01..01add2b878 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -31,7 +31,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) EXPECT_FALSE(controller.is_in_chained_mode()); } -TEST_F(ChainableControllerInterfaceTest, export_estimated_interfaces) +TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) { TestableChainableControllerInterface controller; @@ -42,13 +42,13 @@ TEST_F(ChainableControllerInterfaceTest, export_estimated_interfaces) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - auto estimated_interfaces = controller.export_estimated_interfaces(); + auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_EQ(estimated_interfaces.size(), 1u); - EXPECT_EQ(estimated_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(estimated_interfaces[0].get_interface_name(), "test_state"); + ASSERT_EQ(exported_state_interfaces.size(), 1u); + EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state"); - EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -87,9 +87,9 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); // expect empty return because storage is not resized - controller.estimated_interfaces_data_.clear(); - auto estimated_interfaces = controller.export_estimated_interfaces(); - ASSERT_TRUE(estimated_interfaces.empty()); + controller.exported_state_interfaces_data_.clear(); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_TRUE(exported_state_interfaces.empty()); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -109,8 +109,8 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); // expect empty return because interface prefix is not equal to the node name - auto estimated_interfaces = controller.export_estimated_interfaces(); - ASSERT_TRUE(estimated_interfaces.empty()); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_TRUE(exported_state_interfaces.empty()); } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -126,14 +126,14 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); - auto estimated_interfaces = controller.export_estimated_interfaces(); - ASSERT_EQ(estimated_interfaces.size(), 1u); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 1u); EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); - EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -146,7 +146,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -186,7 +186,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -194,7 +194,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); // Provoke error from update - return ERROR, but reference interface is updated and not reduced controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); @@ -202,7 +202,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); controller.reference_interfaces_[0] = 0.0; @@ -217,7 +217,8 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], -1.0); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1); + ASSERT_EQ( + controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); // Provoke error from update - return ERROR, but reference interface is updated directly controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -226,5 +227,6 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1); + ASSERT_EQ( + controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index 298769fe44..7b8eaf7db5 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -29,8 +29,8 @@ constexpr double INTERFACE_VALUE = 1989.0; constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; -constexpr double ESTIMATED_INTERFACE_VALUE = 21833.0; -constexpr double ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; +constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0; +constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface @@ -43,8 +43,8 @@ class TestableChainableControllerInterface { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); - estimated_interfaces_data_.reserve(1); - estimated_interfaces_data_.push_back(ESTIMATED_INTERFACE_VALUE); + exported_state_interfaces_data_.reserve(1); + exported_state_interfaces_data_.push_back(EXPORTED_STATE_INTERFACE_VALUE); } controller_interface::CallbackReturn on_init() override @@ -68,14 +68,14 @@ class TestableChainableControllerInterface } // Implementation of ChainableController virtual methods - std::vector on_export_estimated_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector estimated_interfaces; + std::vector state_interfaces; - estimated_interfaces.push_back(hardware_interface::StateInterface( - name_prefix_of_interfaces_, "test_state", &estimated_interfaces_data_[0])); + state_interfaces.push_back(hardware_interface::StateInterface( + name_prefix_of_interfaces_, "test_state", &exported_state_interfaces_data_[0])); - return estimated_interfaces; + return state_interfaces; } // Implementation of ChainableController virtual methods @@ -93,7 +93,7 @@ class TestableChainableControllerInterface { if (reference_interfaces_[0] == 0.0) { - estimated_interfaces_data_[0] = ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE; + exported_state_interfaces_data_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; return true; } else @@ -123,7 +123,7 @@ class TestableChainableControllerInterface } reference_interfaces_[0] -= 1; - estimated_interfaces_data_[0] += 1; + exported_state_interfaces_data_[0] += 1; return controller_interface::return_type::OK; } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d1c70e8223..fb03c89381 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -85,7 +85,7 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const /// Checks if an interface belongs to a controller based on its prefix. /** * A State/Command interface can be provided by a controller in which case is called - * "estimated/reference" interface. This means that the @interface_name starts with the name of a + * "state/reference" interface. This means that the @interface_name starts with the name of a * controller. * * \param[in] interface_name to be found in the map. @@ -153,7 +153,7 @@ bool is_interface_exported_from_controller( return false; } -bool is_controller_estimated_interfaces_inuse_by_other_controllers( +bool is_controller_exported_state_interfaces_inuse_by_other_controllers( const std::string & controller_name, const std::vector & controllers, std::vector blacklist) @@ -169,8 +169,8 @@ bool is_controller_estimated_interfaces_inuse_by_other_controllers( { continue; } - auto controller_state_interfaces = controller.c->state_interface_configuration().names; - for (const auto & ctrl_itf_name : controller_state_interfaces) + auto controller_exported_state_interfaces = controller.c->state_interface_configuration().names; + for (const auto & ctrl_itf_name : controller_exported_state_interfaces) { if (is_interface_exported_from_controller(ctrl_itf_name, controller_name)) { @@ -733,20 +733,20 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto estimated_interfaces = controller->export_estimated_interfaces(); + auto state_interfaces = controller->export_state_interfaces(); auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && estimated_interfaces.empty()) + if (ref_interfaces.empty() && state_interfaces.empty()) { // TODO(destogl): Add test for this! RCLCPP_ERROR( get_logger(), - "Controller '%s' is chainable, but does not export any estimated or reference interfaces.", + "Controller '%s' is chainable, but does not export any state or reference interfaces.", controller_name.c_str()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); - resource_manager_->import_controller_estimated_interfaces( - controller_name, estimated_interfaces); + resource_manager_->import_controller_exported_state_interfaces( + controller_name, state_interfaces); } // let's update the list of following and preceding controllers @@ -832,7 +832,7 @@ void ControllerManager::clear_requests() // state without the controller being in active state for (const auto & controller_name : to_chained_mode_request_) { - resource_manager_->make_controller_estimated_interfaces_unavailable(controller_name); + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); @@ -1409,7 +1409,7 @@ void ControllerManager::deactivate_controllers( if (controller->is_chainable()) { controller->toggle_references_from_subscribers(true); - resource_manager_->make_controller_estimated_interfaces_unavailable(controller_name); + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -1589,7 +1589,7 @@ void ControllerManager::activate_controllers( // enable references from the controller interfaces controller->toggle_references_from_subscribers(false); // make all the exported interfaces of the controller available - resource_manager_->make_controller_estimated_interfaces_available(controller_name); + resource_manager_->make_controller_exported_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } @@ -1684,21 +1684,22 @@ void ControllerManager::list_controllers_srv_cb( { auto references = resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); - auto estimated_interfaces = - resource_manager_->get_controller_estimated_interface_names(controllers[i].info.name); + auto exported_state_interfaces = + resource_manager_->get_controller_exported_state_interface_names( + controllers[i].info.name); controller_state.reference_interfaces.reserve(references.size()); - controller_state.estimated_interfaces.reserve(estimated_interfaces.size()); + controller_state.exported_state_interfaces.reserve(exported_state_interfaces.size()); for (const auto & reference : references) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = reference.substr(prefix_name.size() + 1); controller_state.reference_interfaces.push_back(interface_name); } - for (const auto & estimate : estimated_interfaces) + for (const auto & estimate : exported_state_interfaces) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = estimate.substr(prefix_name.size() + 1); - controller_state.reference_interfaces.push_back(interface_name); + controller_state.exported_state_interfaces.push_back(interface_name); } } } @@ -2291,9 +2292,9 @@ void ControllerManager::propagate_deactivation_of_chained_mode( ControllersListIterator following_ctrl_it; if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { - // If the preceding controller's estimated interfaces are in use by other controllers, + // If the preceding controller's state interfaces are in use by other controllers, // then maintain the chained mode - if (is_controller_estimated_interfaces_inuse_by_other_controllers( + if (is_controller_exported_state_interfaces_inuse_by_other_controllers( following_ctrl_it->info.name, controllers, deactivate_request_)) { auto found_it = std::find( @@ -2371,7 +2372,7 @@ controller_interface::return_type ControllerManager::check_following_controllers { RCLCPP_WARN( get_logger(), - "No estimated/reference interface '%s' exist, since the following controller with name " + "No state/reference interface '%s' exist, since the following controller with name " "'%s' is not chainable.", ctrl_itf_name.c_str(), following_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; @@ -2430,7 +2431,7 @@ controller_interface::return_type ControllerManager::check_following_controllers // if it is a chainable controller, make the reference interfaces available on preactivation // (This is needed when you activate a couple of chainable controller altogether) // make all the exported interfaces of the controller available - resource_manager_->make_controller_estimated_interfaces_available( + resource_manager_->make_controller_exported_state_interfaces_available( following_ctrl_it->info.name); resource_manager_->make_controller_reference_interfaces_available( following_ctrl_it->info.name); @@ -2497,9 +2498,9 @@ controller_interface::return_type ControllerManager::check_preceeding_controller const auto ctrl_ref_itfs = resource_manager_->get_controller_reference_interface_names(controller_it->info.name); - const auto ctrl_estim_itfs = - resource_manager_->get_controller_estimated_interface_names(controller_it->info.name); - for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_estim_itfs}) + const auto ctrl_exp_state_itfs = + resource_manager_->get_controller_exported_state_interface_names(controller_it->info.name); + for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_exp_state_itfs}) { for (const auto & ref_itf_name : controller_interfaces) { diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index f9cbd75657..cebc1c0579 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -103,18 +103,19 @@ controller_interface::return_type TestChainableController::update_and_write_comm { command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); } - // If there is a command interface then integrate and set it to the estimated interface data - for (size_t i = 0; i < estimated_interface_names_.size() && i < command_interfaces_.size(); ++i) + // If there is a command interface then integrate and set it to the exported state interface data + for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size(); + ++i) { - estimated_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + exported_state_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; } // If there is no command interface and if there is a state interface then just forward the same // value as in the state interface - for (size_t i = 0; i < estimated_interface_names_.size() && i < state_interfaces_.size() && + for (size_t i = 0; i < exported_state_interface_names_.size() && i < state_interfaces_.size() && command_interfaces_.empty(); ++i) { - estimated_interfaces_data_[i] = state_interfaces_[i].get_value(); + exported_state_interfaces_data_[i] = state_interfaces_[i].get_value(); } return controller_interface::return_type::OK; @@ -171,17 +172,18 @@ CallbackReturn TestChainableController::on_cleanup( } std::vector -TestChainableController::on_export_estimated_interfaces() +TestChainableController::on_export_state_interfaces() { - std::vector estimated_interfaces; + std::vector state_interfaces; - for (size_t i = 0; i < estimated_interface_names_.size(); ++i) + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) { - estimated_interfaces.push_back(hardware_interface::StateInterface( - get_node()->get_name(), estimated_interface_names_[i], &estimated_interfaces_data_[i])); + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), exported_state_interface_names_[i], + &exported_state_interfaces_data_[i])); } - return estimated_interfaces; + return state_interfaces; } std::vector @@ -218,12 +220,12 @@ void TestChainableController::set_reference_interface_names( reference_interfaces_.resize(reference_interface_names.size(), 0.0); } -void TestChainableController::set_estimated_interface_names( - const std::vector & estimated_interface_names) +void TestChainableController::set_exported_state_interface_names( + const std::vector & state_interface_names) { - estimated_interface_names_ = estimated_interface_names; + exported_state_interface_names_ = state_interface_names; - estimated_interfaces_data_.resize(estimated_interface_names_.size(), 0.0); + exported_state_interfaces_data_.resize(exported_state_interface_names_.size(), 0.0); } void TestChainableController::set_imu_sensor_name(const std::string & name) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 96c961a135..2862b8e2e7 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -63,7 +63,7 @@ class TestChainableController : public controller_interface::ChainableController CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CONTROLLER_MANAGER_PUBLIC - std::vector on_export_estimated_interfaces() override; + std::vector on_export_state_interfaces() override; CONTROLLER_MANAGER_PUBLIC std::vector on_export_reference_interfaces() override; @@ -86,7 +86,7 @@ class TestChainableController : public controller_interface::ChainableController void set_reference_interface_names(const std::vector & reference_interface_names); CONTROLLER_MANAGER_PUBLIC - void set_estimated_interface_names(const std::vector & estimated_interface_names); + void set_exported_state_interface_names(const std::vector & state_interface_names); CONTROLLER_MANAGER_PUBLIC void set_imu_sensor_name(const std::string & name); @@ -95,7 +95,7 @@ class TestChainableController : public controller_interface::ChainableController controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; - std::vector estimated_interface_names_; + std::vector exported_state_interface_names_; std::unique_ptr imu_sensor_; realtime_tools::RealtimeBuffer> rt_command_ptr_; 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 ef0922cab8..166b72add1 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -157,13 +157,13 @@ class TestControllerChainingWithControllerManager diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller->set_estimated_interface_names({"odom_x", "odom_y"}); + diff_drive_controller->set_exported_state_interface_names({"odom_x", "odom_y"}); // configure Diff Drive Two controller (Has same command interfaces as Diff Drive controller) diff_drive_controller_two->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller_two->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller_two->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller_two->set_estimated_interface_names({"odom_x", "odom_y"}); + diff_drive_controller_two->set_exported_state_interface_names({"odom_x", "odom_y"}); // configure Position Tracking controller controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { @@ -184,7 +184,7 @@ class TestControllerChainingWithControllerManager // configure sensor fusion controller sensor_fusion_controller->set_imu_sensor_name("base_imu"); sensor_fusion_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); - sensor_fusion_controller->set_estimated_interface_names({"odom_x", "odom_y", "yaw"}); + sensor_fusion_controller->set_exported_state_interface_names({"odom_x", "odom_y", "yaw"}); // configure Robot Localization controller controller_interface::InterfaceConfiguration odom_ifs_cfg = { @@ -345,10 +345,10 @@ class TestControllerChainingWithControllerManager T>::type * = nullptr> void SetToChainedModeAndMakeInterfacesAvailable( std::shared_ptr & controller, const std::string & controller_name, - const std::vector & estimated_interfaces, + const std::vector & exported_state_interfaces, const std::vector & reference_interfaces) { - if (!estimated_interfaces.empty() || !reference_interfaces.empty()) + if (!exported_state_interfaces.empty() || !reference_interfaces.empty()) { controller->set_chained_mode(true); EXPECT_TRUE(controller->is_in_chained_mode()); @@ -363,11 +363,12 @@ class TestControllerChainingWithControllerManager EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } - if (!estimated_interfaces.empty()) + if (!exported_state_interfaces.empty()) { - // make estimated interface state_interfaces available - cm_->resource_manager_->make_controller_estimated_interfaces_available(controller_name); - for (const auto & interface : estimated_interfaces) + // make state interface state_interfaces available + cm_->resource_manager_->make_controller_exported_state_interfaces_available( + controller_name); + for (const auto & interface : exported_state_interfaces) { EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); @@ -500,15 +501,15 @@ class TestControllerChainingWithControllerManager ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); - EXP_ESTIMATED_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); - EXP_ESTIMATED_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(sensor_fusion_controller->estimated_interfaces_data_.size(), 3u); + EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); + EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ(sensor_fusion_controller->exported_state_interfaces_data_.size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); @@ -517,8 +518,8 @@ class TestControllerChainingWithControllerManager // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); @@ -528,8 +529,8 @@ class TestControllerChainingWithControllerManager // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); } // check data propagation through controllers and if individual controllers are working @@ -556,7 +557,7 @@ class TestControllerChainingWithControllerManager "pid_right_wheel_controller/velocity"}; const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; - const std::vector DIFF_DRIVE_ESTIMATED_INTERFACES = { + const std::vector DIFF_DRIVE_STATE_INTERFACES = { "diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}; const std::vector SENSOR_FUSION_ESIMTATED_INTERFACES = { "sensor_fusion_controller/odom_x", "sensor_fusion_controller/odom_y", @@ -588,8 +589,8 @@ class TestControllerChainingWithControllerManager double EXP_RIGHT_WHEEL_HW_STATE = 0.0; double EXP_LEFT_WHEEL_REF = 0.0; double EXP_RIGHT_WHEEL_REF = 0.0; - double EXP_ESTIMATED_ODOM_X = 0.0; - double EXP_ESTIMATED_ODOM_Y = 0.0; + double EXP_STATE_ODOM_X = 0.0; + double EXP_STATE_ODOM_Y = 0.0; // Expected behaviors struct used in chaining activation/deactivation tests struct ExpectedBehaviorStruct @@ -643,7 +644,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) SetToChainedModeAndMakeInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_ESTIMATED_INTERFACES, + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_STATE_INTERFACES, DIFF_DRIVE_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, SENSOR_FUSION_ESIMTATED_INTERFACES, {}); @@ -692,7 +693,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); - // Sensor Controller uses estimated interfaces of Diff-Drive Controller and IMU + // Sensor Controller uses exported state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 1u); ASSERT_EQ(position_tracking_controller->internal_counter, 3u); @@ -700,7 +701,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 7u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 9u); - // Robot localization Controller uses estimated interfaces of Diff-Drive Controller + // Robot localization Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); ASSERT_EQ(robot_localization_controller->internal_counter, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 3u); @@ -709,7 +710,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); - // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) @@ -758,16 +759,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(robot_localization_controller->internal_counter, 4u); odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); - EXP_ESTIMATED_ODOM_X = + EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 / dt - EXP_ESTIMATED_ODOM_Y = + EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 / dt ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -786,8 +787,8 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); // 128 - 0 EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); @@ -801,8 +802,8 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); // update all controllers at once and see that all have expected values --> also checks the order // of controller execution @@ -897,17 +898,17 @@ TEST_P( EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); } - // Sensor fusion Controller uses estimated interfaces of Diff-Drive Controller and IMU + // Sensor fusion Controller uses exported state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); - // Robot localization Controller uses estimated interfaces of sensor fusion Controller + // Robot localization Controller uses exported state interfaces of sensor fusion Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); - // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); @@ -923,7 +924,7 @@ TEST_P( // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate - // the other controllers that rely on the DiffDriveController expected interfaces + // the other controllers that rely on the DiffDriveController exported interfaces DeactivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 10u, true); @@ -932,7 +933,7 @@ TEST_P( ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the estimated + // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( @@ -1504,9 +1505,9 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; // Test switch 'from chained mode' when controllers are deactivated and possible combination of - // disabling controllers that use reference/estimated interfaces of the other controller. This is + // disabling controllers that use reference/state interfaces of the other controller. This is // important to check that deactivation is not trigger irrespective of the type - // (reference/estimated) interface that is shared among the other controllers + // (reference/state) interface that is shared among the other controllers // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate @@ -1520,7 +1521,7 @@ TEST_P( ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the estimated + // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index ed0c64dd4c..65d773bddc 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -6,6 +6,6 @@ string[] required_command_interfaces # command interfaces required by con string[] required_state_interfaces # state interfaces required by controller bool is_chainable # specifies whether or not controller can export references for a controller chain bool is_chained # specifies whether or not controller's exported references are claimed by another controller -string[] estimated_interfaces # estimated interfaces to be exported (only applicable if is_chainable is true) +string[] exported_state_interfaces # state interfaces to be exported (only applicable if is_chainable is true) string[] reference_interfaces # references to be exported (only applicable if is_chainable is true) ChainConnection[] chain_connections # specifies list of controllers and their exported references that the controller is chained to diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a21c742c8b..d01057bb4d 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -134,9 +134,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; - /// Add controllers' estimated interfaces to resource manager. + /// Add controllers' exported state interfaces to resource manager. /** - * Interface for transferring management of estimated interfaces to resource manager. + * Interface for transferring management of exported state interfaces to resource manager. * When chaining controllers, state interfaces are used by the preceding * controllers. * Therefore, they should be managed in the same way as state interface of hardware. @@ -144,30 +144,31 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] controller_name name of the controller which state interfaces are imported. * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ - void import_controller_estimated_interfaces( + void import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces); - /// Get list of estimated interface of a controller. + /// Get list of exported tate interface of a controller. /** - * Returns lists of stored state interfaces names for a controller. + * Returns lists of stored exported state interfaces names for a controller. * * \param[in] controller_name for which list of state interface names is returned. * \returns list of reference interface names. */ - std::vector get_controller_estimated_interface_names( + std::vector get_controller_exported_state_interface_names( const std::string & controller_name); - /// Add controller's estimated interfaces to available list. + /// Add controller's exported state interfaces to available list. /** * Adds state interfacess of a controller with given name to the available list. This method * should be called when a controller gets activated with chained mode turned on. That means, the - * controller's estimated interfaces can be used by another controllers in chained architectures. + * controller's exported state interfaces can be used by another controllers in chained + * architectures. * * \param[in] controller_name name of the controller which interfaces should become available. */ - void make_controller_estimated_interfaces_available(const std::string & controller_name); + void make_controller_exported_state_interfaces_available(const std::string & controller_name); - /// Remove controller's estimated interface to available list. + /// Remove controller's exported state interface to available list. /** * Removes interfaces of a controller with given name from the available list. This method should * be called when a controller gets deactivated and its reference interfaces cannot be used by @@ -175,16 +176,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * * \param[in] controller_name name of the controller which interfaces should become unavailable. */ - void make_controller_estimated_interfaces_unavailable(const std::string & controller_name); + void make_controller_exported_state_interfaces_unavailable(const std::string & controller_name); - /// Remove controllers estimated interfaces from resource manager. + /// Remove controllers exported state interfaces from resource manager. /** - * Remove state interfaces from resource manager, i.e., resource storage. + * Remove exported state interfaces from resource manager, i.e., resource storage. * The interfaces will be deleted from all internal maps and lists. * * \param[in] controller_name list of interface names that will be deleted from resource manager. */ - void remove_controller_estimated_interfaces(const std::string & controller_name); + void remove_controller_exported_state_interfaces(const std::string & controller_name); /// Add controllers' reference interfaces to resource manager. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 89c993bf1b..f49040bb02 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -747,7 +747,8 @@ class ResourceStorage std::unordered_map> hardware_used_by_controllers_; /// Mapping between controllers and list of interfaces they are using - std::unordered_map> controllers_estimated_interfaces_map_; + std::unordered_map> + controllers_exported_state_interfaces_map_; std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -885,27 +886,27 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con } // CM API: Called in "callback/slow"-thread -void ResourceManager::import_controller_estimated_interfaces( +void ResourceManager::import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); - resource_storage_->controllers_estimated_interfaces_map_[controller_name] = interface_names; + resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; } // CM API: Called in "callback/slow"-thread -std::vector ResourceManager::get_controller_estimated_interface_names( +std::vector ResourceManager::get_controller_exported_state_interface_names( const std::string & controller_name) { - return resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); + return resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); } // CM API: Called in "update"-thread -void ResourceManager::make_controller_estimated_interfaces_available( +void ResourceManager::make_controller_exported_state_interfaces_available( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->available_state_interfaces_.insert( resource_storage_->available_state_interfaces_.end(), interface_names.begin(), @@ -913,11 +914,11 @@ void ResourceManager::make_controller_estimated_interfaces_available( } // CM API: Called in "update"-thread -void ResourceManager::make_controller_estimated_interfaces_unavailable( +void ResourceManager::make_controller_exported_state_interfaces_unavailable( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); for (const auto & interface : interface_names) @@ -935,11 +936,12 @@ void ResourceManager::make_controller_estimated_interfaces_unavailable( } // CM API: Called in "callback/slow"-thread -void ResourceManager::remove_controller_estimated_interfaces(const std::string & controller_name) +void ResourceManager::remove_controller_exported_state_interfaces( + const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); - resource_storage_->controllers_estimated_interfaces_map_.erase(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.erase(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->remove_state_interfaces(interface_names); From 409cd15b36a6809d834fa231259b0f054ceecf69 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 2 Aug 2023 23:30:34 +0200 Subject: [PATCH 32/61] renamed to internal state for better semantic meaning --- .../chainable_controller_interface.hpp | 10 ++-- .../controller_interface.hpp | 2 +- .../controller_interface_base.hpp | 4 +- .../src/chainable_controller_interface.cpp | 29 +++++----- .../src/controller_interface.cpp | 3 +- .../test_chainable_controller_interface.cpp | 40 +++++++------- .../test_chainable_controller_interface.hpp | 22 ++++---- controller_manager/src/controller_manager.cpp | 55 ++++++++++--------- .../test_chainable_controller.cpp | 32 +++++------ .../test_chainable_controller.hpp | 7 ++- ...llers_chaining_with_controller_manager.cpp | 52 +++++++++--------- .../msg/ControllerState.msg | 2 +- .../hardware_interface/resource_manager.hpp | 28 +++++----- hardware_interface/src/resource_manager.cpp | 24 ++++---- 14 files changed, 157 insertions(+), 153 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 822f5563da..50de4b7e01 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -56,7 +56,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_internal_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; @@ -75,12 +75,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// chainable interfaces. /** * Each chainable controller implements this methods where all its state(read only) interfaces are - * exported. The method has the same meaning as `export_state_interfaces` method from + * exported. The method has the same meaning as `export_internal_state_interfaces` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * * \returns list of StateInterfaces that other controller can use as their outputs. */ - virtual std::vector on_export_state_interfaces() = 0; + virtual std::vector on_export_internal_state_interfaces() = 0; /// Virtual method that each chainable controller should implement to export its read/write /// chainable interfaces. @@ -131,8 +131,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for exported state interfaces - std::vector exported_state_interfaces_data_; + /// Storage of values for internal_state interfaces + std::vector internal_state_interfaces_data_; /// Storage of values for reference interfaces std::vector reference_interfaces_; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index cce5dd3f84..842d41a5e0 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -48,7 +48,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_internal_state_interfaces() final; /** * Controller has no reference interfaces. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 706a40fa46..ce6346800f 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -230,12 +230,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_internal_state_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode - * usually involves the usage of the controller's reference/state interfaces by the other + * usually involves the usage of the controller's reference/internal state interfaces by the other * controllers * * \returns true if mode is switched successfully and false if not. diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 8398f11955..9473d40178 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -45,41 +45,42 @@ return_type ChainableControllerInterface::update( } std::vector -ChainableControllerInterface::export_state_interfaces() +ChainableControllerInterface::export_internal_state_interfaces() { - auto state_interfaces = on_export_state_interfaces(); - // check if the "state_interfaces_data_" variable is resized to number of interfaces - if (exported_state_interfaces_data_.size() != state_interfaces.size()) + auto internal_state_interfaces = on_export_internal_state_interfaces(); + // check if the "internal_state_interfaces_data_" variable is resized to number of interfaces + if (internal_state_interfaces_data_.size() != internal_state_interfaces.size()) { // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the // framework RCLCPP_FATAL( get_node()->get_logger(), - "The internal storage for exported state values 'state_interfaces_data_' variable has size " - "'%zu', but it is expected to have the size '%zu' equal to the number of exported state " - "interfaces. No state interface will be exported. Please correct and recompile " + "The internal storage for internal state values 'internal_state_interfaces_data_' variable " + "has size '%zu', but it is expected to have the size '%zu' equal to the number of internal " + "state interfaces. No state interface will be exported. Please correct and recompile " "the controller with name '%s' and try again.", - exported_state_interfaces_data_.size(), state_interfaces.size(), get_node()->get_name()); - state_interfaces.clear(); + internal_state_interfaces_data_.size(), internal_state_interfaces.size(), + get_node()->get_name()); + internal_state_interfaces.clear(); } // check if the names of the controller state interfaces begin with the controller's name - for (const auto & interface : state_interfaces) + for (const auto & interface : internal_state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { RCLCPP_FATAL( get_node()->get_logger(), "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory for state interfaces. No state interface will be exported. Please " - "correct and recompile the controller with name '%s' and try again.", + "mandatory for internal state interfaces. No internal state interface will be exported. " + "Please correct and recompile the controller with name '%s' and try again.", interface.get_name().c_str(), get_node()->get_name()); - state_interfaces.clear(); + internal_state_interfaces.clear(); break; } } - return state_interfaces; + return internal_state_interfaces; } std::vector diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 3d82c4d59d..92f2441154 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,7 +28,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_state_interfaces() +std::vector +ControllerInterface::export_internal_state_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 01add2b878..c3cf7d09a5 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -31,7 +31,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) EXPECT_FALSE(controller.is_in_chained_mode()); } -TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) +TEST_F(ChainableControllerInterfaceTest, export_internal_state_interfaces) { TestableChainableControllerInterface controller; @@ -42,13 +42,13 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - auto exported_state_interfaces = controller.export_state_interfaces(); + auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_EQ(exported_state_interfaces.size(), 1u); - EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state"); + ASSERT_EQ(internal_state_interfaces.size(), 1u); + EXPECT_EQ(internal_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(internal_state_interfaces[0].get_interface_name(), "test_state"); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -87,9 +87,9 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); // expect empty return because storage is not resized - controller.exported_state_interfaces_data_.clear(); - auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_TRUE(exported_state_interfaces.empty()); + controller.internal_state_interfaces_data_.clear(); + auto internal_state_interfaces = controller.export_internal_state_interfaces(); + ASSERT_TRUE(internal_state_interfaces.empty()); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -109,8 +109,8 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); // expect empty return because interface prefix is not equal to the node name - auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_TRUE(exported_state_interfaces.empty()); + auto internal_state_interfaces = controller.export_internal_state_interfaces(); + ASSERT_TRUE(internal_state_interfaces.empty()); } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -126,14 +126,14 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); - auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_EQ(exported_state_interfaces.size(), 1u); + auto internal_state_interfaces = controller.export_internal_state_interfaces(); + ASSERT_EQ(internal_state_interfaces.size(), 1u); EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -146,7 +146,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -186,7 +186,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1); // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -194,7 +194,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1); // Provoke error from update - return ERROR, but reference interface is updated and not reduced controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); @@ -202,7 +202,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); - ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1); controller.reference_interfaces_[0] = 0.0; @@ -218,7 +218,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], -1.0); ASSERT_EQ( - controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); // Provoke error from update - return ERROR, but reference interface is updated directly controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -228,5 +228,5 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); ASSERT_EQ( - controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index 7b8eaf7db5..a47fb7f66a 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -29,8 +29,8 @@ constexpr double INTERFACE_VALUE = 1989.0; constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; -constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0; -constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; +constexpr double INTERNAL_STATE_INTERFACE_VALUE = 21833.0; +constexpr double INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface @@ -43,8 +43,8 @@ class TestableChainableControllerInterface { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); - exported_state_interfaces_data_.reserve(1); - exported_state_interfaces_data_.push_back(EXPORTED_STATE_INTERFACE_VALUE); + internal_state_interfaces_data_.reserve(1); + internal_state_interfaces_data_.push_back(INTERNAL_STATE_INTERFACE_VALUE); } controller_interface::CallbackReturn on_init() override @@ -68,14 +68,14 @@ class TestableChainableControllerInterface } // Implementation of ChainableController virtual methods - std::vector on_export_state_interfaces() override + std::vector on_export_internal_state_interfaces() override { - std::vector state_interfaces; + std::vector internal_state_interfaces; - state_interfaces.push_back(hardware_interface::StateInterface( - name_prefix_of_interfaces_, "test_state", &exported_state_interfaces_data_[0])); + internal_state_interfaces.push_back(hardware_interface::StateInterface( + name_prefix_of_interfaces_, "test_state", &internal_state_interfaces_data_[0])); - return state_interfaces; + return internal_state_interfaces; } // Implementation of ChainableController virtual methods @@ -93,7 +93,7 @@ class TestableChainableControllerInterface { if (reference_interfaces_[0] == 0.0) { - exported_state_interfaces_data_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; + internal_state_interfaces_data_[0] = INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE; return true; } else @@ -123,7 +123,7 @@ class TestableChainableControllerInterface } reference_interfaces_[0] -= 1; - exported_state_interfaces_data_[0] += 1; + internal_state_interfaces_data_[0] += 1; return controller_interface::return_type::OK; } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fb03c89381..8593e6ef26 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -85,8 +85,8 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const /// Checks if an interface belongs to a controller based on its prefix. /** * A State/Command interface can be provided by a controller in which case is called - * "state/reference" interface. This means that the @interface_name starts with the name of a - * controller. + * "internal state/reference" interface. This means that the @interface_name starts with the name of + * a controller. * * \param[in] interface_name to be found in the map. * \param[in] controllers list of controllers to compare their names to interface's prefix. @@ -153,7 +153,7 @@ bool is_interface_exported_from_controller( return false; } -bool is_controller_exported_state_interfaces_inuse_by_other_controllers( +bool is_controller_internal_state_interfaces_inuse_by_other_controllers( const std::string & controller_name, const std::vector & controllers, std::vector blacklist) @@ -169,8 +169,8 @@ bool is_controller_exported_state_interfaces_inuse_by_other_controllers( { continue; } - auto controller_exported_state_interfaces = controller.c->state_interface_configuration().names; - for (const auto & ctrl_itf_name : controller_exported_state_interfaces) + auto controller_internal_state_interfaces = controller.c->state_interface_configuration().names; + for (const auto & ctrl_itf_name : controller_internal_state_interfaces) { if (is_interface_exported_from_controller(ctrl_itf_name, controller_name)) { @@ -733,20 +733,21 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto state_interfaces = controller->export_state_interfaces(); + auto internal_state_interfaces = controller->export_internal_state_interfaces(); auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && state_interfaces.empty()) + if (ref_interfaces.empty() && internal_state_interfaces.empty()) { // TODO(destogl): Add test for this! RCLCPP_ERROR( get_logger(), - "Controller '%s' is chainable, but does not export any state or reference interfaces.", + "Controller '%s' is chainable, but does not export any internal state or reference " + "interfaces.", controller_name.c_str()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); - resource_manager_->import_controller_exported_state_interfaces( - controller_name, state_interfaces); + resource_manager_->import_controller_internal_state_interfaces( + controller_name, internal_state_interfaces); } // let's update the list of following and preceding controllers @@ -832,7 +833,7 @@ void ControllerManager::clear_requests() // state without the controller being in active state for (const auto & controller_name : to_chained_mode_request_) { - resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_internal_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); @@ -1409,7 +1410,7 @@ void ControllerManager::deactivate_controllers( if (controller->is_chainable()) { controller->toggle_references_from_subscribers(true); - resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_internal_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -1589,7 +1590,7 @@ void ControllerManager::activate_controllers( // enable references from the controller interfaces controller->toggle_references_from_subscribers(false); // make all the exported interfaces of the controller available - resource_manager_->make_controller_exported_state_interfaces_available(controller_name); + resource_manager_->make_controller_internal_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } @@ -1684,22 +1685,22 @@ void ControllerManager::list_controllers_srv_cb( { auto references = resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); - auto exported_state_interfaces = - resource_manager_->get_controller_exported_state_interface_names( + auto internal_state_interfaces = + resource_manager_->get_controller_internal_state_interface_names( controllers[i].info.name); controller_state.reference_interfaces.reserve(references.size()); - controller_state.exported_state_interfaces.reserve(exported_state_interfaces.size()); + controller_state.internal_state_interfaces.reserve(internal_state_interfaces.size()); for (const auto & reference : references) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = reference.substr(prefix_name.size() + 1); controller_state.reference_interfaces.push_back(interface_name); } - for (const auto & estimate : exported_state_interfaces) + for (const auto & estimate : internal_state_interfaces) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = estimate.substr(prefix_name.size() + 1); - controller_state.exported_state_interfaces.push_back(interface_name); + controller_state.internal_state_interfaces.push_back(interface_name); } } } @@ -2292,9 +2293,9 @@ void ControllerManager::propagate_deactivation_of_chained_mode( ControllersListIterator following_ctrl_it; if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { - // If the preceding controller's state interfaces are in use by other controllers, - // then maintain the chained mode - if (is_controller_exported_state_interfaces_inuse_by_other_controllers( + // If the preceding controller's internal state interfaces are in use by other + // controllers, then maintain the chained mode + if (is_controller_internal_state_interfaces_inuse_by_other_controllers( following_ctrl_it->info.name, controllers, deactivate_request_)) { auto found_it = std::find( @@ -2372,8 +2373,8 @@ controller_interface::return_type ControllerManager::check_following_controllers { RCLCPP_WARN( get_logger(), - "No state/reference interface '%s' exist, since the following controller with name " - "'%s' is not chainable.", + "No internal state/reference interface '%s' exist, since the following controller with " + "name '%s' is not chainable.", ctrl_itf_name.c_str(), following_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } @@ -2431,7 +2432,7 @@ controller_interface::return_type ControllerManager::check_following_controllers // if it is a chainable controller, make the reference interfaces available on preactivation // (This is needed when you activate a couple of chainable controller altogether) // make all the exported interfaces of the controller available - resource_manager_->make_controller_exported_state_interfaces_available( + resource_manager_->make_controller_internal_state_interfaces_available( following_ctrl_it->info.name); resource_manager_->make_controller_reference_interfaces_available( following_ctrl_it->info.name); @@ -2498,9 +2499,9 @@ controller_interface::return_type ControllerManager::check_preceeding_controller const auto ctrl_ref_itfs = resource_manager_->get_controller_reference_interface_names(controller_it->info.name); - const auto ctrl_exp_state_itfs = - resource_manager_->get_controller_exported_state_interface_names(controller_it->info.name); - for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_exp_state_itfs}) + const auto ctrl_int_state_itfs = + resource_manager_->get_controller_internal_state_interface_names(controller_it->info.name); + for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_int_state_itfs}) { for (const auto & ref_itf_name : controller_interfaces) { diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index cebc1c0579..c5eee3b875 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -103,19 +103,19 @@ controller_interface::return_type TestChainableController::update_and_write_comm { command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); } - // If there is a command interface then integrate and set it to the exported state interface data - for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size(); + // If there is a command interface then integrate and set it to the internal state interface data + for (size_t i = 0; i < internal_state_interface_names_.size() && i < command_interfaces_.size(); ++i) { - exported_state_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + internal_state_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; } // If there is no command interface and if there is a state interface then just forward the same // value as in the state interface - for (size_t i = 0; i < exported_state_interface_names_.size() && i < state_interfaces_.size() && + for (size_t i = 0; i < internal_state_interface_names_.size() && i < state_interfaces_.size() && command_interfaces_.empty(); ++i) { - exported_state_interfaces_data_[i] = state_interfaces_[i].get_value(); + internal_state_interfaces_data_[i] = state_interfaces_[i].get_value(); } return controller_interface::return_type::OK; @@ -172,18 +172,18 @@ CallbackReturn TestChainableController::on_cleanup( } std::vector -TestChainableController::on_export_state_interfaces() +TestChainableController::on_export_internal_state_interfaces() { - std::vector state_interfaces; + std::vector internal_state_interfaces; - for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + for (size_t i = 0; i < internal_state_interface_names_.size(); ++i) { - state_interfaces.push_back(hardware_interface::StateInterface( - get_node()->get_name(), exported_state_interface_names_[i], - &exported_state_interfaces_data_[i])); + internal_state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), internal_state_interface_names_[i], + &internal_state_interfaces_data_[i])); } - return state_interfaces; + return internal_state_interfaces; } std::vector @@ -220,12 +220,12 @@ void TestChainableController::set_reference_interface_names( reference_interfaces_.resize(reference_interface_names.size(), 0.0); } -void TestChainableController::set_exported_state_interface_names( - const std::vector & state_interface_names) +void TestChainableController::set_internal_state_interface_names( + const std::vector & internal_state_interface_names) { - exported_state_interface_names_ = state_interface_names; + internal_state_interface_names_ = internal_state_interface_names; - exported_state_interfaces_data_.resize(exported_state_interface_names_.size(), 0.0); + internal_state_interfaces_data_.resize(internal_state_interface_names_.size(), 0.0); } void TestChainableController::set_imu_sensor_name(const std::string & name) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 2862b8e2e7..8d2330ee85 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -63,7 +63,7 @@ class TestChainableController : public controller_interface::ChainableController CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CONTROLLER_MANAGER_PUBLIC - std::vector on_export_state_interfaces() override; + std::vector on_export_internal_state_interfaces() override; CONTROLLER_MANAGER_PUBLIC std::vector on_export_reference_interfaces() override; @@ -86,7 +86,8 @@ class TestChainableController : public controller_interface::ChainableController void set_reference_interface_names(const std::vector & reference_interface_names); CONTROLLER_MANAGER_PUBLIC - void set_exported_state_interface_names(const std::vector & state_interface_names); + void set_internal_state_interface_names( + const std::vector & internal_state_interface_names); CONTROLLER_MANAGER_PUBLIC void set_imu_sensor_name(const std::string & name); @@ -95,7 +96,7 @@ class TestChainableController : public controller_interface::ChainableController controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; - std::vector exported_state_interface_names_; + std::vector internal_state_interface_names_; std::unique_ptr imu_sensor_; realtime_tools::RealtimeBuffer> rt_command_ptr_; 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 166b72add1..2614aa1896 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -157,13 +157,13 @@ class TestControllerChainingWithControllerManager diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller->set_exported_state_interface_names({"odom_x", "odom_y"}); + diff_drive_controller->set_internal_state_interface_names({"odom_x", "odom_y"}); // configure Diff Drive Two controller (Has same command interfaces as Diff Drive controller) diff_drive_controller_two->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller_two->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller_two->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller_two->set_exported_state_interface_names({"odom_x", "odom_y"}); + diff_drive_controller_two->set_internal_state_interface_names({"odom_x", "odom_y"}); // configure Position Tracking controller controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { @@ -184,7 +184,7 @@ class TestControllerChainingWithControllerManager // configure sensor fusion controller sensor_fusion_controller->set_imu_sensor_name("base_imu"); sensor_fusion_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); - sensor_fusion_controller->set_exported_state_interface_names({"odom_x", "odom_y", "yaw"}); + sensor_fusion_controller->set_internal_state_interface_names({"odom_x", "odom_y", "yaw"}); // configure Robot Localization controller controller_interface::InterfaceConfiguration odom_ifs_cfg = { @@ -345,10 +345,10 @@ class TestControllerChainingWithControllerManager T>::type * = nullptr> void SetToChainedModeAndMakeInterfacesAvailable( std::shared_ptr & controller, const std::string & controller_name, - const std::vector & exported_state_interfaces, + const std::vector & internal_state_interfaces, const std::vector & reference_interfaces) { - if (!exported_state_interfaces.empty() || !reference_interfaces.empty()) + if (!internal_state_interfaces.empty() || !reference_interfaces.empty()) { controller->set_chained_mode(true); EXPECT_TRUE(controller->is_in_chained_mode()); @@ -363,12 +363,12 @@ class TestControllerChainingWithControllerManager EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } - if (!exported_state_interfaces.empty()) + if (!internal_state_interfaces.empty()) { - // make state interface state_interfaces available - cm_->resource_manager_->make_controller_exported_state_interfaces_available( + // make internal_state interface state_interfaces available + cm_->resource_manager_->make_controller_internal_state_interfaces_available( controller_name); - for (const auto & interface : exported_state_interfaces) + for (const auto & interface : internal_state_interfaces) { EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); @@ -503,7 +503,7 @@ class TestControllerChainingWithControllerManager EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(sensor_fusion_controller->exported_state_interfaces_data_.size(), 3u); + ASSERT_EQ(sensor_fusion_controller->internal_state_interfaces_data_.size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); @@ -557,7 +557,7 @@ class TestControllerChainingWithControllerManager "pid_right_wheel_controller/velocity"}; const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; - const std::vector DIFF_DRIVE_STATE_INTERFACES = { + const std::vector DIFF_DRIVE_INTERNAL_STATE_INTERFACES = { "diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}; const std::vector SENSOR_FUSION_ESIMTATED_INTERFACES = { "sensor_fusion_controller/odom_x", "sensor_fusion_controller/odom_y", @@ -644,7 +644,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) SetToChainedModeAndMakeInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_STATE_INTERFACES, + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_INTERNAL_STATE_INTERFACES, DIFF_DRIVE_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, SENSOR_FUSION_ESIMTATED_INTERFACES, {}); @@ -693,7 +693,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); - // Sensor Controller uses exported state interfaces of Diff-Drive Controller and IMU + // Sensor Controller uses internal state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 1u); ASSERT_EQ(position_tracking_controller->internal_counter, 3u); @@ -701,7 +701,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 7u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 9u); - // Robot localization Controller uses exported state interfaces of Diff-Drive Controller + // Robot localization Controller uses internal state interfaces of Diff-Drive Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); ASSERT_EQ(robot_localization_controller->internal_counter, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 3u); @@ -710,7 +710,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); - // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses internal state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) @@ -898,17 +898,17 @@ TEST_P( EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); } - // Sensor fusion Controller uses exported state interfaces of Diff-Drive Controller and IMU + // Sensor fusion Controller uses internal state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); - // Robot localization Controller uses exported state interfaces of sensor fusion Controller + // Robot localization Controller uses internal state interfaces of sensor fusion Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); - // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses internal state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); @@ -924,7 +924,7 @@ TEST_P( // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate - // the other controllers that rely on the DiffDriveController exported interfaces + // the other controllers that rely on the DiffDriveController internal state interfaces DeactivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 10u, true); @@ -933,8 +933,8 @@ TEST_P( ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the state - // interfaces + // SensorFusionController continues to stay in the chained mode as it is still using the internal + // state interfaces ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); @@ -1505,9 +1505,9 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; // Test switch 'from chained mode' when controllers are deactivated and possible combination of - // disabling controllers that use reference/state interfaces of the other controller. This is - // important to check that deactivation is not trigger irrespective of the type - // (reference/state) interface that is shared among the other controllers + // disabling controllers that use reference/internal state interfaces of the other controller. + // This is important to check that deactivation is not trigger irrespective of the type + // (reference/internal state) interface that is shared among the other controllers // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate @@ -1521,8 +1521,8 @@ TEST_P( ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the state - // interfaces + // SensorFusionController continues to stay in the chained mode as it is still using the internal + // state interfaces EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); EXPECT_EQ( diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index 65d773bddc..88b02fe3d0 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -6,6 +6,6 @@ string[] required_command_interfaces # command interfaces required by con string[] required_state_interfaces # state interfaces required by controller bool is_chainable # specifies whether or not controller can export references for a controller chain bool is_chained # specifies whether or not controller's exported references are claimed by another controller -string[] exported_state_interfaces # state interfaces to be exported (only applicable if is_chainable is true) +string[] internal_state_interfaces # internal state interfaces to be exported (only applicable if is_chainable is true) string[] reference_interfaces # references to be exported (only applicable if is_chainable is true) ChainConnection[] chain_connections # specifies list of controllers and their exported references that the controller is chained to diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index d01057bb4d..b2192a4a91 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -134,9 +134,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; - /// Add controllers' exported state interfaces to resource manager. + /// Add controllers' internal state interfaces to resource manager. /** - * Interface for transferring management of exported state interfaces to resource manager. + * Interface for transferring management of internal state interfaces to resource manager. * When chaining controllers, state interfaces are used by the preceding * controllers. * Therefore, they should be managed in the same way as state interface of hardware. @@ -144,31 +144,31 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] controller_name name of the controller which state interfaces are imported. * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ - void import_controller_exported_state_interfaces( + void import_controller_internal_state_interfaces( const std::string & controller_name, std::vector & interfaces); - /// Get list of exported tate interface of a controller. + /// Get list of internal state interface of a controller. /** - * Returns lists of stored exported state interfaces names for a controller. + * Returns lists of stored internal state interfaces names for a controller. * * \param[in] controller_name for which list of state interface names is returned. * \returns list of reference interface names. */ - std::vector get_controller_exported_state_interface_names( + std::vector get_controller_internal_state_interface_names( const std::string & controller_name); - /// Add controller's exported state interfaces to available list. + /// Add controller's internal state interfaces to available list. /** * Adds state interfacess of a controller with given name to the available list. This method * should be called when a controller gets activated with chained mode turned on. That means, the - * controller's exported state interfaces can be used by another controllers in chained + * controller's internal state interfaces can be used by another controllers in chained * architectures. * * \param[in] controller_name name of the controller which interfaces should become available. */ - void make_controller_exported_state_interfaces_available(const std::string & controller_name); + void make_controller_internal_state_interfaces_available(const std::string & controller_name); - /// Remove controller's exported state interface to available list. + /// Remove controller's internal state interface to available list. /** * Removes interfaces of a controller with given name from the available list. This method should * be called when a controller gets deactivated and its reference interfaces cannot be used by @@ -176,16 +176,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * * \param[in] controller_name name of the controller which interfaces should become unavailable. */ - void make_controller_exported_state_interfaces_unavailable(const std::string & controller_name); + void make_controller_internal_state_interfaces_unavailable(const std::string & controller_name); - /// Remove controllers exported state interfaces from resource manager. + /// Remove controllers internal state interfaces from resource manager. /** - * Remove exported state interfaces from resource manager, i.e., resource storage. + * Remove internal state interfaces from resource manager, i.e., resource storage. * The interfaces will be deleted from all internal maps and lists. * * \param[in] controller_name list of interface names that will be deleted from resource manager. */ - void remove_controller_exported_state_interfaces(const std::string & controller_name); + void remove_controller_internal_state_interfaces(const std::string & controller_name); /// Add controllers' reference interfaces to resource manager. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index f49040bb02..1ba0baa4aa 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -748,7 +748,7 @@ class ResourceStorage /// Mapping between controllers and list of interfaces they are using std::unordered_map> - controllers_exported_state_interfaces_map_; + controllers_internal_state_interfaces_map_; std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -886,27 +886,27 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con } // CM API: Called in "callback/slow"-thread -void ResourceManager::import_controller_exported_state_interfaces( +void ResourceManager::import_controller_internal_state_interfaces( const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); - resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; + resource_storage_->controllers_internal_state_interfaces_map_[controller_name] = interface_names; } // CM API: Called in "callback/slow"-thread -std::vector ResourceManager::get_controller_exported_state_interface_names( +std::vector ResourceManager::get_controller_internal_state_interface_names( const std::string & controller_name) { - return resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + return resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); } // CM API: Called in "update"-thread -void ResourceManager::make_controller_exported_state_interfaces_available( +void ResourceManager::make_controller_internal_state_interfaces_available( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->available_state_interfaces_.insert( resource_storage_->available_state_interfaces_.end(), interface_names.begin(), @@ -914,11 +914,11 @@ void ResourceManager::make_controller_exported_state_interfaces_available( } // CM API: Called in "update"-thread -void ResourceManager::make_controller_exported_state_interfaces_unavailable( +void ResourceManager::make_controller_internal_state_interfaces_unavailable( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); for (const auto & interface : interface_names) @@ -936,12 +936,12 @@ void ResourceManager::make_controller_exported_state_interfaces_unavailable( } // CM API: Called in "callback/slow"-thread -void ResourceManager::remove_controller_exported_state_interfaces( +void ResourceManager::remove_controller_internal_state_interfaces( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); - resource_storage_->controllers_exported_state_interfaces_map_.erase(controller_name); + resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_internal_state_interfaces_map_.erase(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->remove_state_interfaces(interface_names); From 32bacd952643ba74edc28715fedf07c6e85591fa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 25 Aug 2023 01:05:45 +0200 Subject: [PATCH 33/61] fix the chained_controllers adding_in_random_order test with new controllers --- ...llers_chaining_with_controller_manager.cpp | 33 ++++++++++++++----- 1 file changed, 25 insertions(+), 8 deletions(-) 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 2614aa1896..2a1b11b415 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -1672,6 +1672,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add cm_->add_controller( pid_left_wheel_controller, PID_LEFT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1684,6 +1687,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add cm_->add_controller( diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -1694,7 +1703,8 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add SetToChainedModeAndMakeInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, {}, DIFF_DRIVE_REFERENCE_INTERFACES); + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_INTERNAL_STATE_INTERFACES, + DIFF_DRIVE_REFERENCE_INTERFACES); EXPECT_THROW( cm_->resource_manager_->make_controller_reference_interfaces_available( @@ -1722,6 +1732,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add ActivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) { @@ -1729,13 +1742,17 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } - ASSERT_EQ(diff_drive_controller->internal_counter, 3u); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + ASSERT_EQ(diff_drive_controller->internal_counter, 9u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 11u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 13u); // update controllers std::vector reference = {32.0, 128.0}; + sensor_fusion_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + robot_localization_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // update 'Position Tracking' controller for (auto & value : diff_drive_controller->reference_interfaces_) { @@ -1744,14 +1761,14 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(position_tracking_controller->internal_counter, 2u); + ASSERT_EQ(position_tracking_controller->internal_counter, 8u); ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through // update 'Diff Drive' Controller diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(diff_drive_controller->internal_counter, 4u); + ASSERT_EQ(diff_drive_controller->internal_counter, 10u); // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 @@ -1760,9 +1777,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 14u); pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 12u); // update hardware ('read' is sufficient for test hardware) cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); From 55a9e46a1138f4bc7750ac67b56dc5da83ffdc7b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 12 Mar 2024 16:07:35 +0100 Subject: [PATCH 34/61] Fix the tests after rebasing and formatting changes --- .../src/chainable_controller_interface.cpp | 7 ++++--- controller_manager/src/controller_manager.cpp | 8 ++++++-- .../test_chainable_controller.cpp | 5 ++++- ...st_controllers_chaining_with_controller_manager.cpp | 10 ++++++---- .../test/test_hardware_components/test_imu_sensor.cpp | 3 +-- 5 files changed, 21 insertions(+), 12 deletions(-) diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 9473d40178..d4c691e92d 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -162,9 +162,10 @@ bool ChainableControllerInterface::toggle_references_from_subscribers(bool enabl { RCLCPP_ERROR( get_node()->get_logger(), - "Can not toggle the controller's references between subscribers and interfaces because it is " - "no in '%s' state. Current state is '%s'.", - hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str()); + "Can not toggle the controller's references between subscribers and interfaces in '%s' " + "state. " + " Current state is '%s'.", + hardware_interface::lifecycle_state_names::ACTIVE, get_state().label().c_str()); } return result; } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8593e6ef26..e58add5e3e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -148,9 +148,13 @@ bool is_interface_exported_from_controller( auto interface_prefix = interface_name.substr(0, split_pos); if (controller_name.compare(interface_prefix) == 0) + { return true; + } else + { return false; + } } bool is_controller_internal_state_interfaces_inuse_by_other_controllers( @@ -1573,6 +1577,8 @@ void ControllerManager::activate_controllers( } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); + // enable references from the controller interfaces + controller->toggle_references_from_subscribers(false); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -1587,8 +1593,6 @@ void ControllerManager::activate_controllers( // if it is a chainable controller, make the reference interfaces available on activation if (controller->is_chainable()) { - // enable references from the controller interfaces - controller->toggle_references_from_subscribers(false); // make all the exported interfaces of the controller available resource_manager_->make_controller_internal_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index c5eee3b875..c3bc275510 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -230,7 +230,10 @@ void TestChainableController::set_internal_state_interface_names( void TestChainableController::set_imu_sensor_name(const std::string & name) { - if (!name.empty()) imu_sensor_ = std::make_unique(name); + if (!name.empty()) + { + imu_sensor_ = std::make_unique(name); + } } } // namespace test_chainable_controller 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 2a1b11b415..64972f09d3 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -399,11 +399,13 @@ class TestControllerChainingWithControllerManager { if (claimed_interfaces_from_hw) { - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)) + << "The interface : " << interface << " should be available but it is not available"; } else { - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)) + << "The interface : " << interface << " shouldn't be available but it is available"; } EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -1514,7 +1516,7 @@ TEST_P( // the other controllers that rely on the DiffDriveController expected interfaces DeactivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, - POSITION_CONTROLLER_CLAIMED_INTERFACES, 2u); + POSITION_CONTROLLER_CLAIMED_INTERFACES, 2u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); @@ -1630,7 +1632,7 @@ TEST_P( // Deactivate the diff_drive_controller as all it's following controllers that uses it's // interfaces are deactivated DeactivateAndCheckController( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 0u); + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 0u, true); EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index 16f236fb72..9997466106 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -51,8 +51,7 @@ class TestIMUSensor : public SensorInterface { if ( std::find_if( - state_interfaces.begin(), state_interfaces.end(), - [&imu_key](const auto & interface_info) + state_interfaces.begin(), state_interfaces.end(), [&imu_key](const auto & interface_info) { return interface_info.name == imu_key; }) == state_interfaces.end()) { return CallbackReturn::ERROR; From 64bfaa141b95c9a91f4e25e6925729f596231f23 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 14 Mar 2024 19:56:49 +0100 Subject: [PATCH 35/61] improve controller manager service tests stability --- .../test/test_controller_manager_srvs.cpp | 123 +++++++++--------- 1 file changed, 63 insertions(+), 60 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index f42c155e6d..00ee5cf39a 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -979,34 +979,37 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) // add controllers /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order - cm_->add_controller( - test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } // get controller list before configure auto result = call_service_and_wait(*client, request, srv_executor); @@ -1232,40 +1235,40 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) // add controllers /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order - cm_->add_controller( - test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_chained_controller_9, TEST_CHAINED_CONTROLLER_9, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - { ControllerManagerRunner cm_runner(this); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_9, TEST_CHAINED_CONTROLLER_9, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + for (auto random_ctrl : random_controllers_list) { cm_->add_controller( From 1f962318b83b7b9cae155384c647433348c36473 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 25 Mar 2024 22:39:37 +0100 Subject: [PATCH 36/61] Apply the docstring code review suggestions Co-authored-by: Bence Magyar --- .../include/controller_interface/controller_interface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 842d41a5e0..735830b0b1 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -43,7 +43,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase bool is_chainable() const final; /** - * Controller has no state interfaces. + * A non-chainable controller doesn't export any state interfaces. * * \returns empty list. */ @@ -75,7 +75,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase bool is_in_chained_mode() const final; /** - * Controller is no chainable, so cannot use references from subscribers + * A non-chainable controller cannot use references from subscribers * * @return false */ From b9e85e9125728c671a1e22f679309252cbe278d6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 25 Mar 2024 22:40:20 +0100 Subject: [PATCH 37/61] Apply suggestions of using advanced gmock features Co-authored-by: Bence Magyar --- .../test/test_chainable_controller_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index c3cf7d09a5..b54a895573 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -44,7 +44,7 @@ TEST_F(ChainableControllerInterfaceTest, export_internal_state_interfaces) auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_EQ(internal_state_interfaces.size(), 1u); + ASSERT_THAT(internal_state_interfaces, SizeIs(1)); EXPECT_EQ(internal_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(internal_state_interfaces[0].get_interface_name(), "test_state"); @@ -89,7 +89,7 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) // expect empty return because storage is not resized controller.internal_state_interfaces_data_.clear(); auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_TRUE(internal_state_interfaces.empty()); + ASSERT_THAT(internal_state_interfaces, IsEmpty()); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) From 18e787df43098a9ea2a008eea04bd219b2090e88 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 25 Mar 2024 22:44:18 +0100 Subject: [PATCH 38/61] Add using of IsEmpty and SizeIs from testing of gmock --- .../test/test_chainable_controller_interface.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index b54a895573..72af039dcc 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -16,6 +16,9 @@ #include +using ::testing::IsEmpty; +using ::testing::SizeIs; + TEST_F(ChainableControllerInterfaceTest, default_returns) { TestableChainableControllerInterface controller; From af45bcf332965d6e1a271027801b96a2db9d4fd3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 26 Mar 2024 09:46:39 +0100 Subject: [PATCH 39/61] use SizeIs and IsEmpty in the test_chainable_controller_interface tests --- .../test/test_chainable_controller_interface.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 72af039dcc..1c5b3b6c00 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -67,7 +67,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), 1u); + ASSERT_THAT(reference_interfaces, SizeIs(1)); EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); @@ -88,7 +88,7 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) // expect empty return because storage is not resized controller.reference_interfaces_.clear(); auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_TRUE(reference_interfaces.empty()); + ASSERT_THAT(reference_interfaces, IsEmpty()); // expect empty return because storage is not resized controller.internal_state_interfaces_data_.clear(); auto internal_state_interfaces = controller.export_internal_state_interfaces(); @@ -110,10 +110,10 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) // expect empty return because interface prefix is not equal to the node name auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_TRUE(reference_interfaces.empty()); + ASSERT_THAT(reference_interfaces, IsEmpty()); // expect empty return because interface prefix is not equal to the node name auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_TRUE(internal_state_interfaces.empty()); + ASSERT_THAT(internal_state_interfaces, IsEmpty()); } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -128,9 +128,9 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), 1u); + ASSERT_THAT(reference_interfaces, SizeIs(1)); auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_EQ(internal_state_interfaces.size(), 1u); + ASSERT_THAT(internal_state_interfaces, SizeIs(1)); EXPECT_FALSE(controller.is_in_chained_mode()); From 23291c5afda17ee3ceec0f78efbcf3380be06594 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 5 May 2024 19:12:29 +0200 Subject: [PATCH 40/61] Add review suggestions Co-authored-by: Dr. Denis --- .../chainable_controller_interface.hpp | 6 +++--- .../include/controller_interface/controller_interface.hpp | 2 +- .../controller_interface/controller_interface_base.hpp | 4 ++-- .../src/chainable_controller_interface.cpp | 8 +++----- controller_manager/src/controller_manager.cpp | 2 +- 5 files changed, 10 insertions(+), 12 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 50de4b7e01..a421b8f3c4 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -56,7 +56,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_internal_state_interfaces() final; + std::vector export_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; @@ -78,7 +78,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase * exported. The method has the same meaning as `export_internal_state_interfaces` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * - * \returns list of StateInterfaces that other controller can use as their outputs. + * \returns list of StateInterfaces that other controller can use as their inputs. */ virtual std::vector on_export_internal_state_interfaces() = 0; @@ -132,7 +132,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase const rclcpp::Time & time, const rclcpp::Duration & period) = 0; /// Storage of values for internal_state interfaces - std::vector internal_state_interfaces_data_; + std::vector state_interfaces_values_; /// Storage of values for reference interfaces std::vector reference_interfaces_; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 735830b0b1..02b4abd494 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -48,7 +48,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_internal_state_interfaces() final; + std::vector export_state_interfaces() final; /** * Controller has no reference interfaces. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 81edde0738..bf1cf4e7ad 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -231,12 +231,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_internal_state_interfaces() = 0; + virtual std::vector export_state_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode - * usually involves the usage of the controller's reference/internal state interfaces by the other + * usually involves the usage of the controller's reference or state interfaces by the other * controllers * * \returns true if mode is switched successfully and false if not. diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index d4c691e92d..dc24673ea4 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -45,14 +45,12 @@ return_type ChainableControllerInterface::update( } std::vector -ChainableControllerInterface::export_internal_state_interfaces() +ChainableControllerInterface::export_state_interfaces() { - auto internal_state_interfaces = on_export_internal_state_interfaces(); - // check if the "internal_state_interfaces_data_" variable is resized to number of interfaces + auto internal_state_interfaces = on_export_state_interfaces(); + // check if the "state_interfaces_values_" variable is resized to number of interfaces if (internal_state_interfaces_data_.size() != internal_state_interfaces.size()) { - // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the - // framework RCLCPP_FATAL( get_node()->get_logger(), "The internal storage for internal state values 'internal_state_interfaces_data_' variable " diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 91622e970c..a443f6e0f9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -125,7 +125,7 @@ bool is_interface_a_chained_interface( { RCLCPP_DEBUG( rclcpp::get_logger("ControllerManager::utils"), - "Required interface '%s' with prefix '%s' is not a chained interface.", + "Required interface '%s' with prefix '%s' is not a chain interface.", interface_name.c_str(), interface_prefix.c_str()); return false; From 254c56831205a83f60b42c61f3d641d6445a1c3c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 5 May 2024 19:46:47 +0200 Subject: [PATCH 41/61] renamed from internal state to the state by reverting This reverts commit 409cd15b36a6809d834fa231259b0f054ceecf69. --- .../chainable_controller_interface.hpp | 6 +- .../src/chainable_controller_interface.cpp | 25 ++++---- .../src/controller_interface.cpp | 3 +- .../test_chainable_controller_interface.cpp | 40 ++++++------- .../test_chainable_controller_interface.hpp | 22 +++---- controller_manager/src/controller_manager.cpp | 59 +++++++++---------- .../test_chainable_controller.cpp | 31 +++++----- .../test_chainable_controller.hpp | 7 +-- ...llers_chaining_with_controller_manager.cpp | 54 ++++++++--------- .../msg/ControllerState.msg | 2 +- .../hardware_interface/resource_manager.hpp | 28 ++++----- hardware_interface/src/resource_manager.cpp | 24 ++++---- 12 files changed, 148 insertions(+), 153 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index a421b8f3c4..9bbd5b6a42 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -75,12 +75,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// chainable interfaces. /** * Each chainable controller implements this methods where all its state(read only) interfaces are - * exported. The method has the same meaning as `export_internal_state_interfaces` method from + * exported. The method has the same meaning as `export_state_interfaces` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * * \returns list of StateInterfaces that other controller can use as their inputs. */ - virtual std::vector on_export_internal_state_interfaces() = 0; + virtual std::vector on_export_state_interfaces() = 0; /// Virtual method that each chainable controller should implement to export its read/write /// chainable interfaces. @@ -131,7 +131,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for internal_state interfaces + /// Storage of values for state interfaces std::vector state_interfaces_values_; /// Storage of values for reference interfaces diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index dc24673ea4..70c225d7ca 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -47,38 +47,37 @@ return_type ChainableControllerInterface::update( std::vector ChainableControllerInterface::export_state_interfaces() { - auto internal_state_interfaces = on_export_state_interfaces(); + auto state_interfaces = on_export_state_interfaces(); // check if the "state_interfaces_values_" variable is resized to number of interfaces - if (internal_state_interfaces_data_.size() != internal_state_interfaces.size()) + if (state_interfaces_values_.size() != state_interfaces.size()) { RCLCPP_FATAL( get_node()->get_logger(), - "The internal storage for internal state values 'internal_state_interfaces_data_' variable " - "has size '%zu', but it is expected to have the size '%zu' equal to the number of internal " - "state interfaces. No state interface will be exported. Please correct and recompile " + "The internal storage for exported state values 'state_interfaces_values_' variable has size " + "'%zu', but it is expected to have the size '%zu' equal to the number of exported state " + "interfaces. No state interface will be exported. Please correct and recompile " "the controller with name '%s' and try again.", - internal_state_interfaces_data_.size(), internal_state_interfaces.size(), - get_node()->get_name()); - internal_state_interfaces.clear(); + state_interfaces_values_.size(), state_interfaces.size(), get_node()->get_name()); + state_interfaces.clear(); } // check if the names of the controller state interfaces begin with the controller's name - for (const auto & interface : internal_state_interfaces) + for (const auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { RCLCPP_FATAL( get_node()->get_logger(), "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory for internal state interfaces. No internal state interface will be exported. " - "Please correct and recompile the controller with name '%s' and try again.", + "mandatory for state interfaces. No state interface will be exported. Please " + "correct and recompile the controller with name '%s' and try again.", interface.get_name().c_str(), get_node()->get_name()); - internal_state_interfaces.clear(); + state_interfaces.clear(); break; } } - return internal_state_interfaces; + return state_interfaces; } std::vector diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 92f2441154..3d82c4d59d 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,8 +28,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector -ControllerInterface::export_internal_state_interfaces() +std::vector ControllerInterface::export_state_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 1c5b3b6c00..ad3f4d7c5e 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -34,7 +34,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) EXPECT_FALSE(controller.is_in_chained_mode()); } -TEST_F(ChainableControllerInterfaceTest, export_internal_state_interfaces) +TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) { TestableChainableControllerInterface controller; @@ -45,13 +45,13 @@ TEST_F(ChainableControllerInterfaceTest, export_internal_state_interfaces) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - auto internal_state_interfaces = controller.export_internal_state_interfaces(); + auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_THAT(internal_state_interfaces, SizeIs(1)); - EXPECT_EQ(internal_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(internal_state_interfaces[0].get_interface_name(), "test_state"); + ASSERT_THAT(exported_state_interfaces, SizeIs(1)); + EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state"); - EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -90,9 +90,9 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_THAT(reference_interfaces, IsEmpty()); // expect empty return because storage is not resized - controller.internal_state_interfaces_data_.clear(); - auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_THAT(internal_state_interfaces, IsEmpty()); + controller.state_interfaces_values_.clear(); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_state_interfaces, IsEmpty()); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -112,8 +112,8 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_THAT(reference_interfaces, IsEmpty()); // expect empty return because interface prefix is not equal to the node name - auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_THAT(internal_state_interfaces, IsEmpty()); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_state_interfaces, IsEmpty()); } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -129,14 +129,14 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_THAT(reference_interfaces, SizeIs(1)); - auto internal_state_interfaces = controller.export_internal_state_interfaces(); - ASSERT_THAT(internal_state_interfaces, SizeIs(1)); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_state_interfaces, SizeIs(1)); EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); - EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -149,7 +149,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -189,7 +189,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -197,7 +197,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); // Provoke error from update - return ERROR, but reference interface is updated and not reduced controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); @@ -205,7 +205,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); - ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); controller.reference_interfaces_[0] = 0.0; @@ -221,7 +221,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], -1.0); ASSERT_EQ( - controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); // Provoke error from update - return ERROR, but reference interface is updated directly controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -231,5 +231,5 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); ASSERT_EQ( - controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index a47fb7f66a..f9684c27fd 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -29,8 +29,8 @@ constexpr double INTERFACE_VALUE = 1989.0; constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; -constexpr double INTERNAL_STATE_INTERFACE_VALUE = 21833.0; -constexpr double INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; +constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0; +constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface @@ -43,8 +43,8 @@ class TestableChainableControllerInterface { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); - internal_state_interfaces_data_.reserve(1); - internal_state_interfaces_data_.push_back(INTERNAL_STATE_INTERFACE_VALUE); + state_interfaces_values_.reserve(1); + state_interfaces_values_.push_back(EXPORTED_STATE_INTERFACE_VALUE); } controller_interface::CallbackReturn on_init() override @@ -68,14 +68,14 @@ class TestableChainableControllerInterface } // Implementation of ChainableController virtual methods - std::vector on_export_internal_state_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector internal_state_interfaces; + std::vector state_interfaces; - internal_state_interfaces.push_back(hardware_interface::StateInterface( - name_prefix_of_interfaces_, "test_state", &internal_state_interfaces_data_[0])); + state_interfaces.push_back(hardware_interface::StateInterface( + name_prefix_of_interfaces_, "test_state", &state_interfaces_values_[0])); - return internal_state_interfaces; + return state_interfaces; } // Implementation of ChainableController virtual methods @@ -93,7 +93,7 @@ class TestableChainableControllerInterface { if (reference_interfaces_[0] == 0.0) { - internal_state_interfaces_data_[0] = INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE; + state_interfaces_values_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; return true; } else @@ -123,7 +123,7 @@ class TestableChainableControllerInterface } reference_interfaces_[0] -= 1; - internal_state_interfaces_data_[0] += 1; + state_interfaces_values_[0] += 1; return controller_interface::return_type::OK; } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index afeadd3013..1385edecfa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -86,8 +86,8 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const /// Checks if an interface belongs to a controller based on its prefix. /** * A State/Command interface can be provided by a controller in which case is called - * "internal state/reference" interface. This means that the @interface_name starts with the name of - * a controller. + * "state/reference" interface. This means that the @interface_name starts with the name of a + * controller. * * \param[in] interface_name to be found in the map. * \param[in] controllers list of controllers to compare their names to interface's prefix. @@ -125,8 +125,8 @@ bool is_interface_a_chained_interface( { RCLCPP_DEBUG( rclcpp::get_logger("ControllerManager::utils"), - "Required interface '%s' with prefix '%s' is not a chain interface.", - interface_name.c_str(), interface_prefix.c_str()); + "Required interface '%s' with prefix '%s' is not a chain interface.", interface_name.c_str(), + interface_prefix.c_str()); return false; } @@ -158,7 +158,7 @@ bool is_interface_exported_from_controller( } } -bool is_controller_internal_state_interfaces_inuse_by_other_controllers( +bool is_controller_exported_state_interfaces_inuse_by_other_controllers( const std::string & controller_name, const std::vector & controllers, std::vector blacklist) @@ -174,8 +174,8 @@ bool is_controller_internal_state_interfaces_inuse_by_other_controllers( { continue; } - auto controller_internal_state_interfaces = controller.c->state_interface_configuration().names; - for (const auto & ctrl_itf_name : controller_internal_state_interfaces) + auto controller_exported_state_interfaces = controller.c->state_interface_configuration().names; + for (const auto & ctrl_itf_name : controller_exported_state_interfaces) { if (is_interface_exported_from_controller(ctrl_itf_name, controller_name)) { @@ -791,21 +791,20 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto internal_state_interfaces = controller->export_internal_state_interfaces(); + auto state_interfaces = controller->export_state_interfaces(); auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && internal_state_interfaces.empty()) + if (ref_interfaces.empty() && state_interfaces.empty()) { // TODO(destogl): Add test for this! RCLCPP_ERROR( get_logger(), - "Controller '%s' is chainable, but does not export any internal state or reference " - "interfaces.", + "Controller '%s' is chainable, but does not export any state or reference interfaces.", controller_name.c_str()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); - resource_manager_->import_controller_internal_state_interfaces( - controller_name, internal_state_interfaces); + resource_manager_->import_controller_exported_state_interfaces( + controller_name, state_interfaces); } // let's update the list of following and preceding controllers @@ -891,7 +890,7 @@ void ControllerManager::clear_requests() // state without the controller being in active state for (const auto & controller_name : to_chained_mode_request_) { - resource_manager_->make_controller_internal_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); @@ -1493,7 +1492,7 @@ void ControllerManager::deactivate_controllers( if (controller->is_chainable()) { controller->toggle_references_from_subscribers(true); - resource_manager_->make_controller_internal_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -1705,7 +1704,7 @@ void ControllerManager::activate_controllers( if (controller->is_chainable()) { // make all the exported interfaces of the controller available - resource_manager_->make_controller_internal_state_interfaces_available(controller_name); + resource_manager_->make_controller_exported_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } @@ -1800,22 +1799,22 @@ void ControllerManager::list_controllers_srv_cb( { auto references = resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); - auto internal_state_interfaces = - resource_manager_->get_controller_internal_state_interface_names( + auto exported_state_interfaces = + resource_manager_->get_controller_exported_state_interface_names( controllers[i].info.name); controller_state.reference_interfaces.reserve(references.size()); - controller_state.internal_state_interfaces.reserve(internal_state_interfaces.size()); + controller_state.exported_state_interfaces.reserve(exported_state_interfaces.size()); for (const auto & reference : references) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = reference.substr(prefix_name.size() + 1); controller_state.reference_interfaces.push_back(interface_name); } - for (const auto & estimate : internal_state_interfaces) + for (const auto & estimate : exported_state_interfaces) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = estimate.substr(prefix_name.size() + 1); - controller_state.internal_state_interfaces.push_back(interface_name); + controller_state.exported_state_interfaces.push_back(interface_name); } } } @@ -2442,9 +2441,9 @@ void ControllerManager::propagate_deactivation_of_chained_mode( ControllersListIterator following_ctrl_it; if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { - // If the preceding controller's internal state interfaces are in use by other - // controllers, then maintain the chained mode - if (is_controller_internal_state_interfaces_inuse_by_other_controllers( + // If the preceding controller's state interfaces are in use by other controllers, + // then maintain the chained mode + if (is_controller_exported_state_interfaces_inuse_by_other_controllers( following_ctrl_it->info.name, controllers, deactivate_request_)) { auto found_it = std::find( @@ -2522,8 +2521,8 @@ controller_interface::return_type ControllerManager::check_following_controllers { RCLCPP_WARN( get_logger(), - "No internal state/reference interface '%s' exist, since the following controller with " - "name '%s' is not chainable.", + "No state/reference interface '%s' exist, since the following controller with name " + "'%s' is not chainable.", ctrl_itf_name.c_str(), following_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } @@ -2581,7 +2580,7 @@ controller_interface::return_type ControllerManager::check_following_controllers // if it is a chainable controller, make the reference interfaces available on preactivation // (This is needed when you activate a couple of chainable controller altogether) // make all the exported interfaces of the controller available - resource_manager_->make_controller_internal_state_interfaces_available( + resource_manager_->make_controller_exported_state_interfaces_available( following_ctrl_it->info.name); resource_manager_->make_controller_reference_interfaces_available( following_ctrl_it->info.name); @@ -2648,9 +2647,9 @@ controller_interface::return_type ControllerManager::check_preceeding_controller const auto ctrl_ref_itfs = resource_manager_->get_controller_reference_interface_names(controller_it->info.name); - const auto ctrl_int_state_itfs = - resource_manager_->get_controller_internal_state_interface_names(controller_it->info.name); - for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_int_state_itfs}) + const auto ctrl_exp_state_itfs = + resource_manager_->get_controller_exported_state_interface_names(controller_it->info.name); + for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_exp_state_itfs}) { for (const auto & ref_itf_name : controller_interfaces) { diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index c3bc275510..ae9d14f2bf 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -103,19 +103,19 @@ controller_interface::return_type TestChainableController::update_and_write_comm { command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); } - // If there is a command interface then integrate and set it to the internal state interface data - for (size_t i = 0; i < internal_state_interface_names_.size() && i < command_interfaces_.size(); + // If there is a command interface then integrate and set it to the exported state interface data + for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size(); ++i) { - internal_state_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + state_interfaces_values_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; } // If there is no command interface and if there is a state interface then just forward the same // value as in the state interface - for (size_t i = 0; i < internal_state_interface_names_.size() && i < state_interfaces_.size() && + for (size_t i = 0; i < exported_state_interface_names_.size() && i < state_interfaces_.size() && command_interfaces_.empty(); ++i) { - internal_state_interfaces_data_[i] = state_interfaces_[i].get_value(); + state_interfaces_values_[i] = state_interfaces_[i].get_value(); } return controller_interface::return_type::OK; @@ -172,18 +172,17 @@ CallbackReturn TestChainableController::on_cleanup( } std::vector -TestChainableController::on_export_internal_state_interfaces() +TestChainableController::on_export_state_interfaces() { - std::vector internal_state_interfaces; + std::vector state_interfaces; - for (size_t i = 0; i < internal_state_interface_names_.size(); ++i) + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) { - internal_state_interfaces.push_back(hardware_interface::StateInterface( - get_node()->get_name(), internal_state_interface_names_[i], - &internal_state_interfaces_data_[i])); + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); } - return internal_state_interfaces; + return state_interfaces; } std::vector @@ -220,12 +219,12 @@ void TestChainableController::set_reference_interface_names( reference_interfaces_.resize(reference_interface_names.size(), 0.0); } -void TestChainableController::set_internal_state_interface_names( - const std::vector & internal_state_interface_names) +void TestChainableController::set_exported_state_interface_names( + const std::vector & state_interface_names) { - internal_state_interface_names_ = internal_state_interface_names; + exported_state_interface_names_ = state_interface_names; - internal_state_interfaces_data_.resize(internal_state_interface_names_.size(), 0.0); + state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); } void TestChainableController::set_imu_sensor_name(const std::string & name) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 8d2330ee85..2862b8e2e7 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -63,7 +63,7 @@ class TestChainableController : public controller_interface::ChainableController CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CONTROLLER_MANAGER_PUBLIC - std::vector on_export_internal_state_interfaces() override; + std::vector on_export_state_interfaces() override; CONTROLLER_MANAGER_PUBLIC std::vector on_export_reference_interfaces() override; @@ -86,8 +86,7 @@ class TestChainableController : public controller_interface::ChainableController void set_reference_interface_names(const std::vector & reference_interface_names); CONTROLLER_MANAGER_PUBLIC - void set_internal_state_interface_names( - const std::vector & internal_state_interface_names); + void set_exported_state_interface_names(const std::vector & state_interface_names); CONTROLLER_MANAGER_PUBLIC void set_imu_sensor_name(const std::string & name); @@ -96,7 +95,7 @@ class TestChainableController : public controller_interface::ChainableController controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; - std::vector internal_state_interface_names_; + std::vector exported_state_interface_names_; std::unique_ptr imu_sensor_; realtime_tools::RealtimeBuffer> rt_command_ptr_; 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 64972f09d3..b2f4944920 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -157,13 +157,13 @@ class TestControllerChainingWithControllerManager diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller->set_internal_state_interface_names({"odom_x", "odom_y"}); + diff_drive_controller->set_exported_state_interface_names({"odom_x", "odom_y"}); // configure Diff Drive Two controller (Has same command interfaces as Diff Drive controller) diff_drive_controller_two->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller_two->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller_two->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller_two->set_internal_state_interface_names({"odom_x", "odom_y"}); + diff_drive_controller_two->set_exported_state_interface_names({"odom_x", "odom_y"}); // configure Position Tracking controller controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { @@ -184,7 +184,7 @@ class TestControllerChainingWithControllerManager // configure sensor fusion controller sensor_fusion_controller->set_imu_sensor_name("base_imu"); sensor_fusion_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); - sensor_fusion_controller->set_internal_state_interface_names({"odom_x", "odom_y", "yaw"}); + sensor_fusion_controller->set_exported_state_interface_names({"odom_x", "odom_y", "yaw"}); // configure Robot Localization controller controller_interface::InterfaceConfiguration odom_ifs_cfg = { @@ -345,10 +345,10 @@ class TestControllerChainingWithControllerManager T>::type * = nullptr> void SetToChainedModeAndMakeInterfacesAvailable( std::shared_ptr & controller, const std::string & controller_name, - const std::vector & internal_state_interfaces, + const std::vector & exported_state_interfaces, const std::vector & reference_interfaces) { - if (!internal_state_interfaces.empty() || !reference_interfaces.empty()) + if (!exported_state_interfaces.empty() || !reference_interfaces.empty()) { controller->set_chained_mode(true); EXPECT_TRUE(controller->is_in_chained_mode()); @@ -363,12 +363,12 @@ class TestControllerChainingWithControllerManager EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } - if (!internal_state_interfaces.empty()) + if (!exported_state_interfaces.empty()) { - // make internal_state interface state_interfaces available - cm_->resource_manager_->make_controller_internal_state_interfaces_available( + // make state interface state_interfaces available + cm_->resource_manager_->make_controller_exported_state_interfaces_available( controller_name); - for (const auto & interface : internal_state_interfaces) + for (const auto & interface : exported_state_interfaces) { EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); @@ -505,7 +505,7 @@ class TestControllerChainingWithControllerManager EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(sensor_fusion_controller->internal_state_interfaces_data_.size(), 3u); + ASSERT_EQ(sensor_fusion_controller->state_interfaces_values_.size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); @@ -559,7 +559,7 @@ class TestControllerChainingWithControllerManager "pid_right_wheel_controller/velocity"}; const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; - const std::vector DIFF_DRIVE_INTERNAL_STATE_INTERFACES = { + const std::vector DIFF_DRIVE_STATE_INTERFACES = { "diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}; const std::vector SENSOR_FUSION_ESIMTATED_INTERFACES = { "sensor_fusion_controller/odom_x", "sensor_fusion_controller/odom_y", @@ -646,7 +646,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) SetToChainedModeAndMakeInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_INTERNAL_STATE_INTERFACES, + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_STATE_INTERFACES, DIFF_DRIVE_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, SENSOR_FUSION_ESIMTATED_INTERFACES, {}); @@ -695,7 +695,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); - // Sensor Controller uses internal state interfaces of Diff-Drive Controller and IMU + // Sensor Controller uses exported state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 1u); ASSERT_EQ(position_tracking_controller->internal_counter, 3u); @@ -703,7 +703,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 7u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 9u); - // Robot localization Controller uses internal state interfaces of Diff-Drive Controller + // Robot localization Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); ASSERT_EQ(robot_localization_controller->internal_counter, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 3u); @@ -712,7 +712,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); - // Odometry Publisher Controller uses internal state interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) @@ -900,17 +900,17 @@ TEST_P( EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); } - // Sensor fusion Controller uses internal state interfaces of Diff-Drive Controller and IMU + // Sensor fusion Controller uses exported state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); - // Robot localization Controller uses internal state interfaces of sensor fusion Controller + // Robot localization Controller uses exported state interfaces of sensor fusion Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); - // Odometry Publisher Controller uses internal state interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); @@ -926,7 +926,7 @@ TEST_P( // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate - // the other controllers that rely on the DiffDriveController internal state interfaces + // the other controllers that rely on the DiffDriveController exported interfaces DeactivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 10u, true); @@ -935,8 +935,8 @@ TEST_P( ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the internal - // state interfaces + // SensorFusionController continues to stay in the chained mode as it is still using the state + // interfaces ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); @@ -1507,9 +1507,9 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; // Test switch 'from chained mode' when controllers are deactivated and possible combination of - // disabling controllers that use reference/internal state interfaces of the other controller. - // This is important to check that deactivation is not trigger irrespective of the type - // (reference/internal state) interface that is shared among the other controllers + // disabling controllers that use reference/state interfaces of the other controller. This is + // important to check that deactivation is not trigger irrespective of the type + // (reference/state) interface that is shared among the other controllers // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate @@ -1523,8 +1523,8 @@ TEST_P( ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the internal - // state interfaces + // SensorFusionController continues to stay in the chained mode as it is still using the state + // interfaces EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); EXPECT_EQ( @@ -1705,7 +1705,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add SetToChainedModeAndMakeInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_INTERNAL_STATE_INTERFACES, + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_STATE_INTERFACES, DIFF_DRIVE_REFERENCE_INTERFACES); EXPECT_THROW( diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index 88b02fe3d0..65d773bddc 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -6,6 +6,6 @@ string[] required_command_interfaces # command interfaces required by con string[] required_state_interfaces # state interfaces required by controller bool is_chainable # specifies whether or not controller can export references for a controller chain bool is_chained # specifies whether or not controller's exported references are claimed by another controller -string[] internal_state_interfaces # internal state interfaces to be exported (only applicable if is_chainable is true) +string[] exported_state_interfaces # state interfaces to be exported (only applicable if is_chainable is true) string[] reference_interfaces # references to be exported (only applicable if is_chainable is true) ChainConnection[] chain_connections # specifies list of controllers and their exported references that the controller is chained to diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index b2192a4a91..d01057bb4d 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -134,9 +134,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; - /// Add controllers' internal state interfaces to resource manager. + /// Add controllers' exported state interfaces to resource manager. /** - * Interface for transferring management of internal state interfaces to resource manager. + * Interface for transferring management of exported state interfaces to resource manager. * When chaining controllers, state interfaces are used by the preceding * controllers. * Therefore, they should be managed in the same way as state interface of hardware. @@ -144,31 +144,31 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] controller_name name of the controller which state interfaces are imported. * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ - void import_controller_internal_state_interfaces( + void import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces); - /// Get list of internal state interface of a controller. + /// Get list of exported tate interface of a controller. /** - * Returns lists of stored internal state interfaces names for a controller. + * Returns lists of stored exported state interfaces names for a controller. * * \param[in] controller_name for which list of state interface names is returned. * \returns list of reference interface names. */ - std::vector get_controller_internal_state_interface_names( + std::vector get_controller_exported_state_interface_names( const std::string & controller_name); - /// Add controller's internal state interfaces to available list. + /// Add controller's exported state interfaces to available list. /** * Adds state interfacess of a controller with given name to the available list. This method * should be called when a controller gets activated with chained mode turned on. That means, the - * controller's internal state interfaces can be used by another controllers in chained + * controller's exported state interfaces can be used by another controllers in chained * architectures. * * \param[in] controller_name name of the controller which interfaces should become available. */ - void make_controller_internal_state_interfaces_available(const std::string & controller_name); + void make_controller_exported_state_interfaces_available(const std::string & controller_name); - /// Remove controller's internal state interface to available list. + /// Remove controller's exported state interface to available list. /** * Removes interfaces of a controller with given name from the available list. This method should * be called when a controller gets deactivated and its reference interfaces cannot be used by @@ -176,16 +176,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * * \param[in] controller_name name of the controller which interfaces should become unavailable. */ - void make_controller_internal_state_interfaces_unavailable(const std::string & controller_name); + void make_controller_exported_state_interfaces_unavailable(const std::string & controller_name); - /// Remove controllers internal state interfaces from resource manager. + /// Remove controllers exported state interfaces from resource manager. /** - * Remove internal state interfaces from resource manager, i.e., resource storage. + * Remove exported state interfaces from resource manager, i.e., resource storage. * The interfaces will be deleted from all internal maps and lists. * * \param[in] controller_name list of interface names that will be deleted from resource manager. */ - void remove_controller_internal_state_interfaces(const std::string & controller_name); + void remove_controller_exported_state_interfaces(const std::string & controller_name); /// Add controllers' reference interfaces to resource manager. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 1ba0baa4aa..f49040bb02 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -748,7 +748,7 @@ class ResourceStorage /// Mapping between controllers and list of interfaces they are using std::unordered_map> - controllers_internal_state_interfaces_map_; + controllers_exported_state_interfaces_map_; std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -886,27 +886,27 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con } // CM API: Called in "callback/slow"-thread -void ResourceManager::import_controller_internal_state_interfaces( +void ResourceManager::import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); - resource_storage_->controllers_internal_state_interfaces_map_[controller_name] = interface_names; + resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; } // CM API: Called in "callback/slow"-thread -std::vector ResourceManager::get_controller_internal_state_interface_names( +std::vector ResourceManager::get_controller_exported_state_interface_names( const std::string & controller_name) { - return resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); + return resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); } // CM API: Called in "update"-thread -void ResourceManager::make_controller_internal_state_interfaces_available( +void ResourceManager::make_controller_exported_state_interfaces_available( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->available_state_interfaces_.insert( resource_storage_->available_state_interfaces_.end(), interface_names.begin(), @@ -914,11 +914,11 @@ void ResourceManager::make_controller_internal_state_interfaces_available( } // CM API: Called in "update"-thread -void ResourceManager::make_controller_internal_state_interfaces_unavailable( +void ResourceManager::make_controller_exported_state_interfaces_unavailable( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); for (const auto & interface : interface_names) @@ -936,12 +936,12 @@ void ResourceManager::make_controller_internal_state_interfaces_unavailable( } // CM API: Called in "callback/slow"-thread -void ResourceManager::remove_controller_internal_state_interfaces( +void ResourceManager::remove_controller_exported_state_interfaces( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_internal_state_interfaces_map_.at(controller_name); - resource_storage_->controllers_internal_state_interfaces_map_.erase(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.erase(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->remove_state_interfaces(interface_names); From 05282e92da73a15629cf0ad867c6e6c495bb826c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 5 May 2024 20:01:35 +0200 Subject: [PATCH 42/61] remove the estimate variable from for loop --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1385edecfa..d087337d2e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1810,10 +1810,10 @@ void ControllerManager::list_controllers_srv_cb( const std::string interface_name = reference.substr(prefix_name.size() + 1); controller_state.reference_interfaces.push_back(interface_name); } - for (const auto & estimate : exported_state_interfaces) + for (const auto & state_interface : exported_state_interfaces) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); - const std::string interface_name = estimate.substr(prefix_name.size() + 1); + const std::string interface_name = state_interface.substr(prefix_name.size() + 1); controller_state.exported_state_interfaces.push_back(interface_name); } } From 08da210aa0eb290a82ba8ec4d51446a41b1c2ae8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 5 May 2024 20:06:13 +0200 Subject: [PATCH 43/61] rename toggle_references_from_subscribers to set_using_references_from_subscribers --- .../chainable_controller_interface.hpp | 2 +- .../include/controller_interface/controller_interface.hpp | 2 +- .../controller_interface/controller_interface_base.hpp | 4 ++-- .../src/chainable_controller_interface.cpp | 2 +- controller_interface/src/controller_interface.cpp | 2 +- .../test/test_chainable_controller_interface.cpp | 4 ++-- controller_manager/src/controller_manager.cpp | 8 ++++---- 7 files changed, 12 insertions(+), 12 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 9bbd5b6a42..6f20d6bd15 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -68,7 +68,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_in_chained_mode() const final; CONTROLLER_INTERFACE_PUBLIC - bool toggle_references_from_subscribers(bool enable) final; + bool set_using_references_from_subscribers(bool enable) final; protected: /// Virtual method that each chainable controller should implement to export its read-only diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 02b4abd494..0a3a1e4565 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -80,7 +80,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * @return false */ CONTROLLER_INTERFACE_PUBLIC - bool toggle_references_from_subscribers(bool enable) final; + bool set_using_references_from_subscribers(bool enable) final; }; using ControllerInterfaceSharedPtr = std::shared_ptr; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index bf1cf4e7ad..7bfaf8ce68 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -261,10 +261,10 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * to toggle between the usage of reference from the subscribers or the external interfaces to * avoid potential concurrency in input commands * - * @return true if the toggle is successful and false if not + * @return true if the setting is successful and false if not */ CONTROLLER_INTERFACE_PUBLIC - virtual bool toggle_references_from_subscribers(bool enable) = 0; + virtual bool set_using_references_from_subscribers(bool enable) = 0; protected: std::vector command_interfaces_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 70c225d7ca..4c4628bbcb 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -147,7 +147,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; } -bool ChainableControllerInterface::toggle_references_from_subscribers(bool enable) +bool ChainableControllerInterface::set_using_references_from_subscribers(bool enable) { bool result = false; if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 3d82c4d59d..4bf44daf33 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -42,6 +42,6 @@ bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false bool ControllerInterface::is_in_chained_mode() const { return false; } -bool ControllerInterface::toggle_references_from_subscribers(bool /*enable*/) { return false; } +bool ControllerInterface::set_using_references_from_subscribers(bool /*enable*/) { return false; } } // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index ad3f4d7c5e..a09d5e3ba4 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -181,7 +181,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - EXPECT_TRUE(controller.toggle_references_from_subscribers(true)); + EXPECT_TRUE(controller.set_using_references_from_subscribers(true)); EXPECT_FALSE(controller.is_in_chained_mode()); // call update and update it from subscriber because not in chained mode @@ -210,7 +210,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.reference_interfaces_[0] = 0.0; EXPECT_TRUE(controller.set_chained_mode(true)); - EXPECT_TRUE(controller.toggle_references_from_subscribers(false)); + EXPECT_TRUE(controller.set_using_references_from_subscribers(false)); EXPECT_TRUE(controller.is_in_chained_mode()); // Provoke error in update from subscribers - return OK because update of subscribers is not used diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d087337d2e..a43f9e6040 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1491,7 +1491,7 @@ void ControllerManager::deactivate_controllers( // deactivation if (controller->is_chainable()) { - controller->toggle_references_from_subscribers(true); + controller->set_using_references_from_subscribers(true); resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } @@ -1673,7 +1673,7 @@ void ControllerManager::activate_controllers( try { // enable references from the controller interfaces - controller->toggle_references_from_subscribers(false); + controller->set_using_references_from_subscribers(false); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -2880,12 +2880,12 @@ void ControllerManager::set_controllers_reference_interfaces_availability( { if (available) { - controller->toggle_references_from_subscribers(false); + controller->set_using_references_from_subscribers(false); resource_manager_->make_controller_reference_interfaces_available(request); } else { - controller->toggle_references_from_subscribers(true); + controller->set_using_references_from_subscribers(true); resource_manager_->make_controller_reference_interfaces_unavailable(request); } } From 56bb0339d519d7fea41a572c43723bb327359354 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 May 2024 22:48:48 +0200 Subject: [PATCH 44/61] Remove set_using_references_from_subscribers from the controller interface and calls from controller_manager --- .../chainable_controller_interface.hpp | 7 ------ .../controller_interface.hpp | 8 ------- .../controller_interface_base.hpp | 11 ---------- .../src/chainable_controller_interface.cpp | 22 +------------------ .../src/controller_interface.cpp | 2 -- .../test_chainable_controller_interface.cpp | 3 +-- controller_manager/src/controller_manager.cpp | 5 ----- 7 files changed, 2 insertions(+), 56 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 6f20d6bd15..b62cee3ee8 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -67,9 +67,6 @@ class ChainableControllerInterface : public ControllerInterfaceBase CONTROLLER_INTERFACE_PUBLIC bool is_in_chained_mode() const final; - CONTROLLER_INTERFACE_PUBLIC - bool set_using_references_from_subscribers(bool enable) final; - protected: /// Virtual method that each chainable controller should implement to export its read-only /// chainable interfaces. @@ -140,10 +137,6 @@ class ChainableControllerInterface : public ControllerInterfaceBase private: /// A flag marking if a chainable controller is currently preceded by another controller. bool in_chained_mode_ = false; - - /// A flag marking whether to use references from subscribers or from the interfaces as input - /// commands - bool use_references_from_subscribers_ = false; }; } // namespace controller_interface diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 0a3a1e4565..17f39c8478 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -73,14 +73,6 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase */ CONTROLLER_INTERFACE_PUBLIC bool is_in_chained_mode() const final; - - /** - * A non-chainable controller cannot use references from subscribers - * - * @return false - */ - CONTROLLER_INTERFACE_PUBLIC - bool set_using_references_from_subscribers(bool enable) final; }; using ControllerInterfaceSharedPtr = std::shared_ptr; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 7bfaf8ce68..f67fb23fa0 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -255,17 +255,6 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; - /// A method to enable the usage of references from subscribers - /** - * Enable or disable the retrieval of references for a chainable controller. This methods helps - * to toggle between the usage of reference from the subscribers or the external interfaces to - * avoid potential concurrency in input commands - * - * @return true if the setting is successful and false if not - */ - CONTROLLER_INTERFACE_PUBLIC - virtual bool set_using_references_from_subscribers(bool enable) = 0; - protected: std::vector command_interfaces_; std::vector state_interfaces_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 4c4628bbcb..803a1e67fc 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -30,7 +30,7 @@ return_type ChainableControllerInterface::update( { return_type ret = return_type::ERROR; - if (use_references_from_subscribers_) + if (!is_in_chained_mode()) { ret = update_reference_from_subscribers(time, period); if (ret != return_type::OK) @@ -147,26 +147,6 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; } -bool ChainableControllerInterface::set_using_references_from_subscribers(bool enable) -{ - bool result = false; - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - use_references_from_subscribers_ = enable; - result = true; - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Can not toggle the controller's references between subscribers and interfaces in '%s' " - "state. " - " Current state is '%s'.", - hardware_interface::lifecycle_state_names::ACTIVE, get_state().label().c_str()); - } - return result; -} - bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } } // namespace controller_interface diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 4bf44daf33..0f11bba71c 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -42,6 +42,4 @@ bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false bool ControllerInterface::is_in_chained_mode() const { return false; } -bool ControllerInterface::set_using_references_from_subscribers(bool /*enable*/) { return false; } - } // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index a09d5e3ba4..7380b222e1 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -181,7 +181,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - EXPECT_TRUE(controller.set_using_references_from_subscribers(true)); + EXPECT_FALSE(controller.set_chained_mode(false)); EXPECT_FALSE(controller.is_in_chained_mode()); // call update and update it from subscriber because not in chained mode @@ -210,7 +210,6 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.reference_interfaces_[0] = 0.0; EXPECT_TRUE(controller.set_chained_mode(true)); - EXPECT_TRUE(controller.set_using_references_from_subscribers(false)); EXPECT_TRUE(controller.is_in_chained_mode()); // Provoke error in update from subscribers - return OK because update of subscribers is not used diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a43f9e6040..a65d621232 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1491,7 +1491,6 @@ void ControllerManager::deactivate_controllers( // deactivation if (controller->is_chainable()) { - controller->set_using_references_from_subscribers(true); resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } @@ -1672,8 +1671,6 @@ void ControllerManager::activate_controllers( try { - // enable references from the controller interfaces - controller->set_using_references_from_subscribers(false); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -2880,12 +2877,10 @@ void ControllerManager::set_controllers_reference_interfaces_availability( { if (available) { - controller->set_using_references_from_subscribers(false); resource_manager_->make_controller_reference_interfaces_available(request); } else { - controller->set_using_references_from_subscribers(true); resource_manager_->make_controller_reference_interfaces_unavailable(request); } } From d2f548701fe3b0cb5f40922b362b58982015c367 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 May 2024 23:52:42 +0200 Subject: [PATCH 45/61] remove to_use_references_from_subscribers_ and cleanup set_controllers_reference_interfaces_availability method --- .../controller_manager/controller_manager.hpp | 11 --- controller_manager/src/controller_manager.cpp | 81 +------------------ 2 files changed, 3 insertions(+), 89 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index feb85cb54c..9a709c5f8f 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -390,16 +390,6 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); - /// Sets if the parsed vector of controller's reference interfacs are available to use or not - /** - * Sets the availability status of the reference interfaces of the given list of controllers - * @param[in] controllers list - * @param[in] available - To specify they are needed to be available or unavailable for other - * controllers to use - */ - void set_controllers_reference_interfaces_availability( - const std::vector & controllers, bool available); - /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. @@ -553,7 +543,6 @@ class ControllerManager : public rclcpp::Node std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; - std::vector to_use_references_from_subscribers_; std::vector activate_command_interface_request_, deactivate_command_interface_request_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a65d621232..5eaf896ad8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -895,7 +895,6 @@ void ControllerManager::clear_requests() } to_chained_mode_request_.clear(); from_chained_mode_request_.clear(); - to_use_references_from_subscribers_.clear(); activate_command_interface_request_.clear(); deactivate_command_interface_request_.clear(); } @@ -1139,12 +1138,6 @@ controller_interface::return_type ControllerManager::switch_controller( from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); - auto to_use_ref_from_sub_it = std::find( - to_use_references_from_subscribers_.begin(), to_use_references_from_subscribers_.end(), - controller.info.name); - bool in_to_use_ref_from_sub_list = - to_use_ref_from_sub_it != to_use_references_from_subscribers_.end(); - auto deactivate_list_it = std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); bool in_deactivate_list = deactivate_list_it != deactivate_request_.end(); @@ -1153,7 +1146,7 @@ controller_interface::return_type ControllerManager::switch_controller( const bool is_inactive = is_controller_inactive(*controller.c); // restart controllers that need to switch their 'chained mode' - add to (de)activate lists - if (in_to_chained_mode_list || in_from_chained_mode_list || in_to_use_ref_from_sub_list) + if (in_to_chained_mode_list || in_from_chained_mode_list) { if (is_active && !in_deactivate_list) { @@ -2192,7 +2185,6 @@ void ControllerManager::manage_switch() switch_chained_mode(to_chained_mode_request_, true); switch_chained_mode(from_chained_mode_request_, false); - set_controllers_reference_interfaces_availability(to_use_references_from_subscribers_, false); // activate controllers once the switch is fully complete if (!switch_params_.activate_asap) @@ -2440,23 +2432,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode( { // If the preceding controller's state interfaces are in use by other controllers, // then maintain the chained mode - if (is_controller_exported_state_interfaces_inuse_by_other_controllers( + if (!is_controller_exported_state_interfaces_inuse_by_other_controllers( following_ctrl_it->info.name, controllers, deactivate_request_)) - { - auto found_it = std::find( - to_use_references_from_subscribers_.begin(), - to_use_references_from_subscribers_.end(), following_ctrl_it->info.name); - if (found_it == to_use_references_from_subscribers_.end()) - { - // In this case we check if the interface is state only and then add to use - // references_from_subscribers list - to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name); - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'use references from subscriber' request.", - following_ctrl_it->info.name.c_str()); - } - } - else { // currently iterated "controller" is preceding controller --> add following controller // with matching interface name to "from" chained mode list (if not already in it) @@ -2466,11 +2443,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode( following_ctrl_it->info.name) == from_chained_mode_request_.end()) { from_chained_mode_request_.push_back(following_ctrl_it->info.name); - to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( - get_logger(), - "Adding controller '%s' in 'from chained mode' and 'use references from " - "subscriber' request.", + get_logger(), "Adding controller '%s' in 'from chained mode' request.", following_ctrl_it->info.name.c_str()); } } @@ -2601,18 +2575,6 @@ controller_interface::return_type ControllerManager::check_following_controllers "should stay in chained mode.", following_ctrl_it->info.name.c_str()); } - auto ref_from_sub_it = std::find( - to_use_references_from_subscribers_.begin(), to_use_references_from_subscribers_.end(), - following_ctrl_it->info.name); - if (found_it != to_use_references_from_subscribers_.end()) - { - to_use_references_from_subscribers_.erase(ref_from_sub_it); - RCLCPP_DEBUG( - get_logger(), - "Removing controller '%s' in 'use references from subscriber' request because it " - "should stay in chained mode and accept references from the active controller.", - following_ctrl_it->info.name.c_str()); - } } } return controller_interface::return_type::OK; @@ -2650,8 +2612,6 @@ controller_interface::return_type ControllerManager::check_preceeding_controller { for (const auto & ref_itf_name : controller_interfaces) { - std::vector preceding_controllers_using_ref_itf; - // TODO(destogl): This data could be cached after configuring controller into a map for faster // access here for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); @@ -2852,39 +2812,4 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( return controller_node_options; } -void ControllerManager::set_controllers_reference_interfaces_availability( - const std::vector & controllers, bool available) -{ - std::vector & rt_controller_list = - rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - - for (const auto & request : controllers) - { - auto found_it = std::find_if( - rt_controller_list.begin(), rt_controller_list.end(), - std::bind(controller_name_compare, std::placeholders::_1, request)); - if (found_it == rt_controller_list.end()) - { - RCLCPP_FATAL( - get_logger(), - "Got request to turn %s reference interfaces of controller '%s', but controller is not in " - "the realtime controller list. (This should never happen!)", - (available ? "ON" : "OFF"), request.c_str()); - continue; - } - auto controller = found_it->c; - if (!is_controller_active(*controller)) - { - if (available) - { - resource_manager_->make_controller_reference_interfaces_available(request); - } - else - { - resource_manager_->make_controller_reference_interfaces_unavailable(request); - } - } - } -}; - } // namespace controller_manager From c78085dc5a4859a8ab9716cdb73c001cded66235 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 May 2024 00:25:16 +0200 Subject: [PATCH 46/61] update the logic of propagate deactivation of the controllers --- controller_manager/src/controller_manager.cpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5eaf896ad8..07eab4cf24 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2430,23 +2430,17 @@ void ControllerManager::propagate_deactivation_of_chained_mode( ControllersListIterator following_ctrl_it; if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { - // If the preceding controller's state interfaces are in use by other controllers, - // then maintain the chained mode - if (!is_controller_exported_state_interfaces_inuse_by_other_controllers( - following_ctrl_it->info.name, controllers, deactivate_request_)) + // currently iterated "controller" is preceding controller --> add following controller + // with matching interface name to "from" chained mode list (if not already in it) + if ( + std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), + following_ctrl_it->info.name) == from_chained_mode_request_.end()) { - // currently iterated "controller" is preceding controller --> add following controller - // with matching interface name to "from" chained mode list (if not already in it) - if ( - std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name) == from_chained_mode_request_.end()) - { - from_chained_mode_request_.push_back(following_ctrl_it->info.name); - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'from chained mode' request.", - following_ctrl_it->info.name.c_str()); - } + from_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'from chained mode' request.", + following_ctrl_it->info.name.c_str()); } } } From bd9ca59676b053fd25421dc3057a2dfe845928a2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 May 2024 00:26:05 +0200 Subject: [PATCH 47/61] update logic in check_preceding_controllers_to_deactivate to work with exported state interfaces --- controller_manager/src/controller_manager.cpp | 71 ++++++++++++++++--- 1 file changed, 61 insertions(+), 10 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 07eab4cf24..768f77eaf6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2584,20 +2584,71 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } - if (!controller_it->c->is_in_chained_mode()) - { - RCLCPP_DEBUG( - get_logger(), - "Controller with name '%s' is chainable but not in chained mode. " - "No need to do any checks of preceding controllers when stopping it.", - controller_it->info.name.c_str()); - return controller_interface::return_type::OK; - } - RCLCPP_DEBUG( get_logger(), "Checking preceding controller of following controller with name '%s'.", controller_it->info.name.c_str()); + for (const auto & preceeding_controller : + controller_chain_spec_[controller_it->info.name].preceding_controllers) + { + RCLCPP_DEBUG(get_logger(), "\t Preceding controller : '%s'.", preceeding_controller.c_str()); + auto found_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, preceeding_controller)); + if ( + is_controller_inactive(found_it->c) && + std::find(activate_request_.begin(), activate_request_.end(), preceeding_controller) != + activate_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' is inactive and will be activated.", + controller_it->info.name.c_str(), preceeding_controller.c_str()); + return controller_interface::return_type::ERROR; + } + if ( + is_controller_active(found_it->c) && + std::find(deactivate_request_.begin(), deactivate_request_.end(), preceeding_controller) == + deactivate_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "New Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' is active and will not be deactivated.", + controller_it->info.name.c_str(), preceeding_controller.c_str()); + return controller_interface::return_type::ERROR; + } + } + + for (const auto & controller : + controller_chain_spec_[controller_it->info.name].following_controllers) + { + RCLCPP_DEBUG(get_logger(), "\t Following controller : '%s'.", controller.c_str()); + auto found_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller)); + if (found_it != controllers.end()) + { + auto state_itfs = found_it->c->state_interface_configuration().names; + for (const auto & state_itf : state_itfs) + { + if (is_interface_a_chained_interface(state_itf, controllers, found_it)) + { + if (is_controller_active(found_it->c)) + { + RCLCPP_WARN( + get_logger(), + "New Could not deactivate controller with name '%s' because " + "following controller with name '%s' is active.", + controller_it->info.name.c_str(), found_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + } + } + const auto ctrl_ref_itfs = resource_manager_->get_controller_reference_interface_names(controller_it->info.name); const auto ctrl_exp_state_itfs = From 93bdaea96f5bdf16326bb9b80c33fe4264c2ea49 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 May 2024 23:54:48 +0200 Subject: [PATCH 48/61] add controller chain interfaces usage controller list cache --- .../include/controller_manager/controller_manager.hpp | 3 +++ controller_manager/src/controller_manager.cpp | 6 ++++++ 2 files changed, 9 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 9a709c5f8f..1fa68c2f58 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -546,6 +546,9 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; + std::map> controller_chained_reference_interfaces_cache_; + std::map> controller_chained_state_interfaces_cache_; + std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 768f77eaf6..fdb5a3a92a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -819,6 +819,8 @@ controller_interface::return_type ControllerManager::configure_controller( controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); add_element_to_list( controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller_name); + add_element_to_list( + controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller_name); } } // This is needed when we start exporting the state interfaces from the controllers @@ -831,6 +833,8 @@ controller_interface::return_type ControllerManager::configure_controller( controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); add_element_to_list( controller_chain_spec_[ctrl_it->info.name].following_controllers, controller_name); + add_element_to_list( + controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller_name); } } @@ -1439,6 +1443,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co // initialize the data for the controller chain spec once it is loaded. It is needed, so when we // sort the controllers later, they will be added to the list controller_chain_spec_[controller.info.name] = ControllerChainSpec(); + controller_chained_state_interfaces_cache_[controller.info.name] = {}; + controller_chained_reference_interfaces_cache_[controller.info.name] = {}; executor_->add_node(controller.c->get_node()->get_node_base_interface()); to.emplace_back(controller); From 46d5426e292366d165f78b7b526c0c07de7805db Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 9 May 2024 00:11:19 +0200 Subject: [PATCH 49/61] update the expected is_chained_mode in tests with the new changes --- ...trollers_chaining_with_controller_manager.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) 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 b2f4944920..0b9204a98c 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -932,7 +932,7 @@ TEST_P( POSITION_CONTROLLER_CLAIMED_INTERFACES, 10u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state @@ -952,7 +952,7 @@ TEST_P( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 8u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); @@ -968,7 +968,7 @@ TEST_P( DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 8u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); @@ -1519,7 +1519,7 @@ TEST_P( POSITION_CONTROLLER_CLAIMED_INTERFACES, 2u, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); @@ -1544,7 +1544,7 @@ TEST_P( controller_interface::return_type::ERROR, true, true); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); @@ -1564,7 +1564,7 @@ TEST_P( expected.at(test_param.strictness).future_status); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); @@ -1582,7 +1582,7 @@ TEST_P( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 0u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); @@ -1599,7 +1599,7 @@ TEST_P( DeactivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 0u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); - EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); From 0a20f3eee8a8558bf9fee6592ab7d68760b788b2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 9 May 2024 00:12:54 +0200 Subject: [PATCH 50/61] use the new catched controller chain information to update the check for preceeding controllers for deactivate --- controller_manager/src/controller_manager.cpp | 156 +++++------------- 1 file changed, 39 insertions(+), 117 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fdb5a3a92a..0d08a2b136 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2594,137 +2594,59 @@ controller_interface::return_type ControllerManager::check_preceeding_controller get_logger(), "Checking preceding controller of following controller with name '%s'.", controller_it->info.name.c_str()); - for (const auto & preceeding_controller : - controller_chain_spec_[controller_it->info.name].preceding_controllers) + auto preceeding_controllers_list = + controller_chained_state_interfaces_cache_[controller_it->info.name]; + preceeding_controllers_list.insert( + preceeding_controllers_list.end(), + controller_chained_reference_interfaces_cache_[controller_it->info.name].cbegin(), + controller_chained_reference_interfaces_cache_[controller_it->info.name].cend()); + + for (const auto & preceeding_controller : preceeding_controllers_list) { RCLCPP_DEBUG(get_logger(), "\t Preceding controller : '%s'.", preceeding_controller.c_str()); auto found_it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, preceeding_controller)); - if ( - is_controller_inactive(found_it->c) && - std::find(activate_request_.begin(), activate_request_.end(), preceeding_controller) != - activate_request_.end()) - { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' is inactive and will be activated.", - controller_it->info.name.c_str(), preceeding_controller.c_str()); - return controller_interface::return_type::ERROR; - } - if ( - is_controller_active(found_it->c) && - std::find(deactivate_request_.begin(), deactivate_request_.end(), preceeding_controller) == - deactivate_request_.end()) - { - RCLCPP_WARN( - get_logger(), - "New Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' is active and will not be deactivated.", - controller_it->info.name.c_str(), preceeding_controller.c_str()); - return controller_interface::return_type::ERROR; - } - } - for (const auto & controller : - controller_chain_spec_[controller_it->info.name].following_controllers) - { - RCLCPP_DEBUG(get_logger(), "\t Following controller : '%s'.", controller.c_str()); - auto found_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, controller)); if (found_it != controllers.end()) { - auto state_itfs = found_it->c->state_interface_configuration().names; - for (const auto & state_itf : state_itfs) + if ( + is_controller_inactive(found_it->c) && + std::find(activate_request_.begin(), activate_request_.end(), preceeding_controller) != + activate_request_.end()) { - if (is_interface_a_chained_interface(state_itf, controllers, found_it)) - { - if (is_controller_active(found_it->c)) - { - RCLCPP_WARN( - get_logger(), - "New Could not deactivate controller with name '%s' because " - "following controller with name '%s' is active.", - controller_it->info.name.c_str(), found_it->info.name.c_str()); - return controller_interface::return_type::ERROR; - } - } + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' is inactive and will be activated.", + controller_it->info.name.c_str(), preceeding_controller.c_str()); + return controller_interface::return_type::ERROR; } - } - } - - const auto ctrl_ref_itfs = - resource_manager_->get_controller_reference_interface_names(controller_it->info.name); - const auto ctrl_exp_state_itfs = - resource_manager_->get_controller_exported_state_interface_names(controller_it->info.name); - for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_exp_state_itfs}) - { - for (const auto & ref_itf_name : controller_interfaces) - { - // TODO(destogl): This data could be cached after configuring controller into a map for faster - // access here - for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); - ++preceding_ctrl_it) + if ( + is_controller_active(found_it->c) && + std::find(deactivate_request_.begin(), deactivate_request_.end(), preceeding_controller) == + deactivate_request_.end()) { - const auto preceding_ctrl_cmd_itfs = - preceding_ctrl_it->c->command_interface_configuration().names; - const auto preceding_ctrl_state_itfs = - preceding_ctrl_it->c->state_interface_configuration().names; - - // if controller is not preceding go the next one - if ( - (std::find( - preceding_ctrl_cmd_itfs.begin(), preceding_ctrl_cmd_itfs.end(), ref_itf_name) == - preceding_ctrl_cmd_itfs.end()) && - (std::find( - preceding_ctrl_state_itfs.begin(), preceding_ctrl_state_itfs.end(), ref_itf_name) == - preceding_ctrl_state_itfs.end())) - { - continue; - } - - // check if preceding controller will be activated - if ( - is_controller_inactive(preceding_ctrl_it->c) && - std::find( - activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != - activate_request_.end()) - { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' will be activated. ", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; - } - // check if preceding controller will not be deactivated - else if ( - is_controller_active(preceding_ctrl_it->c) && - std::find( - deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) == - deactivate_request_.end()) - { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' is active and will not be deactivated.", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; - } - // TODO(destogl): this should be discussed how to it the best - just a placeholder for now - // else if ( - // strictness == - // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) - // { - // // insert to the begin of activate request list to be activated before preceding - // controller - // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); - // } + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' is active and will not be deactivated.", + controller_it->info.name.c_str(), preceeding_controller.c_str()); + return controller_interface::return_type::ERROR; } } } + + // TODO(destogl): this should be discussed how to it the best - just a placeholder for now + // else if ( + // strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) + // { + // // insert to the begin of activate request list to be activated before preceding + // controller + // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); + // } + return controller_interface::return_type::OK; } From 3ee24389103835382e901756c83248c5e401973a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 9 May 2024 08:11:37 +0200 Subject: [PATCH 51/61] change robot localization controller a chained controller and some corresponding changes --- .../test_chainable_controller.cpp | 10 ++++++++++ .../test_chainable_controller.hpp | 3 +++ ...st_controllers_chaining_with_controller_manager.cpp | 9 +++++---- 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index ae9d14f2bf..e43f2a13a1 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -234,6 +234,16 @@ void TestChainableController::set_imu_sensor_name(const std::string & name) imu_sensor_ = std::make_unique(name); } } + +std::vector TestChainableController::get_state_interface_data() const +{ + std::vector state_intr_data; + for (const auto & interface : state_interfaces_) + { + state_intr_data.push_back(interface.get_value()); + } + return state_intr_data; +} } // namespace test_chainable_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 2862b8e2e7..f4f59ad9df 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -91,6 +91,9 @@ class TestChainableController : public controller_interface::ChainableController CONTROLLER_MANAGER_PUBLIC void set_imu_sensor_name(const std::string & name); + CONTROLLER_MANAGER_PUBLIC + std::vector get_state_interface_data() const; + size_t internal_counter; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; 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 0b9204a98c..a1109953de 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -127,7 +127,7 @@ class TestControllerChainingWithControllerManager position_tracking_controller = std::make_shared(); odom_publisher_controller = std::make_shared(); sensor_fusion_controller = std::make_shared(); - robot_localization_controller = std::make_shared(); + robot_localization_controller = std::make_shared(); // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { @@ -193,6 +193,7 @@ class TestControllerChainingWithControllerManager std::string(SENSOR_FUSION_CONTROLLER) + "/odom_y", std::string(SENSOR_FUSION_CONTROLLER) + "/yaw"}}; robot_localization_controller->set_state_interface_configuration(odom_ifs_cfg); + robot_localization_controller->set_exported_state_interface_names({"actual_pose"}); } void CheckIfControllersAreAddedCorrectly() @@ -329,14 +330,14 @@ class TestControllerChainingWithControllerManager position_tracking_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); - EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); cm_->configure_controller(ODOM_PUBLISHER_CONTROLLER); EXPECT_EQ( position_tracking_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); - EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); } template < @@ -578,7 +579,7 @@ class TestControllerChainingWithControllerManager std::shared_ptr diff_drive_controller; std::shared_ptr diff_drive_controller_two; std::shared_ptr sensor_fusion_controller; - std::shared_ptr robot_localization_controller; + std::shared_ptr robot_localization_controller; std::shared_ptr odom_publisher_controller; std::shared_ptr position_tracking_controller; From 63f2c87d01016b028c83180dcebb20bdb013ae63 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 10 May 2024 00:24:00 +0200 Subject: [PATCH 52/61] Add position controller two to test the cases of closing loop with other controllers --- ...llers_chaining_with_controller_manager.cpp | 75 ++++++++++++++++++- 1 file changed, 74 insertions(+), 1 deletion(-) 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 a1109953de..b4c71a0ae2 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -125,6 +125,7 @@ class TestControllerChainingWithControllerManager diff_drive_controller = std::make_shared(); diff_drive_controller_two = std::make_shared(); position_tracking_controller = std::make_shared(); + position_tracking_controller_two = std::make_shared(); odom_publisher_controller = std::make_shared(); sensor_fusion_controller = std::make_shared(); robot_localization_controller = std::make_shared(); @@ -194,11 +195,21 @@ class TestControllerChainingWithControllerManager std::string(SENSOR_FUSION_CONTROLLER) + "/yaw"}}; robot_localization_controller->set_state_interface_configuration(odom_ifs_cfg); robot_localization_controller->set_exported_state_interface_names({"actual_pose"}); + + // configure Position Tracking controller Two + controller_interface::InterfaceConfiguration position_tracking_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(ROBOT_LOCALIZATION_CONTROLLER) + "/actual_pose"}}; + // in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel" + position_tracking_controller_two->set_command_interface_configuration( + position_tracking_cmd_ifs_cfg); + position_tracking_controller_two->set_state_interface_configuration( + position_tracking_state_ifs_cfg); } void CheckIfControllersAreAddedCorrectly() { - EXPECT_EQ(8u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(9u, cm_->get_loaded_controllers().size()); EXPECT_EQ(2, pid_left_wheel_controller.use_count()); EXPECT_EQ(2, pid_right_wheel_controller.use_count()); EXPECT_EQ(2, diff_drive_controller.use_count()); @@ -207,6 +218,7 @@ class TestControllerChainingWithControllerManager EXPECT_EQ(2, sensor_fusion_controller.use_count()); EXPECT_EQ(2, robot_localization_controller.use_count()); EXPECT_EQ(2, odom_publisher_controller.use_count()); + EXPECT_EQ(2, position_tracking_controller_two.use_count()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, @@ -232,6 +244,9 @@ class TestControllerChainingWithControllerManager EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + position_tracking_controller_two->get_state().id()); } // order or controller configuration is not important therefore we can reuse the same method @@ -338,6 +353,13 @@ class TestControllerChainingWithControllerManager lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); + + cm_->configure_controller(POSITION_TRACKING_CONTROLLER_TWO); + EXPECT_EQ( + position_tracking_controller_two->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); } template < @@ -553,6 +575,7 @@ class TestControllerChainingWithControllerManager static constexpr char SENSOR_FUSION_CONTROLLER[] = "sensor_fusion_controller"; static constexpr char ROBOT_LOCALIZATION_CONTROLLER[] = "robot_localization_controller"; static constexpr char ODOM_PUBLISHER_CONTROLLER[] = "odometry_publisher_controller"; + static constexpr char POSITION_TRACKING_CONTROLLER_TWO[] = "position_tracking_controller_two"; const std::vector PID_LEFT_WHEEL_REFERENCE_INTERFACES = { "pid_left_wheel_controller/velocity"}; @@ -582,6 +605,7 @@ class TestControllerChainingWithControllerManager std::shared_ptr robot_localization_controller; std::shared_ptr odom_publisher_controller; std::shared_ptr position_tracking_controller; + std::shared_ptr position_tracking_controller_two; testing::WithParamInterface::ParamType test_param; @@ -637,6 +661,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -849,6 +876,9 @@ TEST_P( cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -1067,6 +1097,9 @@ TEST_P( cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -1182,6 +1215,9 @@ TEST_P( cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -1201,6 +1237,9 @@ TEST_P( ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); // Verify that the other preceding controller is deactivated (diff_drive_controller_two) and other // depending controllers are active @@ -1214,6 +1253,31 @@ TEST_P( robot_localization_controller->get_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller_two->get_state().id()); + + // Deactivate position_tracking_controller and activate position_tracking_controller_two + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER_TWO}, {POSITION_TRACKING_CONTROLLER}, test_param.strictness, + std::future_status::timeout, controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller_two->get_state().id()); + + // Now deactivate the position_tracking_controller_two and it should be in inactive state + switch_test_controllers( + {}, {POSITION_TRACKING_CONTROLLER_TWO}, test_param.strictness, std::future_status::timeout, + controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller_two->get_state().id()); // Deactivate the first preceding controller (diff_drive_controller) and // activate the other preceding controller (diff_drive_controller_two) @@ -1274,6 +1338,9 @@ TEST_P( cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -1446,6 +1513,9 @@ TEST_P( cm_->add_controller( robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -1696,6 +1766,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add cm_->add_controller( odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); From 1057562471258b17fb9d78c8964e0459642b7e9e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 12 May 2024 12:33:30 +0200 Subject: [PATCH 53/61] Add activating and deactivating in group with the new controller --- ...llers_chaining_with_controller_manager.cpp | 51 ++++++++++++++++++- 1 file changed, 49 insertions(+), 2 deletions(-) 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 b4c71a0ae2..1cc8a3af7b 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -1279,12 +1279,23 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, position_tracking_controller_two->get_state().id()); + // Activate it again and deactivate it others to see if we can deactivate it in a group + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER_TWO}, {}, test_param.strictness, std::future_status::timeout, + controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller_two->get_state().id()); + // Deactivate the first preceding controller (diff_drive_controller) and // activate the other preceding controller (diff_drive_controller_two) switch_test_controllers( {DIFF_DRIVE_CONTROLLER_TWO}, - {DIFF_DRIVE_CONTROLLER, SENSOR_FUSION_CONTROLLER, ROBOT_LOCALIZATION_CONTROLLER, - ODOM_PUBLISHER_CONTROLLER}, + {POSITION_TRACKING_CONTROLLER_TWO, DIFF_DRIVE_CONTROLLER, SENSOR_FUSION_CONTROLLER, + ROBOT_LOCALIZATION_CONTROLLER, ODOM_PUBLISHER_CONTROLLER}, test_param.strictness, std::future_status::timeout, controller_interface::return_type::OK); // Following controllers should stay active @@ -1306,6 +1317,42 @@ TEST_P( EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller_two->get_state().id()); + + // Activate all the controllers again in group and deactivate the diff_drive_controller_two + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER_TWO, DIFF_DRIVE_CONTROLLER, SENSOR_FUSION_CONTROLLER, + ROBOT_LOCALIZATION_CONTROLLER, ODOM_PUBLISHER_CONTROLLER}, + {DIFF_DRIVE_CONTROLLER_TWO}, test_param.strictness, std::future_status::timeout, + controller_interface::return_type::OK); + // Following controllers should stay active + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller_two->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller_two->get_state().id()); } TEST_P( From b612a1b0cabb765dee06e3573dd0e94f882c40ed Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 12 May 2024 19:57:26 +0200 Subject: [PATCH 54/61] Fix the logic of controllers exporting state interfaces not in chained mode --- controller_manager/src/controller_manager.cpp | 27 +++++++++++++------ ...llers_chaining_with_controller_manager.cpp | 17 +++++++----- 2 files changed, 30 insertions(+), 14 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 22936217b1..f4de720bca 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2471,13 +2471,18 @@ controller_interface::return_type ControllerManager::check_following_controllers get_logger(), "Checking following controllers of preceding controller with name '%s'.", controller_it->info.name.c_str()); - auto controller_interfaces = controller_it->c->command_interface_configuration().names; - auto controller_state_interfaces = controller_it->c->state_interface_configuration().names; + const auto controller_cmd_interfaces = controller_it->c->command_interface_configuration().names; + const auto controller_state_interfaces = controller_it->c->state_interface_configuration().names; + // get all interfaces of the controller + auto controller_interfaces = controller_cmd_interfaces; controller_interfaces.insert( controller_interfaces.end(), controller_state_interfaces.begin(), controller_state_interfaces.end()); for (const auto & ctrl_itf_name : controller_interfaces) { + RCLCPP_ERROR( + get_logger(), "Checking interface '%s' of controller '%s'.", ctrl_itf_name.c_str(), + controller_it->info.name.c_str()); ControllersListIterator following_ctrl_it; // Check if interface if reference interface and following controller exist. if (!is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) @@ -2555,17 +2560,23 @@ controller_interface::return_type ControllerManager::check_following_controllers following_ctrl_it->info.name); if (found_it == to_chained_mode_request_.end()) { - to_chained_mode_request_.push_back(following_ctrl_it->info.name); // if it is a chainable controller, make the reference interfaces available on preactivation // (This is needed when you activate a couple of chainable controller altogether) // make all the exported interfaces of the controller available resource_manager_->make_controller_exported_state_interfaces_available( following_ctrl_it->info.name); - resource_manager_->make_controller_reference_interfaces_available( - following_ctrl_it->info.name); - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'to chained mode' request.", - following_ctrl_it->info.name.c_str()); + if ( + std::find( + controller_cmd_interfaces.begin(), controller_cmd_interfaces.end(), ctrl_itf_name) != + controller_cmd_interfaces.end()) + { + resource_manager_->make_controller_reference_interfaces_available( + following_ctrl_it->info.name); + to_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'to chained mode' request.", + following_ctrl_it->info.name.c_str()); + } } } else 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 1cc8a3af7b..cd2485e60e 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -946,7 +946,7 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); - ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); ASSERT_EQ(robot_localization_controller->internal_counter, 3u); ASSERT_EQ(odom_publisher_controller->internal_counter, 1u); @@ -968,7 +968,7 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces - ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); EXPECT_EQ( @@ -1333,15 +1333,20 @@ TEST_P( // Following controllers should stay active EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller_two->get_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + // This is false, because it only uses the state interfaces and exposes state interfaces + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, robot_localization_controller->get_state().id()); @@ -1595,7 +1600,7 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); - ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); // Verify that initially all of them are in active state EXPECT_EQ( @@ -1638,7 +1643,7 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); - ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state @@ -1663,7 +1668,7 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); - ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); EXPECT_EQ( @@ -1683,7 +1688,7 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); - ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); EXPECT_EQ( From 5e48aa9e987719845c30ebf3520205d9498992f4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 12 May 2024 21:36:06 +0200 Subject: [PATCH 55/61] Cleanup unused helper method --- controller_manager/src/controller_manager.cpp | 53 ------------------- 1 file changed, 53 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f4de720bca..abe85b3f43 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -133,59 +133,6 @@ bool is_interface_a_chained_interface( return true; } -bool is_interface_exported_from_controller( - const std::string & interface_name, const std::string & controller_name) -{ - auto split_pos = interface_name.find_first_of('/'); - if (split_pos == std::string::npos) // '/' exist in the string (should be always false) - { - RCLCPP_FATAL( - rclcpp::get_logger("ControllerManager::utils"), - "Character '/', was not find in the interface name '%s'. This should never happen. " - "Stop the controller manager immediately and restart it.", - interface_name.c_str()); - throw std::runtime_error("Mismatched interface name. See the FATAL message above."); - } - - auto interface_prefix = interface_name.substr(0, split_pos); - if (controller_name.compare(interface_prefix) == 0) - { - return true; - } - else - { - return false; - } -} - -bool is_controller_exported_state_interfaces_inuse_by_other_controllers( - const std::string & controller_name, - const std::vector & controllers, - std::vector blacklist) -{ - for (const auto & controller : controllers) - { - auto blacklist_it = std::find(blacklist.begin(), blacklist.end(), controller.info.name); - if (blacklist_it != blacklist.end() || controller_name.compare(controller.info.name) == 0) - { - continue; - } - if (!is_controller_active(controller.c)) - { - continue; - } - auto controller_exported_state_interfaces = controller.c->state_interface_configuration().names; - for (const auto & ctrl_itf_name : controller_exported_state_interfaces) - { - if (is_interface_exported_from_controller(ctrl_itf_name, controller_name)) - { - return true; - } - } - } - return false; -} - template void add_element_to_list(std::vector & list, const T & element) { From 96eced7e0e0a813c1ef19917370563feb9a1d093 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 13 May 2024 08:40:23 +0200 Subject: [PATCH 56/61] Add documentation and release note about exporting state interfaces from controllers --- .../doc/controller_chaining.rst | 30 ++++++++++++------ .../doc/images/chaining_example2.png | Bin 37092 -> 89572 bytes doc/release_notes/Jazzy.rst | 3 ++ 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index c83ed97ac0..82c66bb3fc 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -27,7 +27,7 @@ To describe the intent of this document, lets focus on the simple yet sufficient :alt: Example2 -In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers. +In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers as well as the 'robot_localization' controller. Let's now imagine a use-case where we don't only want to run all those controllers as a group, but also flexibly add preceding steps. This means the following: @@ -37,9 +37,19 @@ This means the following: 2. Then "diff_drive_controller" is activated and attaches itself to the virtual input interfaces of PID controllers. PID controllers also get informed that they are working in chained mode and therefore disable their external interface through subscriber. Now we check if kinematics of differential robot is running properly. - 3. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces. - 4. If any of the controllers is deactivated, also all preceding controllers are deactivated. + 3. Once the 'diff_drive_controller' is activated, it exposes the 'odom' state interfaces that is used by 'odom_publisher' as well as 'sensor_fusion' controllers. + The 'odom_publisher' controller is activated and attaches itself to the exported 'odom' state interfaces of 'diff_drive_controller'. + The 'sensor_fusion' controller is activated and attaches itself to the exported 'odom' state interfaces of 'diff_drive_controller' along with the 'imu' state interfaces. + 4. Once the 'sensor_fusion' controller is activated, it exposes the 'odom' state interfaces that is used by 'robot_localization' controller. + The 'robot_localization' controller is activated and attaches itself to the 'odom' state interfaces of 'sensor_fusion' controller. + Once activated, the 'robot_localization' controller exposes the 'actual_pose' state interfaces that is used by 'position_tracking' controller. + 5. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces and to the 'robot_localization' controller which provides the 'actual_pose' state interface. + 6. If any of the controllers is deactivated, also all preceding controllers needs to be deactivated. +.. note:: + + Only the controllers that exposes the reference interfaces are switched to chained mode, when their reference interfaces are used by other controllers. When their reference interfaces are not used by the other controllers, they are switched back to get references from the subscriber. + However, the controllers that exposes the state interfaces are not triggered to chained mode, when their state interfaces are used by other controllers. Implementation -------------- @@ -47,10 +57,10 @@ Implementation A Controller Base-Class: ChainableController ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual InterfaceConfiguration input_interface_configuration() const = 0`` method. -This method should implement for each controller that **can be preceded** by another controller exporting all the input interfaces. -For simplicity reasons, it is assumed for now that controller's all input interfaces are used. -Therefore, do not try to implement any exclusive combinations of input interfaces, but rather write multiple controllers if you need exclusivity. +A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual std::vector export_reference_interfaces() = 0`` method as well as ``virtual std::vector export_state_interfaces() = 0`` method. +This method should implement for each controller that **can be preceded** by another controller exporting all the reference/state interfaces. +For simplicity reasons, it is assumed for now that controller's all reference interfaces are used by other controller. However, the state interfaces exported by the controller, can be used by multieple controllers at the same time and with the combination they want. +Therefore, do not try to implement any exclusive combinations of reference interfaces, but rather write multiple controllers if you need exclusivity. The ``ChainableController`` base class implements ``void set_chained_mode(bool activate)`` that sets an internal flag that a controller is used by another controller (in chained mode) and calls ``virtual void on_set_chained_mode(bool activate) = 0`` that implements controller's specific actions when chained modes is activated or deactivated, e.g., deactivating subscribers. @@ -59,7 +69,7 @@ As an example, PID controllers export one virtual interface ``pid_reference`` an Inner Resource Management ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -After configuring a chainable controller, controller manager calls ``input_interface_configuration`` method and takes ownership over controller's input interfaces. +After configuring a chainable controller, controller manager calls ``export_reference_interfaces`` and ``export_state_interfaces`` method and takes ownership over controller's exported reference/state interfaces. This is the same process as done by ``ResourceManager`` and hardware interfaces. Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``). @@ -71,6 +81,8 @@ Chained controllers must be activated and deactivated together or in the proper This means you must first activate all following controllers to have the preceding one activated. For the deactivation there is the inverse rule - all preceding controllers have to be deactivated before the following controller is deactivated. One can also think of it as an actual chain, you can not add a chain link or break the chain in the middle. +The chained controllers can also be activated when parsed as in a single list through the fields ``activate_controllers`` or ``deactivate_controllers`` in the ``switch_controllers`` service provided by the controller_manager. +The controller_manager ``spawner`` can also be used to activate all the controllers of the chain in a single call, by parsing the argument ``--activate-as-group``. Debugging outputs @@ -84,4 +96,4 @@ Debugging outputs Closing remarks ---------------------------- -- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. +- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``export_reference_interfaces()`` and ``export_state_interfaces()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. diff --git a/controller_manager/doc/images/chaining_example2.png b/controller_manager/doc/images/chaining_example2.png index 1ba49a116edcba1f365d76d4b52b3464f316a118..c21ab88d727c106f679e3e19c21739d0cd955795 100644 GIT binary patch literal 89572 zcmeEv2|Sc*`+rGM9ifs^wkWCW`z~Zj2$h{=YqB=RZY-&sHbh9a%19+-&o-3G7BXaK z6k!ae?2P4qkC{QsdFyxH_ncGj|9wB_)7w1D{oK!eUDx;eUd#P-`Ix2(%?73ot5&U| zQBzgYS+!~nX4NW+c$xA z9zj8KCnsJ@h`9~a+}@ej!3qWrf%Eo`mJn-*l_l{SL4H9Y9)3|Cev#t>B71}m2@8Th z_(gdI1;h=B*PGi|Igkd_aP)-O*_rPVR2Jdo2Se@C7ZBJZqyYYQ; z-cfy3hcmi%i)*uTH1e4C?kZ4or!yLkmR5FP2upY36a<7s!H9%GEj@@w#3V$B2W+6^ z=a3#tIG_vhBuz+wv=%ll5KAj((rCmxVUCV=Fo@IV2Q3^O9IPxz0waxR4uv|peSVp> zqaEq(NY^-l7yoQ@!U0_ibGxsP>Om}FXNcE|i;$*AxLVZ;VsnQ4WFT4MEbYz77n2S+ zpE0*|bR*uscDzM_>KVX?{UNO}&?rT_cAORE80()xdA zI0t2&6!#Nu-c_}pB8=|Vm9!L@T?In)OD&qc5LuT;v)@-qdza`Nh znZoi!jyXbruV1oxq0d!Jh^$WJ2RS>sKrO6DH~pA36QA{mPZTD3u_7F=;g6~j(S~){J%}+1-5rR%T5k$HOy&?*GM3lgh zueJP}6DC{bmo7w*pF}Z84*|n1s>#1zre%pQNnlCMc=&mR!~{ssBMDxJILp6B&&7!k zBR!hr9>^N!Xl?CG=FZ>uR^<2mXO&Hqq-|d)mM)23I6`4(9BmvO%Xbt4^B_#8u* zuQ}@I=mb=cKs3%;!C)RFZffoVa|DN0&=4zk2y78`i9ic}8IbNHoLR2ixO}BE~JZ)wBp~#^dDAYg7SQyvo5OE ze-N2S3$g;4K!9j%W(fuU#SGXw4C-iS2S8_$NvzPQ&-LXepr}j!iDbyXFeM_zuq$Np z)$;!zDG|vw306$7#zh-jveRWF`?hToZd$U;KTo5+Ynvn+{;qA3RQBID&7X>H36tX| zlKu*denHj$$G3o6_&VPICd`x2dxZsA*jt*p60$M`rn&;h{szir4Nz3k(asStH{h3q z6cs_JOz?1o<0N+~0D|)$rDkMw_|dFgNf4KBg<3j+w3Q&gos~5q@Nzq2^$iXE={&8-7m=YL`S%Ju6(C5Jpeifl*Dn$6 zCrAd7X8Dywpo_`OUsfbSuK~XzUvsw%?0S&iUia$lqxe&_gAlekNuvm;l1(y%4h>Xbx2~jE!s49T+ z0-zSyzC^9T{je}6LKUV^B< z$H54Qd|nM9cclCiQ8#HOzjDwmOiGxnh#x)=ssEi~2T-czgsR|TiJ6c@U8*pDGo!i~ zQ2ZSkRbdh?d^e>^j@JKuC-cKfN_gboU+y5U$O^VW%DV6`mOP+e!PMuv@)Hy~#L4Lv z68{wVb$dU^S&F~jG{t^8)BMqRU+_N`@c-#tD;k{x`PLJ5=L z>%U$Z$gMFeaN5z*(ca9-<+L5d`OL4-D?(~o6XYjRC}9y2QU3MHCFBO{71H=3^j|hw za>)MukpA2G0)Bo`Q9_pjc^q;AQ!Xn!90MMyaJU1Jyh|Amy1S7e2H zIa@h6J3`H@T^4uVEash-RZQ@w3zvj}b6A|@uW#$3Mynll0uRmMpnn4ypjK?xuvc7;o~m0+RDN@ zA{r|0;)iU=dH^J;^s>^e@2pn|m3*-#+uB)NN5O zegtg^y{0R+tNgIm{^g17Z|VxU2Vx~P_8B~@5&6Lnw|RW7 zNZ(wwKdTIcloOZ@KQC$7$xe_s!yi_aA5p_U>|Y}FO^_bVL#8&r2;hC&L6MAjMYI8R zJnaZGvvUNy-N4Sa#Wtc}F60m)2k9$FIgF|=_}Hdz3lQFL-0jchSL zK|Au-C6KR{`*R2XoqD9DIY`J6KJ`HG#9zzkUyA%olKAHZ#PdaXLhesoAs}&5^zfO0 z1W3#A^9YDsC;wOW06s?nKFdMajr$$pm^(WWztv$4aVH2#!5M6}v$gsb!3W=YvHRBX ze@vubNK~BUj8;fg_!|->ANz>}N(vMHCV?*R{rOy=OUHi}feMKJvJ(9|eE$kV|AQvF zB)Kn4bm{odB2mF#*G9i)g@2_)m$E8fNObA=&mvKwU)DxNzV7)ZZh<3)nx~=U%F=&e zN8DFVReAA$|0EwG(Hat6B`?NG-$}x-m260m#A<&Y-$^c-uJE1z|4I>j-@s&*fOU#X2t{~@QzGHsmtk*wce%~w<2$7I{ zMH>41?IoWJ@0Tczej}m&-AX8V!@zf&*vPi~Mg8?pW9hSq)(S$Uu+2{A>l8LMx?yc&yPlbvApjRv6ZNa~vpDt%z|J;Xq zg-9~|TViE?a=qg(rlVx?OA;H|Z~x>t_05zoiBkVUFtxOw!eBXu{&!*On@dBI?srj& z+;slm1XJH!5Yisf6&XZ`y~`p%EvEH;9<2I0bMnpiA@_Cv^$d%jmtTUEttVs0-x@fR zRGIW_60`idU~BQ;8~THP?T-8}7cKw0f-3{C#k1sp$&h%0bOWdqf)-@LKc2V7jC*L+ zDz;T>N{5cSn+|qR-P(Ghb>dSXdm#J9{nY!YRq31AJblc#qW9G>SGy}6^G9Eh`)A=8*Lrk)O{gt_(;ciTG0f>U2MZc zw608pN#F4ktLfFm7Hkf!T1`nOzw(dxwHwZ;ie0VkI5kSm=C_Js#UB@p?#TO7D5%m- z3Y2tme148n$uEQ@MBxq17sF9fV|J}sF~;JM|GRkQvi@&1{;}2YrF0!`=eDYf2$!3m zQmnkm9cIC`jn{Zzg7WpyLjmhPBqyi@vxMi^w^btHcuR18+Axxq!%uz+sP(&K39oBQ z_7S4S)*2#f@#;4Wy4V+ae&ViTpk>{?!_c|$rm8}ZQ708(Vb)PT)d z)hyQ`cC5cn@sVB{ubJ;$b_}$Vdt|CRxC)aL&ubIw5@FYpakw?tkq=im8uZ&v(W94s z+p*hIp(EWw#bcy`YY3NCIC5mVOm5zMA=RwdXQV$NxN>G3o9KdV9BHwMuXuA;aR_bS zmZLH7HkpeD~7ygr0YZQMxw!mP?( zjoX}P_++dli&~Rh!B9?{Wos71^@?}2sJ)##uA?BA24ma!_y(=q>@iK*Y0D@Xubg&I zY|2QzmRtpxWuikz{={Tboadu<=USQ2xHw*>9F=R^*&U1La=hoK`|dGrtdYgrNu5nI zmo2b&8EO?tnA?BiQF&sd>3%Tr$Y@7o)OjE#ij8!rEl=HH%u680=HvgEeeRl2&68(O zk3ZCLeKeEZWO`t$`I^2> zUg!fS9&xtiI_4$}7B9>fm=vE#J}KIRox|pPk5-H6Bq-D1qXu&l8L5S1il^RRg}yzZ zrTbYbohIn)rnbd#&$ZT0B3$cnSkW&%jO0Jxyl5*(Qb=mX~8I<3td)cTBU- zh9mCZw}zRl8NIN6i=8U2JulN3KK|BsJw_#H*9|IG?_;m7?cgY5^(`!-JC+Bv+KF

IbuZBaNY#W4HOD)VQWADst%M*)&%xc*giW4qBQ$UD4AZN4kKP?p7m*4V zH;gnIIaWlP`9KtBB>G}3aB58uZ2p;3OI8d>i`r6<)%p80EHlHLFMR^6(XY zR5;Cep_79S3MNrbyULZ7EE?(pc^?JOcv-k*@9QX6PGi=6Vm`C(Qq{(aURSsA^t=mX ztr0=cvdS923SzcZX&(=_^W-bggg42j(uURJ>*VIfbn$m2in!*I(~zMI2TocR@W91m zFS*&Pvq~rFe#ovC>vZQB++(#suX+YNTOl&IX4!?kI6!BwicYr(6t-_|&{Sy(d=61< z3M?8z7o%l`HE}e2ItbNNc9UF&DA_cQt{M5KUbv#B)MWoTBz`!Pan%^7i7%_i6JyOb zl-GMRj}HuWkg@99B4I$NCYsWpUbw62gZK0X=8ylq_CEb*{+5!i{GhyQpZgaF<=NPP zt@&*|@9>2aW&;;*|C1N1ZtyR_A&o9=Vm>?hp-zQ0T!qEs#r(YQe4p>2?`U71RE+QZ zw4hbxO@#Qi=Jk}D4g5ml%HJm7mi_k*3Z_>a&vr%3ZtRNm6>5|(p;@hvzw=Sn;2UcTfIxI5sM@Wti3<5$&ZcW0LI z$;ve!FG`CvoGW+>>!tcf@v*{LbFkwzGVd_xp<#A^jz=W~2 zjbOq%z>I`~vd%Bfs4J5&;RCCDK24{KQfRKHL&b(7m{*XX&H8>()a#9<^W%;az;i#B z)XLYf1IL=%^*63gT1$=5yozF3a)o|;6i1taf|-k_X7zWjP67@`Qw_Cg`E(k^HA;MC z?JF=Tb}%W$5aH!nfc}b6R)8LGdhQ0Se{Y*+FPm2W~)D5tqsnmcLJ+8e3Nr% zg_4DoP`p-B?RsA~j@ZnG;5s4EvSayX%F{r2WxcNqsl|D~G8x{SSY9S?`cwI?-h)#w z`^s9W#rJ^IM?zg!oMt%1*{tUy3MRD)EEDs2#ENl!!Ryb@WV%q&IVpn~UE@eup3&67 z64=nx20t@zpU2nzbSW;tk~kq$WZaNnJ*lme2%gb*@31r7A^+(EPws!maiSYRAU{Z=;Qh{uc<#9{Q z!0Q*!+W}t~Ll0)ut$JX2MzS0EO{9<%U4LL5@@)QVZ(R;4d6|G!19H`R1kDgyzM1;P zUU}8v>g(BbPKIEavi1$JFU1B^y9u&u{`3dRHnqP*^-0>S-LBP?y9mofskUO=`3-ca zxp5mTHCxv;pwBy03|H8zK?x4i>fBcoKHO1IRV%j;ll%hszWr&%^Z9e__cfv|8y;v2 zo_WI-Kdv-8G0N<>iH(j9z`p#3Lt@J+x!DMiHq#tC1A{zh;!Qp?Q|s5a*yr$*k=(lX zr;wwsl|B|x!d9&YqVAJl0l6is{a>@smH4kgCF}7`_k?Nn0Jav{S>#pa(qzIFE9nwS zD?24W(D0BW-e=~aF|=Y&hE>%z2pU;bI9emMX>9kzULfJN^t}|C;1y#V5IoBgMpMql zrN&AiCb3Og+jtB)0RfE|$2L`Y^u=;nlm&%bSKn4Ud-hRmuPKP~NX?BUC1LaPTSF10I5U)s z=EYA8g+-crs)^b)v-QP!r54E*0eTKd4}50qMwF0^3_=yUB0V@7V^-#mCgZYLo4fxvJ#9YX!K z4ARBG3Vr8VB!O1}i{o{E-9~`RwIzSb)y;0zf9iA>HqfBfTN{s46%&*b=)AGc z8FC?HIER0TgDw_mgCv*ivSX!l@@G5Rq}da~LRsv`dKyN%S_{lJjz5!rNqVbu*S52) zAAXx0xs}K8c%1i$T9k-wM2t(LfgpSa2C2JuQF^4DBLh;u7tcFpo0_NGnr+KP(9bQJ z;)m94+O?0-*vY?R&b;(eMF_j%pj3sF>)@F-6gthM;G_d`+M2-fh0ivq#<{;=--e9j zz&JDoP5?eC3G5Q4>AhhI0@=DK90JFHQMX?@OVe8#NE;c*;;A|_iQ}8>aEmvF)g6Xb z@#Lz`&5S34AWifd`dk_Fy@Yx~>||ntLa0I8>~kx!AoYl&QG%%Z8aoM=H#Xd1bX&~P z!8gtR`c5&;;lfc(gD0o_XD8YmQAJ*E(;(Jald3gIC;v6+=t}l$tIu}Bb=s@E%5~`2 zFV+DIo0}W?Q1zjdR_-SF|Eent2P{W>tHGnUv>@RHH}{`7(pw$PWs(m|pL_tOHLjEE zV1j8%i+4fYn0OXIRdq|iVo*|Yvst10IlP<8KtsB(8{p?2+fW@7qvKttx~VxsfB^lk z9M)SVi?a-G=y4z>Ix;_==G#IKjSZCr1Uc{1tDQqpRBvOa##hosUi&Fd#(_ zgG%B1Paqu8NN9^yq#B^4!7UQ8d(DbGU1}Bb+l1(6Q+64tu$=>%a9Q_&=aps8m|5bt zdW|Knvq;co;5ab5px92IPcIqO?zDRVH=k<+ZT|!ns~iN ztv@27|CFnxsK3UuyT zu5CfO6=Xf`(D5{BR&Nz?i0qSefR{MR5ja>`btMFRt*%fL|e;||fSIT);kTs5v zwR-d1TNpKNGUHvv8~vesDl4pLc*ET0$Ac{FmFwx)WtjHJ2KeCtUQb?cp;$(6DhgB0 zCewv7&xbO1a(2l=^*6D~3{G`2XmTGsypb_He2re^xrb#Eoj$HKo|wLhP3iQMn~wvh zbmwA1+cHHe+u$y;dA9oz>xY=6+8d>in!wS=B0M*BRFS?Py{3)Ei?tmn>8LM*iF01G z7F$v#OGcnf*!tGCO^ye=XotqHWm?_{9eHVD)*M)s&gLgS0tjMQ@vA|0@_gwwtjh$qz*>u#AV1~xy){f*EuBJRq_tdW6O3?h> zh3$C1D$O=!!%^NKYqftD(B$FYuX*%*vj_{ z2D@&}zX6`i=_DYakGWD-5Kzu8WmQ)Es6OVCEA2GA7AHIO)=bQZJ3F+{b1oS*F_z2Tw1yeX2ZE~1Cs!mX0+2Iot(a1YBYX$QrEmiOzF2yG>BLpZF zcFU{Ms@-kTAbC-0WXW@yjaCh>wFMuP-Z7SvnklP;3eP=&K^pf=NriXG9DI`?&o;@H zD1QY}6w`YK-G1wG*BL6vZ~h@5P*c=7okp(4KyJ z4@5P`GxS5s;dhLq#UaTm#zR=!Cg}SE+K3C@-qa$~#1jHX1Wt5O_ptnga2 zl&0r(%pO~+lyHz{O3qD*$fs=zkDr<2OgbrhV%TGWMNLa9GO_@9FiHi*#dDi8x5zpA zC{{?Law{K){iWO+6W3eSag9ZAv{sr_*O;o)=%H$>-pVLboN_~(t#;qr%6MkP2p7fF`VXOFM8W`GUF_EzZy@(?exU`R>|cT z3b-!RbYd{AiCz&A9aL^qjp+cjGe)OlgT`wia%Gariqsd5f&@v%`OE?mG=&t&C!b8Q z1ffdY$9~=RJ-=~DkWH`cx<2V_PoCwK9 z`l=&i7Cx9McqZvdsud^?A)M=La&VptGxtrR5r@Lp#SN+pIwD|KQ5nhesU^ejxf!%> zFs#uNZfzP zjW~Tixa=T@5Nix~Y0__z>2kr@9Pja)hB7B_V;d%e9p=h4-O$cX!P4hGY;i@*efkt! zH5J#LsQ>Qty}Ks36FyI?OK(bQ`ripgUByB@|;46&Hm96_X52bQd z_r=7enj|>V7!<)>PQhc4Tpf_&lIL{-loY%GQ{4Ug$+;kRjxHuc?R~kQ_=^qZ8k-_+PXiHv_F9*iQ zen7l{YnuZ(SLM?8}EJoD%NCB9}SH6v4!x3 zukOD93-a$@tK*4K&7U@V6LEg+hRPd}!Q!nvyvHRf)hwuAfUIV4R1gCRd)T5VsrOYJ z*XW!0F#D{pt*K|NJp^uO8wIQ7R4GVA`>)-&jX>y`?IT~&$+P(a&6xH01R=efst#ZU z?-?7fV_M^D=8iisxrEtuWQy?1GpL`qDOOO-9Vu4WUMJfDtJfHrn?$RFj61?>au}6j zB&MdJ;R4$!d}$Sh2XL07qVKnmRG*Gwamnc87Adb{uUrtwTqtY-2LdC4s`irWBrUoVgPwEmK@V* z0gC7*6;>K?WE(ekn3qu@4eskNhIGZcSmb+MKy+6ze@KXz8!_o0C^q4-aM;m(L=HOI z2FgIM`tyDCVEYS$o<#B(HHIU5a*M5`2h>$*I|WpONd{9w<^Lp>az;MpV|06tO>~UY!g{Fq(33vA&c4JDJGg7|O>Way z+4f~4%u|DzwRZaP(jL%P{WCjkV%{0DI-eDfKzenmiEhta95{ITnE7F{*!L0!-m5#b z8~?r+Hd!v(`#6qc#xMtfo0qbFzCI+%6(raU=x%%YSmd``m1>nb`QBHd4v8YJ6Pv}ncgs06W`dxVw9N#N+0 zo0!wSecbl_OioOzAJSH41U3aSFD>*Fpt{idus74bEV;EHe~WNj!s4v^s+QQz_Qg3{ z)D=B`*J~1;cyhrI`tjo-3Gaq!4DO=t2K7(7Us}B^VYr7otkv-0Dj)I*$KZ@@PX`uc zBwW1R3Lh7B$0m&{>#LA^0iH>(eSQ+aY+N8ED=7!%&rf|}h?9SZEmJLQNM9zuu<@{L z{AAvsWLNu|j>>I(2kdudhii*nyOM1rkkWS_4QiKy%1t1EjXk&E38Ej10o%s#sd>rg z+nbda$38`SSQ1#s;^LTY0As&VZ?i-!1T9DCqHI#$_qAQ=gb` ziJ!a2Y`y>zZT9+23-R!Ymg&yJax-x_%?yRt6z0i%>@vNI7gwOobJxwcj%HsSyc%7f zJ&F}M_f9Da>7DPL*|XE2$Ub|py;yhu%|~oAOk&IeY1(%$Wj(4tQX?~NocFZz<+Ocg zcr6~2q;;X`N$LVR-^KRT9QMSc&5p}o zvvqAE&)MxakGyCK23aes_F|t_NKy;FslavUwrxU#tnBEWH0GR{&8Pq$C6pW)xs?d_ z#@+EOq(9p)@RTYVKZ8vhn!|$BN{xGSdtP50K9Wb*i}uR9?!5$Y%v0XJb5ah4&Co?dc>mz^PO>4m zFB$@p4fe*_Uuk;NW8*wQjwkV;0=virJBpXJ>*8xkhH-sGSp z?uz_wAFBJBvA54HNV0c?m+q#!zW06l4%%JpGAuHUWao~cr*2a_h1$wnA4KVq@4DNa zD}xn{RuX@H8fQV6Rq&j?IhjSP?p`z^ZH%DnMW($LClcDY&a%ep-t*aSH@*kKHE`od zF$p9~v2Sv2~EDnN%3%%T((z!{COr0vedeF!bmx^(3cE|;OQ4)AAG?ZL_Ko|O4WrMyaVJ3)AB zoZi%da_*wwKbjHNVg71;z@7D+BW`Rr@GM!~Y`@Q~Cuqna=?$1IB;a-DB>-=&zfAL~ z-C9i(eq6O0oP-sSu8MxcO1-F0@TVvbvOXPI)Tg|)51G$yN1SzUV?2vX$!#*)NVw3H z)!dGjL>?))!LTLr+t_O~r%g)pocoSU4tIR$i}$T@(PDv*X?uVMgrPoB`&Ld;Ps7W+ z$#cCT?c6)! zR#Jt;HK&(0!rF4|bsin(hScA`PE%qA9}hp%QFW7>1CZiSLZy^R)NWsQS#V^wZPO8O zsa5k6b3#fSj}}9FXA(F9lgL?4E$L4e2wBpc`I#to#Y?{r>y`yEf2iP!&nm8kaKb^> z4cdKOtTk$fO`OO5qABMZiN59qG-zx<-8;N7ir@VFwe1254n^Z9r-uu0Jx{xX!aPQM zE5QABxgMCr$jTFXCVN078J<&?;&b0P4?5Cnn^r-{m3uzcOEqrE?0#D{?WVW#K(~lx`_pr-Of%=#Zd6J_pk3SS1?^jfcTS8``1xq^jr43qfxNEw&Kf|5 z&Q145JNArJ@kN4Wnbwfh)pIvgSklc+@b}nFk_v3mhM@Bzx4>)CLE_zoEtca0Zw1{y zcI^r_qQQX2hP^8dY_n*tdWoI!%yVq9zZx>^wzgT8WNefQgm?FscYC_UN2{r37b^Kg z@?KogWE_n}K3IX$bXtYY3J7_(Iza5#^R za%(PG(mZE+4K(eHn}*7x0vKku#%g zx2d?^DdlR=P1}%0myC@V$@45b)Cjp_6O|+++NqRA6k+=@PR@58c+z=)S=x8^qa8xy zWV~9D{pX_E&Ly+Mr$;{M+*MesWt+TnG#?ps*TXZ1z_J_ei7{G`Zu{f z2USSW8YFA19?-7NSH!tf)Gh>Ac9W(_Bi=9b(wHoKrWqeGXF5A^)n;LCYJQ-o%ah%@ zCBrH-BEs56vmsee3%M}wRd7J!A`CPip;9ITY*ibzp)~a~{CR+hqdJXJX`}1kq|h=o z;44{urrzxWO*Y&^GcR0C9B0Nb(03OZD@SYP;%!u$EJguU$fV81e=Mc7k9U@GSdaO% z>+ZBsWJsJGpX_xKzf;?NN9#0lqSbb2OeZ#jNur|3OK6;3ea0^dkWq29iHMGm3ZN;i z$?K-b1&mB3ka`3jSBwtQ9|)B|V8fn^oQn0C!Hj_J5s$j+UAga6-ng6%oa-;>%)f&x z2tF#(guyaB;+VYCw-q&BgaN3MUd#O^Fb@NBrFlyz5W!1gF4@C3;|clotZ7Cw@4A}j z<(OyZW=h1EC7N=CXXDSk+5sA=u1hQCn@Xd!-QjKWJto6>%(;Mn1T_}6u_OBIUvQCP z4>q;6FvO9h#BB%lkBK_8pNV(!p89Bxr`rcl+`N&(eDg<+e#sBf<#4x3wy3s6pIqTB zoasnX!;oY@qyhw#I-ot}^iuf`({`h%xaRQag$O{I??s-XxGZfVMcr_DkTHjj@j~~@ zD%lGlo`UfO^X8B%YyMl_=&>=2JFz90fmS-1-r_Y)=8`u-RG`@-v2|X{ua#@Ej*dVY z!tn0aIP$D%S2Jrg1qxz>-HP1wRq9y1KOSn|d<>SW24IxGl_3}y)S=b z3Hw>-yXO}t)}wE@>6R5>qvE%-IvRPT#Fro1cLN`%XENp08CD~(6v$lAluydX;;#_; znedM|wo#r2<&3iZjA+jbV^g!|k*>`HRjQtEf>mdT9Tk3B6rQ0aq3skin!T2W?Qy9c zpgLY>v>Vp@^07f?&*RLRP)vQ2I-!{h;QS5X?OWXLnmJdCpi)hwcF1^U!BgUqVn%5H zl&UA@rIzI(?Dx=VLCjLEIb)NyO{w9asn5W=HqIXHGWTMOR`W#BE;YBd+G-0PJN6ol zDz+RVuXf*x;v#Y=3O2xu?yU;(YQ1RrIIRso21Iymp(}iKUEZ-@&VMzHC8QbZ-sqs$KLw;8eyO{N)@y)BH;K5 zRDqi#p`bojlq2HME}jPp`&Q4+uN`S~ELI~iu!*rL)vW7zCU~V$)tvcD1ld?hoo`_! zPH)#{z=v-r+O4lYD%jXG1aB?!%4OaGf{o-N83o^B?`h~Xu;WDU>5+`nFE_LldSnVi zP)a$DXkJkJ2n7s5Et1z{P-XxygQA8MBR$aZX%oDK8Nw><{^Z&Yp;xI19G_sv^ACQyZ&4>rq;H*JB0`$dHZUFS-A(%=kX#nt~#OyB~0f>U{=Z78|(}HN(;X zg|K1o*jTI|^!7}n+N*Af+jE<$Y!>5A{UEyuV!qqo8-$&Reeu%k7tRgrG+Iyf+jcgP z!3hT>t};f#d2nn3pp*UvhW(scpwmy_t~96%CLys!1F=$WIXlrg_R-8TeELuB%#8xS zV+}j9#y7<<+Y-dOaTmw#uxtSa=D$naN&eBvl>MICiVJ3-dBS4D_6B5`>&^6&C;gwi zjSQ)ABcl6i$_xg=S~PE317`A4BqrCs?PO}fu%rhlfF^=|;1NL6uB@v7a4*~xl3L$N zpfXY;rjPXn@Q*~{{V|`H31U@h z_E^Bf&c)Bl>71`yQzU**3+am=${kuuZ!-4Gd&UMt4o~3Mnb~EwXG0Sh33%3VJ>hpz zBAZvGSbU>H6(iXjEA-Kp?MdDYBJ1TI>n?x>S8mYklC9f1-gq9=|J(dhg|^3e4NDX> zHOQ{N8V`wFR39j7?I1Ie@d<1ssE_pnN)v${SnNyKBqfjA@s3Sb(C=Sf{ZK82&U$BT zrjeA(a7Mn#h-wTo1A#zLcNMAdlQ;J42=04uWm_oiMS_8b>}np|OB`}~Q44(Fo99Lw z#Oe<8doRpM?*;TJlK%BLd~D{4G)-Ul0lk{$I)D%@PN``bc#z!+TKzE-SEF}+TjqvE zSAit&eF#YbF5${^*X_s5`VJtw!?Y}~M)?%=wK*o9njQ(YHcgLc(s`n=R!xXj@xa@R z#Nz{R)hna)(xmziYFT5JN+SY$Fo8ZsgajwK8X8Hnp6^?15Isnp+*jMywKU zf&e8xMC=>lw1CewCxtJ}CJo*e*GA2Izkj-AobQb3r;{@oRXtlrThlEKT%)csdeY_v z-DZ`9?Xzeo1Ub5(!F#OPZisH-&EdrC-@ubDMWS=QpatCFVe@1sfR0sg$t<6ugO5#& zf^KtNk1o3|N_aJvKa~F+X>P>|*=00ycOZJ1wAX|QrUCbydGghgt74QD7EO*#y zh#4nM(+k36;Dkq|Ij2ZG=-t*uBYzZgZG8M}{zNCM@6A1ZF`y~s!suhF({~kgCh@q$ z9`tC5DeYp2kG0OP9UaB-9}%7;DmY z`Pywl_O_bWCW~1MErVg(d0Cx}ke4WPdHO_^pqk12C$Q;GKO;>0>fA8X5V4 zi#fB7(txWP#A0cPD#50YAX>%ingG7&)@M79fRxJx1P+a(^gU|d|GQ_HjMr&HvD@MI z#-nAtTwm3<809+HmwHICnlUHgBY=_!i){Q?$o>h88;#FukhX_UURaN8D!R75G+TDA zVbsU2BVUB$7NZDlRz3c;NKDf0rdMmt&t*;=YA${<$xc{=z`5UT7Kpe}erQoMr07D^ zkq$%k!w+?>@;U>D-|utHDl4+4X5kD=1mpT%8Ybc2 zlQqOnHnwJR=E{3|dO~rh?iX<&0`E5r?ju?K7N-Lrd(s%aTY3B~A3J7&;~4HPtZt2qA8T*ebIxjK(iCPZaax?gUxo%?uLzId z2o2OfxlkpkXX=s2{LBvYS#b<~X#5>LUR}G1S8!bRWkZ2}gr|*t>2=I-W`Jj!^N}mF zAU7#@&BLdWrdV3666}TP;sfQ-O&f@;h_3u(365?Otf(r8o)b@t;w}bpA>=08oe8Z( zJ0xKbI&jZ>=jsD#S%Wb903Ur3feXJ!gVAKol&XFi&CsiGEk4bpYJIX_)FspU!i2)t zdm?+svKyJqhY-Smm$K(1WLsR4PK(~?=X#Ro%Y#?v~PS6Au zVw+afcnLaJ0Qx`GUR~eGHIsD~KIwd8_hCCRH_&}Z*tB5>HslXkQvt|UV8 z2~d=hpa$&kg*kOk%oTUY`4m1jDQKMVAf=t!${x4znIQ}9&E$kE%jxhAHXZR?@!qI; zSp#L%WNoZ1u3PpA?BPwj*5i+gN3Lv@z;H%+wV%^Lsx!7Y48)x_?Kn|wyszc#a7w2w z%kh%~?am)=)+HQ%P5sG17bkMRQ{`w;)sFa%R~h#ox|wodre&fJ0MoG+mVv?sGx zHq0Q?+Umj>=x8nHPKw!`cAGybetNQafhSmI;ucN2goc|OwCDGRM<=S*eax&*e#8PI zBG8a^+1?9m#38gCdVoxxeWWv!n%wAXA$xaZ|GldS&+&l_+a}&Rx5KqD61oHmZ;*B7 zM!)I|*l;x%kHPw%jZX5zzFfnMsfFOfUSl&I3Wh{?z84w9u3Z(|*;(L*Lr`NdftxPN z(##=j;=@tB5f@!1JGcT*rbQUu-zJKP_Qax7u=OgU(Pr5Bj{#egbyc>-p1aiGJ|x0j z_cZ;r*m$b>Knji$eP*DjuAu-4mW^GexIX*sLI@^}MGJn^H-ooW)REUq!#*JYrZD89duHg2il4@H(4d+XcsipN-6@9huGffw|9y{cl`NrQ1g;x)$@P8S@tZ|v|b zZV<{9>CYIL!6uGfWW0i;^1k}eEmjU2>o72P_2DFhsdw}#mrSvl6}$fAv7B(khEe4@t(M-QKE2)Sgf9H@Jh5$%yQb3}@olh9-rm0X1+S}t9nA0yYG zWavETL#zb3H$l(V=4%M#$LAYew;F+6S{IvE;a}}^wDeXENY@OjNX1T zF|~qSX&ny~@_i~ft}Rz<;1)f4{g%Du*Tp)|s1%DJ2yy9zp2_aX2iSeS znr(#5SF!u!&))Y^i2xhTnq9-#5!mAs%)PrWck;!$)Pyg6(pwrxGuGx<4%^SQ46 z;f{0OSKCcQUKZ|8xtzz=)A0VK8>mo%T^|*on6A}3kU>gZ^eD=!Y!RWuE{w|#63UV# z?|VVcRHXuJN%Ja*5+2PuOVc=m2A~;CgOP-$#@FFdE_HQI@LTQ1&S+;%aA}f@u&GM5 zOC9|TYc;&Kx_a!x8IW?_*F@EXk?FqSg&IHq+;R&q=v>v4Iw6A%=-$1MGFj7?1&1>g zYPNx@*wxO-)OwtYS>Yy(LxXG5`i`A5lacwpjbp#J6x;80(i5$(NLY_qTm5n)vs&7m zO;P>PB42^=RL236c4?`S>w8|dQ>B&pT5=m^Jur6ba6#3-zAbJUCqmbd_?rQHFtq^+3tmsc1v%@Z?rV2my9 zPkYj%TqN@@nbABCSVxml|6;sin8{KE5f9StP_SF_s-{L`Z9g2lX5DWg)1!TT+fgoN zV{uUX$={sQGCQ56!46JBS~`OUvSwn`P$Y?1KL!*xhHuti7O1c#No*ujxp~s)g*nfNqxEsk0W{p$*0ezZ+c=hB<^8^#u*k?2>aOvzYkWmYd%M-nz~ta%5t- z!*)$x-1dHKj?aV;Jow>MdNK z*ivi)a@phgLL8)?!ma-%D0XOy)Q=kg&NU4jT!o(Q1T`_qR7~C9pQmZH&=o){s9ZaG z8p`ppCtG8_QwmV+w4L~|K#%L_0g#R7Fv|7Ri;>PbUS7zgX{N2$J5sf#o=Xzwe+>YG z*i6J8A{bC`g|zj1n8v!S9LK0WoAH+38W!W6*9Gr;(i2Z7#qJ$vbfEZS?G0$l8jijuLK@<+d?A>N z+qEqdB@rz*f>ox<9TU6+%bmoO=hzG4E^OiK z@J36UZD0s})%(t6ZPm?B-t~5Lt0@aX!q*F1-AJ62lQEm1XDUvn`2CdwyKK=zCEfZe zkbQdzqGQBoxz+44IBQzv3Y#3h)ox+Hod|QE2RbYmJQ{} zcxtLnA{zXgsr_F^2|gY~9d`AZ&)enl(cfV$_-=q8_%=XAg0e(?#Xh5Vw|?JzC78V{ z1DK`NsoTVA5u2g^2m4l42_bmV`5o-F4xdhs#e}ffXK!9`uGn5adpotrruV1;yj}x( zW~lY)@Chve+KUHtvL6vhrXFarY0}0g3-iqR~_sf?BxbgxVTXc%rmkrEr)>k-jZT8U9j5}4Avn;e_jHiFGr1Glxu z@o`?7IZ<0YV|$0oHX};9%jUW8qG9Psus0U>;N>^=PRYxzMOAOP+tlt7IS6W>DB4{i zq1Dk<6&xy+5qe_&`tVY~jcigsHoEd)wvg)tb!~ z;p*4MA2^$f!DMci7<8YSM>+k=%cUb7^$26VCQGnA?Q{QtSeLUyK+cR z0qk6&b(wBi?9K6aUmJ95@>Y^Pn)k4lj7aVMiQ8`RY5i}W$sTmTy^hiW8C}q_R!i&j zd^ib-Z@Z5i-pSRvO?>a>l+$Bk2H?F}PM%CR2sL5@JB{+(`28n>a7Gji2VSMlOv9y~ zp1x=&sN0+zJ$cJfh@-o>cr28eflsF>f9pZry-hNbS!cr!7R*8=^4-#ejf*5-6-`=; zAXPfW@p%k##VQViVxT0b`F39 zN(@i5eJIBjSDkOoPIL<2KM^;&yVu;wgGaK%L@__LbkXHb&1F8zler6=E;R_nI!Xsd zX#)eBW$mua%=pDs?R}{7WZ)glaW)m#*kl^UZ1GC9;=V@WwH*v-k#qNE=LeEeT}@_t z5-rt|+#&(#Vmq?B*jH4kMMIX5(dyX`wuOCM^^SEDZ92Sp>-hNU*btV3Fj}wf$w~V% zJgy2D$Y6bomgj^4{!Z?RUfI!mAb-FmJE5PtnF{27pO-OnaX);>as_mHJ!dCuHXgY4 z|M+_Auqd~-ZCnu*R761O5Geuak{Cdcln@Z zzBNA2e%|N%e(&+y|Lo(~?(tsty03Muwa)82&+8wlCYrF8^`T=gw);7>Q)=l^Mx*^g zZ5h+C-Ax`*d6eq>FABkI(VOqMfV#WeafrR?vwy3N&5KDB{@`Ju+!N0?bky8;bJCgIa;DfuVYch7zk zH5o){Wi4%N&RgX%A7SF#Ah9&nkv}yz0#7-dVrF@L<*Px zU`WcO{hOMsMd5qpd^5%K$#B!hUVDJgd{(0KL%SnFIm6B%I!nrAx-;SxBjH3hP5{Aw~#C zikT}J6yug~$fin4(ZrOJ!UyEfgZON|N|>ckGeC~QzFwTquZ|P4WH*wivb`(MB*lwAgdey)&B=!{`WEr%cS5_WXhTlqO#ZU zML{2=46Nl#3IE35UbY!Obs;En@dTXcEog7Cwg%}7Ez|Us#u7y;#<$sDaJD5Z$G#qW zoy$&@=qT^B6GbYGFx1$3T{`7EuBV&%CiIFlBArp|Y0&^M*-gR53y$S%az3SBTnb*l?deJHQow z-sFTZP2bQhFnKe{Z<9jqf+$u737Oz^m<{@n<#yloxPoUdZ66!61uecTySi97dNds* zucSvc_;Hf<-+`%i3;}s3bxYUh%9GKv1^cJv(rf;GL}`rs;%DXhSGRIqLcyxNaZ~Sh zJ!*0bq43il6JHd_tPG<6{9dQ7!cER>6wV-k`+wdwu&qZm_HpfrmhSJ?IkbW31bLQn zmpU0hUFvA(g3zh!V*meVbrzymXN#0JSe>jHup!#xFSpokzIUo?M6r(ovbAPN;`xA7 z!^KbX*=ogmss&$fa601o+1; zIiENrGeCAQl3!<@10+PivLuEm4leuI{Pl}axVCc2QTz4jua6YvSPGRVrQvxQ;}+tU zJ8N?Gzml|lL1XmL5unk9D17{D)7J80VPs&8o$Tix)=O$2CKu!A=Kb@Vi0<7d?=aMz zLc0+f^1=Z0E{#m-u9DO#Bn$uOE>Nobd95GySZ@yma@sP$Poin0wjr6Q>x&ospOHFs&Rzee=k_UnjPT@@KJK#& zBIGfDmYX1|S>%0LFHCwozn3nk_TFia_1OE0f6Hm+Lf;z^&7XY@*qzFi7OMH`<(X)+ zf)e0VCa9z7kLv!$Y_c)<{cOwKnHu7-t(eTqvRs!?Dhau;LyQ&w@VUN%1J4YjD))^+ zYDQqb6afk5$(B0$NL`KE0)f`>cTM=&VXvH;`P4X0Xn|woW8iP%{|!{8N?$9Qe$-q# z3tCQfnJdqN36NG=O>**IB7tISTrXZl5uzGF31Ny9I{Z=()bq~6;0?d|-MTw<;|dIS zdLF3WwfX@`)eX1}{CFLVACr{C1Xw>YK37NSoPg^iDE89h#47*o_fMA}fg}O2bXLCy zQO4{{S5H6UFl@sj(?nvbSC>u;+Aa7}Xpn=CHmj@f22@2NCKUi9WfeG%#R8g;&RUXP zwcVoFCa^;*QZE-n?%lzqd;$KJ_+P)c$sS6ElhOf3*%KT=fRDx-Kiq`w(#2~(qw+#V zKW2I-jphpr#$2`BXX^yjjukTkQoz|}@wq;q-$^(BlO*EP=*Z{i0LlVr2fFF#Mu3#A zL2=zEm<~qxN~=<@&W*7)54`7*6FZX?xtF@Bb}iV>NbOrS5%pWrwUkn*dH0KN05pr} z-jc4<9fYEbT!1IXA7}Cpo!Mv)c!>OUKR`Qw0ZMReoCmi+9(s@XUFe~3 z9y==OL%6cO&X~3CMoH*-!!MgiI@>2Swvl-IDF#rZMLnW!aNe2X)!+Q00vrlffw{m> zhI#>u0tUE1yN04L7@drVLoowX0P+zKKpoJ+jT>&jm7xnTtgICu0#cXd^!c98H8Z$7 zhLEr!LV!;e9)*Vcc6mp(V!E)8IbWL3OYUsTKVu6;ey700VV7JLB_~Wn)u-L{nQuk(yL=p2VMRnScV=^$%eG$Gd;AW9vhZ z+N}z3l(1z9|6%6R`ey>}@zb9J>mPn~-AY!Q7LaX#f^@#vjH$C@zcBdnETFBg9a-a+( z4*4lk_X}Lf@Uk`va8WsabkupDBG0plB9V0mAS179ayC58Sp&?i5n9k)2IvTO1FMQSoMMGp^ZxJl10>)lPTmhqh?$o)Wq2uDa@DUW8+-9T)wa2zUSif4?oxpi`V zHGf@!DDV%&f_QU3Vo@(^zN+n)8@&JO70%}7TFU^&a8qoW*L&II`|OCYK5BwFi$sz~ zpzaCks`l@MS^A=2azOi(y~Jqw(fD?)jIF2 z*AML6A95dxjK!9f2uRnH_6)=;X8Y*5DcQdt7!#1 zx@8c#alb`maw#B}RO+d{z%J}u6?n&|VP2aErppoZI|d2pnFftK1Vpd7u#yAJ%_u$i(Tkjrf*^23`O!(NnVF~iypJ_|y-Eai*2~Mkn{gJ}IWP=PQc0V)pMaYI z05>OYFY&8|dleP|hdqtk_`szyI60zsBa>QQQJ1I2@~tJX<+=O_u^%B2$MF<^|1gki zO;uPOv&&_$GfP;=42`&VZ6o=_>DO%6>Pvzo?w4Dc!`(CnI#PMaIr+;ybwY^SSnQwP ztkBHB~Hbew9jz1+ymIwb8y3r)x-oZ@`Z=1&#F=l-Pd(*&ix$~Cceb5rnN&FL?8 zcxkdt`aTo=tKuAzxCbkdi8Y?RJbeQp;C9k5k*t4{tkn0IOUpPEThpmm0GdrD^t>ry znp9z!J-r9AIo(%`H!b%;XfyyuL4U)V>w9_J*<2s^7l;SC>j&WiEqa6G%mPWfCeq_x z!RzNah%LFVz=%|fsNuL!x@D% z$0)bdvrM8nB;mu$$@sHc9QSPmmWx7Yf80>$BY{-Ft0Zc2KiDPu zV6^Vc!ok2qpd`SNqJ1c8eI)bLVC$U=#m3k zGOxiWy!DZYYe$0!r;=3qxJ{Mq^@qXH`*#IK^%V1XCf1+8c_u9OM{g8`Mm*e-u8zn? zcIxctB=^h1~k}8T<>9#@o7?Inm*2pXs_Y7KZp#GRde)@BuBqhBD?vGV%cT3PU^trs z&vZLy>guLv2{o;SI3j@h4{XJEu&G=J^`-v*zxtOa_2v$a%9bm z=s4bq(%laY(>60JnkRvz9k;}@q8bK6??8R{y-JTgVrEb2>cEo0eMYqUD{?BQe`-j= z>}2CMSL#^WthBq$c~<&>RqXUZR!72>-0;Exz?raKqc5KG<^Q?Qf$yIyKx8hZ_=o%5 zCT_%DZ{Bx>(y_ZN5|km?4Ds$u1q>}{`Nel7DfQzytACU2}8&;2DpS;pXEx zHJqOU0>#5HrxA6j;tF63tC4XTdT|zrL$-Zci(-GC8l3U{l|SlyP1uy8Aq1S+t76ZL zwf+6()JhCA_PEBTHm$LskW|L+r&$PZOZIUOxRVIYX*QxW_e52S^mt-WKjthAxh)pd zFYp!@hsr%{qj9qnN-3HEZHi5s~St*ea%ED39|U6~zw08zHd7*>e{wpn2miR`mUycuE@9Rwhk_JLgA)e?5n6Xg4MH+@F6+eWCp@duns4)lO??AW z1l^D>*WCD}Owku2^jMyX`v+pquLnz+MSZe3wyRpIF+;!9P;YlOrXb#eE# zoeh;_uE>Ng zaN_-}`?zLaj$r(Jaam(2-_~XtVu3M*!3Ecw?QI*}L?9?V-#*;hXi9v7FE7nzi81AQ zBSZMv^46f4qG=_{=*9!F{3^a@nzDZS-7w&myZiLKGOLJ zXOv`M`|AcGU7KtLm|s)Db8Ptr*9A8k=1474u{H$^i)q$TkKSBrWK~aYSPQrT4b~!x zVo>?OWL(DINzk zgr}Vt*aUE=749?!TMs2nigI{H?rDhW!*zwvjt$a_EB?Fn?AS}Z50gB8^1R#duSSNR z4GjFl1;`ATt1iv*b+>0oYwsiQZ&Q zs_RK&@B-`(1x^Fz(x?(@?cDlw!xoKN5vm6Xree#a1f(vBgX+xzuw{90hMGD-ZSLB5 z&oA*mR39h1nYOzd&ZYJ0>Cj*3tNp*BuWWO(7&p7|c2L+e3OxvTNn!HPL3(`1lY*|- zdtpM)x;?V2EgR`!dJL=p8Q+hI?5fFLI6pc>M)fO5X0>da;+m?S z)R{s3$yL0S(yG zZ8X{#dE>v(#{a;v^4?oyB&y?j$H&nC;*P4vI6Iq+s=v2Q0_w9+bn)fh;s1)Fl}3&n zBY7Orze)Lf%_#Zjnt}G={r`c{)Db;-(BW?xz-1Ox;?#Yr=wojJ@a)lF@XYUj1<$G* z!wLj-@PNCrpYfwFrvPswF2nt6NV9A3M!t z|EC6(Qevx+wFNZg-Fq~W(m&E5T+YC#^=7o#=WO3I5l|<=eAB#{j#Iv^18#aI0BKSN zLLhRr@lb!XYwJS_fs0ixAny9spDOsKH4w*WQV0QT*6`7=;0wb?;CtD*h*vXikrlP}hNh67>CGxOU7W`8Jd)N0pjJ?&6CxYl zs-v$`i}iM1pMvt=!DTb1CN=7j6n*sBwvn;0@A-IeB*avzPo;N5{ft$I)_R&(hEY}a zUK|VJU(ct_5Uq7sUHK3E)St>+3D^;6Azn0cC`;-M;PqI!jt?E6MOQ$Y%;5g&tyFhM zBo!MtyZvo~0CagPXkY`NmNEbZSt+ibivg5gSEi-4f%l3o0pZk93+T|k--#>ypK@L( z{l9>8pLsawu|SyS4NXCAS16YlN!6K2$-UI`TuW3Q4=W-PK6s6L8VA$}qEWZGv#Tw~ z6#{XoSo8DqPp(Pf)mD?w`uZM_Q)q|)>~sYHAisV?@ZsygWfQGSMudmJ> zq7#dk*QKG;B3l01Qm8=DsK1fy|38r|Kn+pfpge)?>`gOqEqyao$SG_(l!fUQvuAC! zD?n5WgwC>qJi2qhLKVUAmKz_75(Wft^O>6R9Shp*CID%nReqEEB|Xe*>s@(DS)Aa! zdV~fLo9ea-3bR8K^RrI#&Pm{x`#p+YXD=`e(D1jqd9@-FWabI)x9}UsIAaE{{!^g9 zP0Y__E~!`EN-)nCDAtL$wymd5PWRDOI6sh@O57)nn*>vZYCY+ zm!b+hPH%&-x}V3-v)Lg+PPwbQ!6)J6?}YXC-!ZML$94Jc-s7@aVRXUSW=w7?A^PVp zh=*~a+ZXNd>nk-`J@@<~kA<6zpm_e(dRX$FTCs&l(=OlwSe>6JcMH7?0m6g&Cw7-+ z;=FZP{|Kr-8;PMnFA5W|uugP8>L44B`T8nYpMg8}@GSP`93_Jlj$Z~aX@V3*QC-`) z)_8oMe+*`dTJ+yDrQrp7ap=iUu-Z!YGS7aD1YZ9&pspXnH*4%7BQ+YI&a zddF&4V%`XqR#SPW?a`9GetHufIfPvmv9h?Q!Sa_tz=1P1^v`Hayj+qP3rtw+L1*xFl$FrF=`J2)+uFu_QbmHu!@X=BL#_GI*)kEK#6@vyG_ zrGWC|7awTo-b-Mh@a%>icMy{4MBjWRPWr}LX>c4Q0~H8w?Ax;on{!oTElsM@C&%WeiUoj_f9<8 zT8d+NZgKIavEGBPieyiW71hHgecXJjAYTcLQVE8o8GZY{o%@LuInP+38MowA`^eGL zoA_lBzO$!y5SlG9GQOCO^rEpgS*VMpGQ6r=s!DoyGZ@~|9oxlIno1c}H*$`8<(9KT zW|t$EVL#ryG$THgHsZ=NCgC!yD>vn?IPhM!+N;k>a%-Kq=^c8|xKbY&9hs*rAs^gi zwSA#&xULXzZF_1~;Z`Pu|J3gqzwCTYbeC2XgI53c%_K+WJeUf7(@U#>9Z}Vh@tYgm zo4cQEjJOq$)yd-}i@k~>i_r2{+VsnpFoS#*@!s?3o3}>#cR5O}rl;%qTf!((j>5@< z@52He1PfKgOXeD)pXmu3yYBLnF_tso4@Ta|wG7JPKl==eREzc10N=Xr8Hp&r!bRSL zr!UeB@o#Qf;!DSde5a)Mb2R6$89haYk-YqKb#77Z!m*{1MCl@b$4;qn$FzItI0*0hn&p0Uz+66-l?oC$0pzqNja0j!e zpMPy$TeQXCvz&3i^_D^z z9+T?$Dm@b~4n3B`9|2>J*PtiL{)mcVejE>Q9`G-5?GSx|XSnxw-{J{_qz3&~S$JO0NrGbE*!?c%}cHQIC&aHu@Uvvv& z(Iz^#`I{W*ocXN2WcI{SOU%_{8@@EIZ()f)Se^^tduOJ5Cg^{eS2}791{}vZYS=4s zzRh(UQW+U0Jd>}&AOwnPA0~g&g%Ght;M2)I=ehz7VP1vpwS2xm$^*LUNCC{Otha^_ zsQGV#-Y*yy<0awXl(oRI%z}65!e|Lvy|@Csa5)h8Y|qw>S8eWI^}AL@VAhNK($5NN zx-luAVIbxj>QIJT@E@VMhGu273lbRbui1~E5#$;*{u$SkHn~t&?DW**)LpKcow#}R zy)|eQtA6_7+PCS)9DI}_Tb54A{Ul^k-JO4e2^a=}T&lnm;A;&l(W$dT zZYX_a)gfh2OnVJ=+ysZ5JKUTnKcaZHfYZw>V`Exi3RBSbHTnb`dkhX&y0%D3f)%Z; zWErJEgQrn5xA41O&c!l(e68xF*ioX&*n`;&-^fefM2Qx>%eOl_G~uD!jyZrnqXbKA z48IH&lwUb?Ugl^tRaY+sspLqC8?<-a*VpEj?7ruD6fUrnGsD)}Y9D#ffphn$8;41q z$e-%C(g<1=<_bRdqrPxH{!Xk~Ihk}WvxmAqmrj+n$we~po4beC4l_{6hLD4|HUnvp zFW{diye_*l7NS{!2T2`3Gc-$IBNsc6E&}u!vk$$;Ot|0B-@Ja5A#)lTS7II zVpc18`uTt~6S$!`aL@8=u|I0U-v^FsY?e;L>W4>>9LdiUIZ}CoIGmnz#mrXnAG%k0 z%zF}$gdZae$6T%vdY#~K)M?Ty=Fs}P-ogLgSi7iKEUXI1L|^(`3B|GMgn)By{^rhT zSR=WOkoNh+1TZcOU$(ilojleVE8ydfYwg5)^5lo)KrG2A*4ZhjhqT?H3v9AmY?%P| z2-fte{L#Ns7F22Vc|oSC2>vP57M9tOvG744_7i3iLo?O;F)jsJoOMDr*a2J9>$FC% z=uG|J(;+qXix|8s-JgNiR9o;+mi?dWOOX}c{vOnn<;%Jccs9iJPa6(e_2xQuN5Hs4 z>LJI<^V#-V)lt%C#Uk12UQzZ-?K-@vGa2? zrN}bAZ^@VfDwmZm=)y|MXH&!*8_S4vmUl5zpk-ljeS)1J55n3iGb!E$@f=#z8WIt) z_5r%a|9fp(+*hP%doks!W=Xnm@my+J{~zkK_KCM#YcG$E1}uI-+#<+p%zK5puaC#2 zO=tLAgw9{Iz_VY42)T{09!TJbN6oMwade!i7DiQho@EE%BYAoYGoszLFu(%naJ1Iw zkMDA|Z0(Gu8KtiU%6e6mdBs$L&%h&}{sY{E&x>Od$b(xz|EEz&dqbqj+9J@oe_M>_ z+2*|M4a)QuQm(tZyZJWl`ZCRrL0ZhwH>T#mG#=3&)94yr1omOn@dvHS&h8=~*TOWj z(8;-(6vO+X+!}{*T^3><_A8ak*|GonY@L0sI=~3lKNu9|ly|6m?4ect-fzcqmtULH zJCBG<_xINqMv^Jqyx#;&!s?$S@ydHqULr~-Op2UXs7sx>c=Lav^mR4lCngU;PNJG# zv5G5m#(#Q~0pw@FSF^cJ-D`}T)(FtoK~?3KVdL|TaIzp!^3A7^LwBr7uS)BCcGC=d zKPbQA;#ToxAV=NvUSgI={t_}8TaG=YCjBCjS)C?f2T~7r^2-&16`tK6T@@xOr4Gzh z-jI@XNGJ=;aiSi!p($y!Q$PV0=fm#)(M2gBe5YUM4B&~qf&Eefd|$>x(vcoES)A9V z1Ullrmwi0SwER&jvGj}CjO8F$`MC0M#Yx&Mvf5oRax?_8Itxem(m~-OWhrT1U2Vq z=N!r{!~++;@$MA63V|?r2JmGC@~uB=xn)(8yeu3}x^bQ?vLa#tO!CWiey$3}S!yM|BP}s9?zx?tqu9Y=lrM{9Ys&MYDIvq1 z#utn+>H36VP<*EIL$=ctxhKcVVpglA-5cCCvb0XCj|&#F$4g6%`?g5U?tV$Z*I?GD z_X*#5v`!qH6>!YzNDku>2Bz{@(U-|JG4c- zUy$-jO)Bx~pBS-EFK;~`&62Wjoe+7JFO^V!W^>QT?DRs<>&G0H!Rg{-k-F(FlyvhB zIDyQTLn*=26yNd^ewfL1lAs+7?W} zV*T={Te&`hXcs*1!?4Zr@5xEGqo-uAlAJmNV>ZL@0uOmA+xO8gg1|x$2Ty>|#*K?P zErr4@!-(0i6C}XX)k@>d zu`XCqe z{AQuls2i2yI}F8iFey7=QoN5XtV-4K_=?%rT};1mSkl8MggC7yf5xpR>s-9B#bSPX zGm-%81~tAm59cHGu58x&CXh65p)TfOmckF)KRy{RJ+L~k#w;RLUH6)q50>ONp=W7s7?5^@UO~KhcWD zQqr=gr`xpW3G6LjDUfcF)i(`S)`NO+Cq8OjThe!i~iSk|0C-(SZdubkp$*seALk z4>p$b)N&}}cGLr9SQr-lQgAam6e0UNd-DZHr17?4!cM5C8tR-QRH{@O!zS&$I9&E; zQzo9s8L5}9TTkhW69X_b?&&*L4Atz-oyU-}mVTtZNup{yQAQ2uC*+3@_v<6;-s%&G zA7(YaR~*-kr^m4&wbi%Vio!dF4-Lhs+VY6>V3_@;UEl)KAWSeK-**6_Ecp*7p%1>R zHPW)cAf4}uTnpD$V)VCa`Gx;#XSDF$-Sd56&(Rg;5%)4e_rBfFa}^#&Dv|Q)6}C8m zFUUW+S+6r1=^Cly{PN7Cn{!>lF@fWz2>AY>h&P;DbIarU`S4CX_YI1)U0>Kdx1@1| z^zl>QpeC#tcHR^ZU?3tscQftO-eU0-`;kI1$Hxk6KI>VJZnSpSCUe7q`>YVN zyZ9!hQ6_TXTb`8E_Pr(Ae2+OiB`01n)W@E8^;F31I(92b)#Ls5b{Cei@(xK8IO1Dz zkQ$};?QvUeHTuh~&g({|54|BA>QFC6d3W|_Zu>KDCDFsoSa=n%E#*}VlssLoqf@J? zg8mtnJNfy~Xp}q=?}wSoL((_>ev0Dn^Me3RzrSsKbn$0(k5+N4_^#g<7n-u-ao&~w z1T7x*v5fWJ1Qq!=@zF>+iugPq5wAt=pU#~j$Ft*w(a1Ziq!~(Ql3Nv2W0$^4O<^q; z&PV3(W9ejH&BU)tPIDc%#1NCQdA9oS)FfVQ9bSEY?&l17g6 zz5e_L^zv~M7%~i^v1yLZ^_+MZ^+Q||(obD6`~j(DSmMA$@%^xLc+i;@xrW0qrSl$WwIpX0^R=c z6YTH;v;Gtk3IWGvAX9(b<9lkam+PNYPGfs}A)4K92WB~t>2ucZl<+Lg{hkCN3nvfj zT0s6w4PtEC#2fPNF+&F}ZCLw~>BQ9D3P1szBIJW#+)@30c#tc*-c>yoHv==G#sWQ%L7#vw!Dmik$fj>ib zfAn1;!wDN6Yv=h6wPMZB)$`tDfR!edQe#F;W&(I}5Ai>WbpT+-ZmA6$?bdXVR^@hN z3Y@`ikn!1s0S4sBtjqkXZtN53yczwTm?OaWi3C;dMZmX`>s}4k1vm8IJ71fzwPfRA zUV&HnAw-I0#LPLCrI+)ES#OuBS2VK*;u^eXM>2~S%q)Cggkf@1i^i4&4=QjEztOu= z*f&T;C-DVZkZelznuKG>_H+rwH8WphYeOBe0bJXz2IJP`-)@XdhjgrbI6d>pRf&${K)$#u~%p+4M?G30`O%&i|nMP0qdfd+^@wjHobGIhGo_U?Q*A^PDW!|nji8~Ao?U{vu+ ztH{k^aXE+AuLDSHS>EMEnb7Mz_*MUG7qE-o;$owDd!Bz5gVgJsSv0F{iY4Kze!qdA zYN$EPeF5;;G6N4#g`iPu2Z;>Y3kVQU!@z5O2hF9Q0}n6GEzTHk6w;gr{}SM=-+@gK z`t~5;vKvOu?~sSGBq{xg0`42CXP_n6Z}hd-VYPb^7!~E|)bYY1si>H>0goh_(?k}z zH%|q?KBaY;TF^o0r8{!t`_@<2z4yB+uwTaf09}p%cCF7#fU~H||6HcUEA31H&ij|$aj zW(gD*gvD`o zk(15blAY#Bkv})w!i1JyfiKSna_<#@Df0}SHgNOC$0&TA4JIl2K5-qsb%Kuv!F3^!Cv`C-{W?X41S+Jgm6QRz}Ax&V!V z=SH)89_w%%DGHy_0=g-=B^+QV5f4Z>^?@Bkw_T`#9%w68pk?~-i+S8#OoxVxx`f?7 zl5n%Wq#tsXDKzEX;7) z7*qLnPU87NbFS)93ZU=zvBR^rq+BG%e8~Dh!p-m?=DXp}I+NA7pTnydW9Mqh zQQHe1dy9Do*U>P}pm!3XhG?vDA6$7<3Lf*$xlr9BUelSw_O4N5Bj24RYsaAvPDJYG zT80vG9%WW5U3o|fxlm3&u^GjRgG5KSFVwhr#trBuw*R+063F1u;u zS8j)yBLAk@-e+A7(oeZT)P6r$9>spN5T3RP;#d@8m+mPH2^O)cWwS}!d$~+G+<3-W zL-j+lwbycaPwIR^W#aJ;!2)L#H7r9L4CO^{5yyI$8UO>y+BWJSA?~Q5LqWcX*v(zR zFvd}b267I19m(1sVOmt8o#NPuvXor2I%82i_6W_xfU)eJFV$h)Jy;FkO8ymXiMs&4 zjM^K_Z1Tii?`ns_clnu=#GU^B2{Zr~*#LNwyE=8xeH+Ni_kdrsq2NCMGaa>fw^A3? zFD?fS`x0a}5b+V#zY>Jg7fP(szdCU~W%-4jvU}!B1k6qR97EC@M$XBpH!(PG70bH% zCatrTh#yXjs5k0$2RXUC%k`yQtE=ep7)`Hwjxws3WVmJ7mT&m&&!_lU!C;G%{^M#d zhUuDR<{jnhg*h?5ZM1WmbV8ODXgJkg^aZ=JJGF`6P?Yk zd5wn$N%@deP=}BqW>m;}9v8h_*|~FQ$Fvp|q*~gDVwy|k!&M7|+?sLEW{-M>SLmjzy$l%m|MDl(>>1a z7xZe95k`{m_7|NfYZ8-qoE_uT3*f>*UM3TN*!HOBt33%M-sv5#}}L}Y`70s{KUon!a6(CYT@;{vfpGOv{CZw#%+4Mp|MUI zoZ;TPGqp?#lX40S`>}wD5*dJ!<6(3&p7+BNz%DKY?oXUnK zZj||QJb`4DJ71T5pBI7Midlj=1hh%QV`C~@BC(JPhR!BIjwGoO z9GgUJy6<4ByvjXsEXe}vpf5Kzh$7aVpM=_yIjc?|fjvwe=Q!u8P-C>C+lQyaOD43} zca6GFii7HxSitd!7&JR^SDgi0>F9_{Exq;BFYF{oj#fjsR5E@K=vQxHTa>k^!>ocn?o|4tA z_B;oMJm0K@;>Xo}lFN;>yOcm?Q zwm`$XG*y&9*VBp3DH1NzcctgCv$M85>m1e#ySoYnT~HZOJ&>zV7Pf9Zmw#4m`MKYV zV#v@oU&Xz)tAFtdJDEWR(++l7lX*Hu+>J7(Qea(M%o`9p zo_=5#;R{AL{_Mz77=U{ds@1GEq**&Qf-9;{pTp=#HVIriA6fPG2I~hOc;c4@0tlZ+ zTZ1Wmkn65=^Lva70~2N&YbGPzGc5~|EwXLLQ_;t|mfIIL^=nU~RCXX8u3Vy%mg928 z!}q*%?sT?DixPl$O~TrF%>~|d4^wJP2E*@>zR&#t`~mE^8%Ip)hI0b*ktoffoE=!* zbXGVYPR)+A?RK?Y6Nk=xuMw~e-s-y#s)Sq>>fqi$`-t{t67s3v0kRoe{Xl}mbw?-` z6Zx$9Dkpx4zOlL|1oq->I#8}*vn{r$SPE5U_O*I}4cZ)CvS6tT@#tgp7Wyi= z9li!P!coT^d5MsP!~WEgA^y=@xEVDEIT#g@#NOZ>e-|+EZ@Ee6fDQbchR<(TGYBp? zJ$ax@IIS8zn&9qL3cAw#V!$eEaucH;xZPEMm}30uww%|kjwjY(?0ek3Tj3iU&1^q6 zdfAPP>%JUNp8ZA+M#v})NpfgTDOqV<=$S8lY}N?*zvoA zV;K~gDp+Z@yTarVhB>Wfol(DI)IYI&JtKd?VVhc-+{nE-*R7~1QK)5h`1R$3F&D5J zJQ?ztNdevO^MknC0E8?~+NGpvNeA~Tn-me-MEc+-t_;R!P#WrQ+p&8bV9(}CiaKCmM1E@RX9N$c#`0Ne|!j>SPe1$hmjSg4O@@}4Az_qnEO<%^Ft3Crs$Gq~*IbkGx|Lc&QSftpyy$n;y(c$X z!!*5#X*O_+bi=uM!xWO+ZGpm}95K#JoMJ5C2{Dpy`W6JHM^;ytSqnWq4RPNoL)5DK zSE$X23vj{0K70F)h&$MA6$9_Z13y&YoztzMtciCu>^9ns57ap>A#M%YQKA zh1zr&NGS9s@w^Ey1DqF=gnE<4;e*=f_-r7-55*6U;aHH z(<}?p{d@Rmwbk0UrpnyLwT-$uC7KkDunimnKF2q495kFGMZuruz04G%ZpIm!D5UW5 z&V;(1y8k+0s!Q11pR6#$WrqeRR&YA2U>H+V;|a$$VTfUE$pVD4s4tIqrcDCzA0EULMA- zS7)zF4~DTpvjJ6_ZBWAuO?}9QnAmd;qYxyF4R>h+)_+|69SW9v4DvG*gy?y6F(O3K z($5M2k6j}!Az1*nX0_A`fm!UieUC#(WDAkQo0`anJJfU;_%W1dxx3M;#%9}_(q`L{ zwbVX#JnzTzw>R)$f-ssiSB%X<)%!~FdwSlJ>eEzQ6juQU1+`#qL7$Zf>lq-QIki?V7W70ro~~xtK7$u?ok_mrZ9@m65MwgJ-&D6&H!}(Y2_jy8 z{2_LID4X;BCg5QWQwZ`|@Vh&L<$;bu05=GoOc>SJFW*5|OzoFp?Qo=#aNSE)D_nVW zFj^~ecrlF>b8TjA`)EOX6S}J%H$KoqBvwF8{;)SxvS&hrdm;W#7c z7a@y%Q(fSQkQ3ON&XU6Y`w&xZXAEJA<|(OhUQz#Mn8Ewzc5JqDE;NRk^FVB6sWBZR#|I3AZBm= zh%45R#Guf&({ZjUMs5yI?c|i-0+VC@%+iW0ztkqAqTxIKzG3=-Vej{??BQbout!?M z85x!>5hM93qfq4jzFj5dC2)$CP8Hx;p+ErnU#pgF!)M_2Hk5t-Qo|834e(n9)7<17 z5YS20v&SlWb-@+Fr3C-_TXhEr1eZ^<8ojMzQ~4LiprA{`U2sIb?H1T%EY3BDGh6PKr8vp4GpO zp-%K~44`FpTL>{F;+VpJ{mN90!LR32RwFAx+wDbHeujGTy8GEKURco0H?g~@uSgqT z@UuOpZ@)V8ZQ(TsOkU`pN%w-iuGzBmwwxReo8q`ak+rkuu8Fs9eaF=Ie7&u|q2FX& za)0_en5yrXae=SBSlWA9yrg}-V6&N^aH_gP$x`Yjn^P@*9h@QW1xpfY5Zv)96~_4 z#9&yC1Rq+&(U^IgrdimC3XwfB0@#$m(~E*L<~e=jV4E@VXtivd1kJ>w^FCBgg#arv6JR}i{~18 z<2Wnr^^a^U-V1KdX1_7_`+O7%oQP~QMV&{l8|KL8ES zrVFYFE_yAV?v4#737Z&m!fYn_4rj<5Sf|I7 zYmXU~Gt36k$mLy1wM#VwZTa-rEx#=qzJld^zW_CjpdA-|Qnf zHo@($8T2!MdX52pj~PoZ-va8XI1Ezo2G4W{qit*!OKI6>O~C5!!&ngs$WEyXt3b1? zh8qe%zvAhB(BS3?+D;fP!j#FM8?v0qEBz+=0bZP4=r#!kJiC(92xaU)36%BqqJQ^k zOoxgqgi_^})NrxUPDf08HaJ6If={8`7s{~-sIqNy4Ibaz;+KI2zYS1Qi7esm38$OG zQeG$UH*DXPvLr3S1?1h~Kik4j)%g+~6B%iJK%>S$DM~k5^QdbV)n53+L~6tFMM zD?xeaH%A^e(9Q#Jr>8j=vU6cUQ@te}g07Jrfrj)j)2lOeP!0SO2Qc|(U< z@_-r5+O`P+*csaa33U*(sr%+1 zQ_7i~rT_a=K|fHh6$VKF28&-<8=C7_Sq|#-uAt@DD8CEqPG=gVPUDbHGa?m5Pa1~9 z08Ym@G{%`gnul)6c{!=1?BIO6aW0pKSzN0t*=Mq%A~}ppArOGRc>k-+j~eQ0)xu2K zFf-J?0-%*~SIjDJtgzJtJD4DmZo4jO#29dRrLctC&d$!V(9ddF%u^Q6PKA;X9=&+; z_;7t7vchbjKsO>llb7RxDi*&4wHXy&`&K|bp#%*2?v9C@-z zIkDbGp&L<0Z7({^;DD)8)awR182az<1!($fBcO)wS+oj>;RJMNkJZL6p zWq!O!mo|akj8Jbt4!+ zJ_rYVQ-h_pU<;Ak{D9Q*gVNX=_@Z`a&qIH;DopN-jrUg#Wedt{RiK;@a!Fl1@@0+$ zARPV=XKx*rWxKo!O9*0sf`BwgNefD+K`1TKDJd<|4WiQBje>MYcS%ZjN=SEie)IUQ zwf4JyYwi8*WB==c_jBLZHP>7-=bUp+^p#C-3d*9YD@MB9+U*SQv_YzaMxDd^6x33C zz*)1OE2T2EKgf#u$~#OybutB0F<1d%_M?+aeGsa*_#0KQaLmB=GG5SC01cDaD^)U9 z8~FES?|y$^8~d6k!5`Uc-n$QGKMN`GiZ zxeS1{Lh|~1$qovvTe{b)=K`zbg}Q=g6+)Q{KQ;bp_dw>>Os#B3L67!f!NM?8H)sA_ z3!^Xp`aZ^WWV#kJXB6+BZI)aiX4LAP?L!bn(Qxh$N?Md~zD8bq^aZ=e8v2r_S}MhK9;^f614d|G0~U)Ybi6pDX3py-;}a`i zU)ZW3kqzfTQkWlRYfAWEzoE?Kyn=16Kt$<)JBYoypy+#Ezs>&&W$ zGRuwWwb+zPMJTqi`GOKWjZxqHE!2=uxK|q?u#)e99{ki8T=K-93D*%b@?zeztPUIY zt*;h8UOvbl%B7(-ix_)i^Sypx_{mZB7{tg1zgURa-`9$G3op$tk9ik&26R_ z0DDG{d29SjhgixGL}y|M~L-eu^sp`rE$6lGP!RN5-I zrHxY5F=MQF9e%s60@Wt4*oK|kqHqASz~feG4dxGn!33&0fmxT)^x#wWmyA!1qV7># zx=5{`XPNpLhNIuBfD&89gLNTZ`$DTL75?(CG!ZzE=54J+i(0>d)^kE?JPLLRzaUq^ zw1bPm2%3qhu>xxjoaxR!{b>$GQD;-Mt`$CMh?UN_f!)^_RBvXz`;$pOZPD;!~EKoT}H&8 zh2=#otcF)lJIzG9b6tDBBSl_(fGi@Ji0|V1KU-1WaL<2ISBqmy)@mjYkwzxtsIK^= zKT}?BhIBEfSfAJRsvD{2GITowAIK~SQ*Cg*<&N} ztGp&7Q1wtro=kaxv(LN^5ou;>tJrDM6OV`XJG$2XevP0MrQ)nm^j3CXd9uph^wF~S zw)O6!g7M};$?EM`vy;A*)u2JlVp#wpZtJ&Xeya)iV$eiNjj+kz|FB8k$6xaB?(0mE8AmNZZq{{jc+e;_odc>C_0kb%3A=F4L`s%OQFd^ z=T(aSIUw&!hbBaAH80w{B@?>G*L43+&}}ArKVNB*@jL;gVuJBJmv-EMAL)Q#mB3R4 zh5dSX((bS-|Ae~A2;2v8g07_Ek<3jM)~f@12Oup$!N*4om%_Gb>X}q1b7f7t%06@r z(f=%?RH2JD2f@+6A)m%%GxXlh+-}9+2~ph6?xPD197wZL+78Pa{5J6X#(v}0>+d4m{}7MKo0!Z-v*7h$Y44-8mLrbgGn|0 zfzl5l`K9;+rtiVsMIS0)H1I&J$__dqXhj5}>WJ`UY-a_Z)l=&r#(KSet$ShurJe9# zs*cNkpAqr1heoLmBld{W0 zsP@6!Ff=wfcTYjcs!9P7jBd$&8xH9YZjb5B4<}>SV&#uNK>Bm4mm;z;{2>2Y#vARA zXWQi_skEtT9mk-opEk7CP6l<0CC5^QzaA0W9#AR)#>(|B7A2{M1I55Pffiw`7#V*# z(0}ES`Ys5^M#3s+wG=dlCFEyc<8*wN@IaH7t+AZv54=~dw(HypDa6zb2T_!N=JkI9 zv;AQ?%2k3wG+Uf^I%Gk%z3pDdK+_AS!?(PEQE`n(%grMbIVGiE`w@7C`FvX1*TI}e z_AiNXK76a85z@V*atCpwe<6-E>r-QlvTs5#64AQ|C8iO((nhnKrza7) z2sra(p<RNElEVqr790_m)x_c@EcF+m zrxpi&%DOKiugdnyiY3zX@91@T(hFxLY5(@I%bm0IF6j)wiJPO%EAbt@!c0$3Z^Pr5 zt5%L|#Iu=?^k!%6#h-tvXA`@)-Jrpb;z9>`ypZz5vC#U*+ z*yq}fGu+Nvb z3=~c4VFwgfJAF}T{vgCiX=u_nBizj5>68RC1QSmj)Son)bApXmdZTNSv0qP!9?ylC z^Pslo41d8i+EWf+D*7{}T#Q7z>{qmAPBL?qX!pCi)U``XOC#0`AZzAD!rl#2f62d4 zJ7AugfA!*X{($hFjy&oUBvoM36Gl;FB>U`%bmDu6pE+C=hocl6d!NthMkI9l**t0A?t#b-nk zE95Fov>W1wXu0L>-e{vjM&-S|HNH47Z0=LQuK;&Pe$2b2dAZ~sf|XZ4R`|1Iw`YCI z944GBU1%G&zQ4;}tHRdXjVvb4Q+Cz@=p(;>6fs{;uM2`@W5YB3CNskzM!uha0B`=E z89#2|AOP=|ex6eTX*Di#Ve9!o+w~j#_>3iy@KaaMGIn-cE=fPfXioKl0lURToBeo9 zEQhd4hl@eUm|9mG;s2In({ ztNvZfeQZDvMl-H>+xgY7eaZppw5Z31*qQQK3$`~+$S_bPAg$MWoy#3OrJ5Uj|EIfB z&hOwyqCt7$0;(cCn;(Ptl;58{nkLirZ;jCvP*6jpVR~I4&yU9&U;R4cmBQf zb4L%Q-==>)GM=Q44H;~{U8o{`8vX((J2%B(RxyG!1c}`Y0X6uk%Dp5!usPfyZx^^NqwmjzDsX<0|v?Dw&7CJ29wWGk&oUTD>mjOj2s-} z9ph3U<>gAbaXrRLJEUJ|l+Ay`-@#p32hi_rQqlm(&%eO+hBw?b z^PV=%$KT(kC@p8snL%KYq2R z!#E0oJ+{@IKa8*~D&Xz=_KXtw(?7WYyb+n!OTCftWKB|$Y$OY%@qyPr(MJXRHgiU_ zF0{qvsT3oei#Im4L>}(|GPMoq1 zB`u`^ts@jh3_&50FT}*&qJjgn+1k9xwE0%gHf1NXIKIVZOx?68V@lt|cOaX_w^@v{@bhT@wPcD?;{L6mh#wd3K!g9`LLZLcmw_@?0@bbtC#fjsP& za{w-(!bu9Y%jmK1D&fWJ-n8%O4fo|{Rm$E`0MW*ywN*^6UGEKWS?ZxGjTM03jrqY1 zS2OI%>uxzFDss6A)uOlV&}@rcV1E+|jqV>YzIWTf1$;3%cGj)$R64_ZGo6?GiPlu! z1u3W5q)_sXe-+YD^Mjn8tptS)wN+vp>3g}ZE)yaZ**?I!WrkX4cL}m)N0pH{Yb*%a~h1Do0m6p=3QA@L>~$K2ni6!zMI?K?fh=NnSIah zi)SHn<#(IHw94*DE~0(WLEF7zqz#ukXni?(XkLp1phHsY=BLxNK>ieoyw% zzx17l%jBAK6Z6{MEEXSZK3bS%HQ9c6Q<)1D?zjgLS@)VB6M68ij`R!t5lkX{QkIO4Zr;4Ngo*M<}AF&e7g_cc+-YAkiWI!;sT%aDj zp{S?c!#rWOS5Z8Cl}_LI8ob&sT_XWG&hRB+=XIZB;Bz+@uMR&fs+Z|d6U14kEp2f} zLVvd^c2bk>?6pSfEeLa-PYb64Z9>PM=+ih&9$Eh42@!B@cPb*7LTjH?{d_ay(e$To z4WV^UqvVE1B)`Hi6a44%kpkZ|ZQ$nmUv57QoGm?ceZ(YCdE#b>R7j4!MTg+>USIvJ zzig8{oKtPq%F3&gRI|`eBexq}k@1{m#aOX}-{7FJZGwR{yZ+(bpBhvOC90Aq;l_&Z zPG$^SnI~zx6F;h1p8c|Lx@p?Kb#ulh8oK9`WB2$0PeA{}_^4VX0K229A6NR| zwSJo^62o%!{cX9FC2SYTcxOGW1<7?{{Ot0^9N)@ibeaMb-8X5kVj!XMh=99qQK;he zauEN0N8(ToJsI3J?tFB()V1fk<5@WuO5#^jt-dqi6|zfQnmNQ(I(3yCdE`8B&fMdA zv?uXB5?Bl5NRiM>U|TMvf!V#r^0KtB2VQ2lVeKeAZ}fS2pvUp^3Hh)1rxGwQ_B&Lr z+Ml#BZyY~b6kWNSk#Ew%Hb5YYKJH-JurXVy1MIIm+tPSWAC65IDa9)o?^V8Mbb&1- z4xOBCM@!$AVS4P8^2JAWD36TW5taIV1;VXLU0VH@DQm z1rdT^78(AslfQymb%es?rr+e~W43+<7S*_fjj6ES$u+nhvj&S;H=qe>gIfI|8CS%;Gw{nV=<0{iXk<22R zJmq^nAm%v5?aaD3!7Ucfhuc`x&NIKkVrBc!4V@4OVSe1(2vpH@c!ay;748}77*(cloeOxz|(T=v40ww0lA z9$-lxF#e(2&&5qEYl8UC*~G2l2la$BsHRLc3g6RWb;o^r3}?9p_iq!pa|G zw^;}wTKNi#CG?NQ8kRJA7?FYvG$9QnekiOe=i@PNR}Qr zaC3%Y^X^MtFaDTE@z?2ES*2m$^KM$J3I#RufJ|R;MV4|*?TWDF0n)33KJu38CO5rM z|ISBzcrldzM(rl6 z^)?UsEXDcqb-M+3gNL;fPI3(e9}+W=3LXM^7mg>*Q{G;QzZQh{ecMl-jJ)H$pMQL8 z@JYpsTg}+l-3e9=lAr_O-Tjm251OJJOoyzLSt~~0@@eka!!=aPj!S!k(wz#qe_5A#Rd!JoXmW@3zBlcxVjoB-Y{Esh_#7ggv*qa!nH$~Z;qi~beMI@+7 zFtDVOAE@>RLq~!bHc)w}-pmTS-|kr#$y0Si`H3VL|IE2lF;t?{U?mkhBQJKKhZZj@ z)tKedkvkJBD`2U;H=scx_C6DywbI9(tL?F*zVc@hSYcN~+`54-eyXNL8=E$c^>|`z zRo9GLmJbG?DUIAYX}JotK{HI`>q!ch+#;7$!R(BQY&nqR{670>-RyBbSJ) zT_0Z58GzY4)1(89;ja%$RV$>#TQL^U`}-))f;C zcQErv*|UA9sc1I}>rx!ssTeV5679GyMota~1nJGpix(701^fJO6`~LMzBS*1#iHmj&Niof(Hd zGqjE#W4698bUlXf{ysZy8ol4YnF8%jZtt-FL*oX++_2QTo?dCl<(A}K|y^Da-Y{$)Ne4yq&i(phH zHcgv2;TcnkrKjz`{D2rG*aK7Ooeo9~>^905KcoV;)MzlU59&^P#R?ii&Nt4#AwYVH!(<)az^sL$(arqc&msg;+%8imvLj3 zz-h1Nv0h1n+0u5)-y%a&#-LoiH5V#LbWw|q-k znjzrfvfRE|kkXE9?;q<4pLX z7RV{kQQ>Z0d#(^aTxOvIFoNj$+sNQ0z)Rl1uy{nW1ikcI0K~lk9`- zl*A_dZV(wKGa32*q*E){Q-`fz_<{+NuZVB2wNIGU%5F;wo)_uu7ivMGF^xerWbVNd ztQd3!gX$CHJg?HXW8AKRd*Mv3hVFp|6f8bB0HZYt)L2`g*G6%{4tOm`!?hQmpvFX@ zS>x2Ul?&yi=sg%v@Sva7e3DPE!+0Rw9YB*A7=tO({7j!?(X8lPW>r z1Y+WCcUx@5ZbECwQJ#Dj9#DMJQWtZT3u&H&G7DbtS}i5r4D#En27&?IM|-!}(Q4Jrspag0D9>NKfAaqM%~HnND3Cj68{KiEg?+O7 z{Rv;D(#AAAt4}e7rPU(~=0A{x*oGLqcZM3U{8T6bBNqSO?Te#XtifTZc%_}hpN+#X z6vm{=aex$y zTaKGpgMyCv98|Lho?W#stM@?dpsHIQ`_fBQ+jF#Sg#FML&OT7urHGLgF0R;8nmrn0 zpxF;s)Tl?i*7)<|CrlzLA`yD(Hb4#-G|(Q?i^-0%Iu2$T_KuFw`P1pailxox0)sf8 zi-^#B+{e0dak9!{ogk5qOL?C%03+Ipz!_hM#q(aG$!I|aI;ytF4ABcvc@UIJ6z~Ub zO2%sAbUPXA9aiH$9f^vf7aiq(5x(|f5O|w#Vb>O^ca#&e(bHow>32lA12U1x0hRmd z`QM)g=qS&3v=F?`)NeinCoy<_($@<@GUf(g$p$-MlCe)Cc4%B=4LRk)doPMezA#Uz z5aW>k!OU~pv5dK6&>hdWm=&oy7&AWFsNCEF8cRzTU-0UjWi-?M7MPmw8dC!x@Ij)t zm}uI4^AxY0oT-{q+Ql55w#BAGN`5w=bI;Yzt3}Ie4O*ntXm{x~2X_1l7Q}9VkswC$ z`;3Zm$O@B+kA>Ux5pDNE&~}fsRPj1v9RC}^dOW{l3ENFykR*}BA;2{Ufn>H6wiSGx zbPymQV|SpXQTNMJqfKsp@AW0ZxKYh(JIHVf$dZbb?ra6sTJ^}*3HPuFxFtGLd@1l| z$t=U@%6Iw4wpTrPg$;>)5LSB)qTv^hxxWj1?GN=vrQ;vOQ&JR!B`Z z=!lZ-ydQjL4+hL4s3Jzoq8$_q;M>aSOQ|lk-_t`)DRZ;cgM#2iq^BU%iC$qf=w^6J zSVq3rEfMdah%F=Gv%3v2-VH8v9JXav=a;FKEyJSF55gDa#X?%{DmLEO+D;+2`pr}! z)@s!9ss3?e)Gl;&q@TYzP+KyBt$tPuS#C*%x|9kGf|1&tw!&Sr-9&lH?nM`Q8QH)LJdG=~k})Zy z3r)mqKFp_Yx30yz@#%&W5b<67(Kd>>HHS|qs3D20578{AFIBtuv${62X|qOC5yz&t zZLif>bF_7vP+egxeOr+)9iAn5@-W-C5>W0jYE%=+(re&iy~_5KS

q$f~{d3jy>y zVd_0r7pd2?;Pol$~Q}ipib{Dc)!6dmw&=3NOZr^B|~ zF#}~^=0ZtbDrM3+F{!TTN1{wwL7u8?pVUPf%-0!8W2}BYv!7;%N|D})9#2)aeI)wR z0;Jdunc#{^=Rw&xW^H*y?h|@yz5azN+1cZfznJ(zU#}5Y`1El(=Ai&=cv~EErn20&BPE- zneF{xRa@IY7Sc3{lN|GaVD@EJef6S9S?AmrSUG8`qn=NF{S+O;VTrQZSj}p|rVeAj zG8+Y&aXQqND00Mx+%aQa;URMw9DX7BbKu3v&@f424Qpj(I|JVQZ?wP^94#`R_JIE4 z&;;rC)c>Zs_4EADLVSh4gqA~llwXKLiY-pa+SBQ=c4 zwAso6U12e(9rpo9#(NQw%#U=Tv}1c+Mss(c`XAtcaW$c?3BS&Sk(Sgl6g*cIAYC$( z|7>9&aHV#Hk5*X*Np33X-Bl~LXta+%3u$FFSHDr}tP-p$pjf^HPOU~MHXv77Y?v+V zdh2)ZR+Qdun`6MSqMFdRdKPe#drM*dh?DY0g$$Nj9SaAR*WIUH1DvRK+iEZWf8RDS ze7EaxGil}=ce|}24|ReqW-lpt8yV~Nbl{s^;roAked0*gXr)BkYV4OUvl0AatbLKe1~TfQSh`blT&L;aD- zPXn<#2~YWDj%;&Wd7EPeZxTN<7jJ@euj8LdcaEZmpJ884H`@@RuN@VzQ`&T1nZ?Lj z7jM8clWo^0+ukbPlR>(?d4?`{ah%Ln;mga|49LG-S$D%ozbzJz zf%_n+_=-w^ske>%c!r(e(QlLvV@|A}q12)PNPKY_J$b8w40C*phGKo!p5xKjmpby=+J4tOT6z){hlj+sqXiiVopOplR-e#w|PS+qe zGZXIH+DCEYE1Kw+!7>FdyA$%2@C?riy_L*f7uG`@1!kbM~6@gw=1 ztjW8ccB^@WQ-;UfuuAz@Qa<_K!rGnD?F+2Uslk*y!7>d=QBWA$4kz$>;cB!c$MtwC zD08=G`PqZx%@D4h%Pa4#+P+AXOK8?w?T4@}w+L{jk`lKze7dTNKV6ACm0@_)Zp$R< zqf6KGI~p@H?uH`sG;kfoV%c(j?jbV-XqvcN?5rrm%g_H+aR@$9$wMPh?N%x!jFLl; zg)71#idGG0<<8CY$oe|_h(tE|`7H@`PTbCx$BLB81$gSIIV^8Wuh4##PHLq)Z(fn|lwTYFI#M4vR3 z79O00La~Y}Vgh0OU8n68;nyr0B(7`cD1noQ4=yy)s zwA+gsMVZwgOA|>Smp040s5QAL7_oMWWLt&}vXNH$3ardThPa%E9G8Uz_H6U@QTjST zoR%6U{8jS)8||r9qkRUx6I>gi$B9}!AD)zP&S->nDk?;81B&rxWadpqs(*Kgpc@%s z`#)kUjHo(9L1k|3&RlyO%d~OTyZf$;_D{a0w1_!}b}`p5{Qal@)1J1Us8viX0W6I5 z>Tx=a%>T`;AmAkq(Fp)#Pkhh@QCEoW=DJDI^t(N@KcHGMuC~$z&;8$C)ccuGDH#^x zW()f-Hyc~Nz@`!kC~oWn3I}J_^j3wl>*h=!aN$t|zeW7%MRY z8RLQI6JuE~2wONp|Hog?mick3^A=T6{zooaac&0{nTl_GV4xO%H_)L+Lpf**VThg? zGeqr8LC@)SBm^6P=X%_|m5DLH`9KXRULy4mCuLx`u#fefKgRbdBSwMP4SLwo+>i#% z`oAE~i?*Rg)o0E)26xv8bO{lSjMC24-y$`?&Rebbg#BlJ4B%M*f4tLn7DAHg?#1_q zm^Zn!%5~+2q@H|nyzxb%hHZOjf7reD`}vYh)y@_}M2n8~)ehb)j6y{9i5I-{_!H%R z9;?%@1Bi#R|NTSZp-}K%6UbMMHpmLJt#L)i-OP(hLb`l2fu&StO(DWTXlTt95!qc; zNX!blSpQUkwa$53%JR~X1G(hrJXhMS8F&<*asL?vAQFj zE=sOn@UKoqT`XGdn|CsBh=P98W1@+h1g{M0mh_Wl$;FYX1Qx7K_=@geH;8t-euQh` z>ke(W#mW!5z3fpOX~C_>$!BE*|KtKp-aAFJA)avmF?wCc&g1ukgsd`EIYucppoGM{ z{vT%vOq4UU>n?mP6tS;uCA6M-QM^lgkyxuIuuvTDSsuIib#ih4k2F&HuJun(l4{%O zL-kr(qrOtoNgsRxN2GB_)0So5v~cdr<($MG8}0Ft;5`=CbL>;;4bjc9>UGWwh8 zZmwt-9;%{!49>4Gm=fQ5i_8((l=0_KG)qtpFQJ&d=*}ud;6Y zdzV7544a@;uMGQ60#I_oR@AR+a-I{;Dg?+Suihdv^!l$CEs)D{J5KTQT)w1s)GDnA z7pFQ8eO5l28D<$Mvd<$vTmOyw?H(i6WhSVcZ}w6DanpG|!tVxc ztONv)CkFenWm0f!$vN=r&typ%5JIzZC<&#P=}nT1v@ zQ6;5&CFWBus>Z1j(H{V!BuEww6oK~BZRi(Ef`-hMP1Y8m@7^XL@Xk`q`z&_fFp{^4 z#jtmW9(#)guJuc!RlD{hj{ysed$1jDzHp+J?z7&zju9m>4fw**)*G1lhzwMDznl!v zm5pLI#)I*amt{jKy8?RewZC&=i`_?5=RVBpdELi3mmxr%lrnO!xL%$--# zA(~B32B^Y&D}&JS7G-q^S{GOR7l{EiKh}%CHY=627GT4q5<~)3*ZTkAdj@`g8+dCQXWDIL0dN zC|?z@wqH_iH1Py)2#_x9zTafJ0Yx`ZtC+;$e-P9DOkb)Ax0Q|tjsPHuU*)qD?mg1K z3DUs166M*7WV|-nS&|j)atXhK-8jHLii5{4KJ)Ag8@h@sQ-Z}}P)WHRHZ~G$`%Pba znD)!G4@i9JspYooW4NG~)CjOyPr586=!^Bm8HBn;Af6C5ksz|$;=v!Md;`{)6S2m( z;;ZgbThiyMz@FBOi#dQAz~Is`5Ude(h$l9-uG>xcI+7J@dm3M!UIp}Cpo9YUPXt=y z5c07M@N3mbf!$JcT`-GwkmRG;pYcC_~e4X4|)EqCtRv50*Uxdb3$ z#?9jz=hGm-pY`VS8p?qX$KA)Z5JowoOb0Lg2pt+AiK?Bmm2qG}P7!C$#lH%V0v1FY zoW2TP=)XJ$<+6UTF1M{SvK8EgAr#>Dk+2_L>ycr-)j?Ai=OZ2_(b2{BM+p0`e9ib3 zZS4d0J29P9NZ8lBz}q28kWX%FN%f|n2Q%}9gIWDc>T*61J&&&04us6Jsp=fRhFk43 z#&Y8-+CmDS!xHqbSNNCHveWwGwCXPJO!Ehig1GNO*ywX*QcE9hkPsq=Fa(ptiQuaF zUQJlI|9zl%-3N;cy%rGJB<{xDH1{wT;1QFr09yWy++mlDbp>1cug@ZPr^22-G*NpO zmmA-xdorc>A6K87AZgTHxLzu79!yj|Ze}O*oGJHN4fwEL>UO2pVD>3B&JApTVSp+| z!2Bm{=1yL>vb=OI(iewM+(4X&=r`0zF5P`8s?nN^v9J`};BdO~51l*Dwx=DvM}@F^ zLjE^$l|D@&>*8Ag3{2bYSqw19VYS;ZHl##^ZV7dv_sph|9vvK-58C^;bBZMLI0rE@ z608Jf24*i8XfJ$@E^}IE6L8p|B<8ce9XcfO)SdC_LtqR1vfXr6DbP>Gp{d&(Epy_6 z7*ONczoi@naa^cJv+getVq>DndHybv?5n%Yi8`ylbJBUr=p$Ue69WmT&Y5h|GcU&J zlM2?*)4b8@9!DE~pij>p=B*=2MmMwEBczKOiKMH(NSwIKp?@!$ebq6} zhh;4?RPPpvmekNs(sLBs?A386gA{d(+#N(Q9ET>jSsiCh7^m{Di*sCfpUYlOuNAun zQS)KT4wq0k+cD3zhun}SW%Tzs6!ckqDX{3%{4h<80>y(I35*4y3Md?|x^S`6Oe+RrIR@$JGhd~Eq!-_SM(Etqk>dWGfE!w8int|j!!7AQ$yS6ABdeo8USvrWJtAY$p6Lr}Hr)DD+st`Vo34n31atoIDk$VPqmllh-uW8lw(& z^S>3}0Ys$Nmlsf#*+o>SeKW#L^JVBp(4<*j#8jaL?t7z(5 z?-FVJ5L6p0=bt?{g-^b-FsN|w(epO0f7@e?UJ&_;W-^lcz)UeySx)Y&*V4|UYG0trDto)y2j?LjyZa(#yq~}nlzqNXcwb68@QEbVS{0-^+Dc} zuQP5h%XnOT^O1yysEnhQK4k=wSG}KcYMoU^(>MG*Dxv_(l=lpJabqgUhdJFLuh!^2OK1=X#A6i)+GgL8XYU&-& zj-MgbY`Z5n)z887X(?EgQJg;OSQ(ECLX1QN>>ZtCW`ya=R|Y((rwC~|s%cKdBeK%N zC5-kAKb2FiqIh;pQ87>d>Ue}-Jb8}sh{t01&@FoVIU+)dvBL6M2oK6R`?_}>rTN95 z2MB?h_Vi)4GmZ1|fu?MsqT+Muqd|IZ6lPBKJ0si}6jU=uWVmwUipm8|dq2IT^ zQ|mN{I&%7z9~bSFcs-rnivLfg0lk;x2Ix+hmh`Z{!KjP~%_TaG>Klvx8s_Jt4v_zi zL*UTrhqzbinPow_bgG~2=Fv`!Z1nb}8TXG|@@#t^waXcwj62Hp-2+!ZwDs*%pb_JI ziGNGu%E$E=s1M$9)7wnGxzd6aMi3itgpLv!B)bIODdRFNS;eN7tTx!z>Bh7xIV#Nc z^+yW6@^{TQeOFm*P#}dPj_5kpQ0R`&f0UNZxxC(D-IB)di(E-q&VsQ)=7DvxCVY9q z{|ldp62gMN$?Cz~cKkgZ>qN7c7O%WU&m?a6T@7~o7vpiK#Us8*86Scv#F{MGe5%4Hvutzd+$m-8J)f<) zA{AC#Q=nyBc8GQN?HSF%%?rvAhFShZZau=6c+Zh3z-q8C$XKMju9)w2Aatm(j|3t1 zW+S?Abv#6NURK&fIxFqCcT0ak$uq)&u2k47V6KEJm2H4HSe!Ugggy6Xyl^=aW`2lq z>yI|-PhxE`c<&kB&y+o<`|&g9ej|BgbVu}}`Z-C<(PgL0!RguMrpuv0p57aM!R}MO z#Hn4^!;6hW`@>o@B;!o8-ttqT+Qmk@?X$}J8qrD@TJfZB>IIvQ;svo;;<3lBBH z4fuJRyayk7)4tnL`^QHNR_MSyf=r7Tcwv6f?g9zIqZycqwt534lv4R&xO=)5E>0r#Pi5E7`HAjYJWFy@*z7jJv^^j&L0e(@=?r-o8?fnS|@Uts>E4#al?F@D2=!etT;;;?yts+75Uy)A2ocU73Qm+Y~^LV@Kv&3 zQJlS)b(K9;dU-r3AVY%vwj9202RU$V@8e$ZfUOB~C%)L?wLqI29Uzb~f%CYs=1$rE z7ge49WXym_<{In9#i}06)yDmnIJ{NHN~v|7?a3UQS0vc2VD*I~k7MwWXiw~AcZiYC z8K#MQH^#EeD_6$@3&&5w_-B1^O1b&B0#a$mvsWV%gK}7!KQvt3R&6A|S zt8tWL>Q4Knw$8CUmA{p2ZTZi%OsqyUwr8k^F^L+E);R)V1Nj*)Ov`aUuSa1%cdKiK zb3FFl4AkOQ z{Uv(Yl4)^DVcSgD9*MQoFh4x7ucL+sr3dY7?qH`+L@v$Txs2foa3V%$Ow^ z6GIo?PuYa9)7K5>yy}5Hruzl%yMVX0>w_Th#9n!cGx9r|e|BYjBY`hXIGNF>BSNhMrmpp8Q4Cr@lAE!-ow-D4_%R;-QW@}a{QEz zuz-P4{{frzanc&5)ObfNK9_F7?HLpz5xxrhMk1Ysc*C0E`w`{zG}%sajKwF;7L&}h zc<@u397(}Dj-?jIj>zXUWd$3YYx}#I0m`J{ByJK8u?p0yF#Es)Ls`x{d9Y7ssdAcWzwJX@8+zJag<4)Y0$~ z9;)HeXSA|3De73rpQ@Gir^}Me=zJ!MdTlQij49T7)mt`j?}nZg%OrfA;X?k)#%8b1xT2xXI>QAH<&`Sm`l zO>^=zoZ$my*KXfKLZWa+?Kzzfl63g8hHtq`)gum;TKzhwt0l80KGv|e+HHMof?#L0 zOWGLO8aB%M#fkwll6eo~b!61f*zg*MCo*6}!)s$ORkI}Cnmub0=XRfCo<9F0ZQ7Fr zx^Q>wdv{=o?z|x@@yOi#F3WoxywKUNA(0G9WOaC;`nKU3eKfm}n^3MBr$_nw>Ru^l z$7Cr}qHplS@?n~`E-E?UTe_ zajyX9RzbB-nWBnu zg#^}b=O4r^sVXak-j|7m-!%>MUlVgCQjQqHYO`72c-fK1E0}{ZKuw-9Or4Qfj#~Ak z@BwVnOE_N>tIwRr)u&Rsx+;u~^!$9UxJ$w3N)_>yQX;X0kyzNXx!2$ztm5ZCtRlNI zRiPQScnRFu=JR$-UhJ)>qjw@{ah7;tJ8UA={_!~YUHXRq&F?1danJmuP4t5}fRhw% z+<@!b=bNsAlXkQIBX2xOKARklcDr!cOxiDw0B@Lg<#Hn7Y{;c0fP|=;9k!&9UuS)4l1ZJ3G8P`H)%Mo;{u~>xh(M)+PBqMUgmZ9Otrf1oWUna1hDFl zR(jfGxq3?rvT1~qINW)JfFKrfx-T}!0JoK;bO#t6*1|~%Y3gaoKoD^DJ z<^Hs1J)}%qbJWDrT)vo~s_riUTvnZ|oRZAku(aD3@taO*gfjuW8~5nqG;JrW5%JSX zcVGAtwCCIQ&{ggG9L`5-l4_og4bQ;;&w9EKl3?mrXI#%_$>e2fwtn2ualeY^b$&3; zQnM4D*9e45LLkoDo*lIa=E6vU#4}wrt6t&Y+_>7}DTFei=EY8=5u>7py&ii6BA$|y z=EsFuZ%?hOCTzx;U|MUyJ$?r(r{i|b+y(0n)|N&L+PK@B&YO-|F8x)Y>2U_g?b|ot>fHuMIK8(DpHX^P)?lhkXlatC2xGUBoO>v&&f$6heGQL6b>_oM+G7Sqe z0U8pxfLEMV4y5%n3&4gI>Q5TTF5dC&v{L2+Us{n&V8EbYmyVC4xF|AM%2AD5SW=|ih-4_$X zHr5`oeVXO8|4O!)q8#0$K9?1lb08VitaYRna&kC zkM4F#Jz31m%JnfVohYgJkU$!#ZlfG$J83W9v++0uahwJ{QI?II9T0Q>wrdWnSv2hD z!;io+G@QkPp1yYg%fFT1!PefW?aQQ~-kJFD%~Ram{_{=V^&&ZI`Lp=k6s(WXCy=K2 zp6_M*EDmw5Y7yQ!N^`R|po3Tv4Sd`!d6OmLDjANVho6O$DE@r9cE8PmX~`&! zhRgB17zMxjvDEoD(Xoa2TotnStrYpqgvPJG*iQ$4aK7Fuc+wj<6FYULU(C+W@3Nnz z*#u&QGc~6ht6V8l@7tN=wFG9oC@1-(#!3>JgG&4~Et+uiVKCgK;Qr%OtN9C<+rdyL{^lI>fzbI8pqLHZ5Fq zDQOJT%ac2MVo2QT6dLb;QfFOCk9X2a#VJjwCgi51IhLa5K2IxAHOUt)mKQK(B0EAm zyqhBTpc<&zyqL?&FCBO;xEJH(NG`z-_)|wHeXR&d5I_}E?Lxfc%4VNMUldw`j`0r;I zc4a=bKe2VVwoJkzdF$FPIt@;uGG+^7wyU6y>9-8xpr!C_U^}-auH9d-CG@A;`#R^I zbMELL57;2$SKy6_E2bdNit-;P5z@z(J2lECW5XAj;r65(cvyb0(9l@jIjE$P zI^mwfBjDaf`2+`LkOLRE+ZsBJ%3R&vQ)-DTU|Z($Dzc-JW=a5xYx}r zc>%xLWih)(P9JGcwdXCG#8>L}gyVlvw~t88)i?dqz4QU=mGkq{>KnJ&5XI&VnFc|% z_Oq~jMlxY<>p5>FkruDzMD0xYVAwL8dM}^wv}XE*9~-$TNGxJ&6lO;t%(c4YiDj!C43V( z@WuGVr)7!J)pv@=Qp?_G6>2n-E=jr+tvbZJ*9-Ua>$$*=#q!x_wJPdSlQnCWE636R zPW*gP7e#@MoJ8gpY`H8co#hn}`kW}PC|ahS={xXAVg1nFaYI0gsiUps;4Z~543RW- z&3XemVvrxj2G!DvORi{(Q?f#95p3KM-aV4DEv!-J0ZZ%q~+ia?hpzy(blh{*UH)kx1ed4Fz7uUxu*~n zNXv#cIzJA`!l9-^s3V*LWP1T~PZBR%he=6=OphW9(`~Dx%yMHe`0V7-q#3RX~q_6!aqzfkjf<(8TB!?Rbmo;ZkIRU@%8j zvmDI=(9F0M7{xK1TZP=y4QCM=_W?(F(!HpQrG`YTy}mo(={rcJdce!vrZayc5j<$l z_T2|4PSmyrHD-W92+@Lw?`dFVjm*JW53;2&^C-9~qf`7YbOhx&Jpb*=4PO4FSfXlFPL2W31!82M^93OJ|k!4O^NDz~{T0gebE&PjD5bO@A>9pv z!0*iSJjUnqe820r-uLf!EtZVTJ?B2>-23c(?Q36q!V#+Sm+^1nV_;xhR)EWBU|?Lx z#=yX|#JvdqvVT7`3j^a~v4^alha<|$*1-~kkyrZfSByMdHqP!IjJz_8JUp@vmKGjv z&Q9PZ_`9Ps($>n>68ZOgJX}27>|Ffp+}v8+f{eUU;1XUbK7I~iAufZzuQ#){boyr? zehw}$0E-?sHzO|$yi&Gx@!BW@KT*=mhkw=c7SCE5O0R0gq zxR#1CBabwA?O3r^wfVZM2?#Wyo0`0)6B}u&GwI*p)d5ZbaS@_3;t{QKZixXVd-IJ{r6vaHFWv8 zZA?9NZ3LBtJREd=RqVlV=xaQDTrB^-25ISKYXRl~_CywJEz;J^+Re-nTw?pr?nw!H zYTChUEKLQ3e5`G_+#P&Sa(`bgsNN0*TYCd1^JOwn{t*4LD50+uc)5Ud`6Z-GfJ(%iB!| z=3*h>#BZhNA!{ioWPnn36?Acy@ zS6Ocauco`Oh6VVbKEh32Q(8sQM@iiSrD`sxZ4Ne9Lta_fSr{qhF7GU_A?sjkj^I~R zG7vD3mG;q+cGLECwJ;OZ5tc*Qx!5SXJ1XhwTflkUot<2z(%kyo+-}^qGIp*)=KS_%C_YzvZAEDXML}zr9j~C7 zy*j^(y{)>I9+FSi*VR=3rDA8{u4$)e;A3sA=VYa&%EPBEqpIMgC8OiUXXRs|DW@l8 zDeUTvKqz^@E#(j}YXtg6_Ez9uq=U5&+}%vbMqZJRTThnX%0*Ab#aY?c)>g;dOW4ZZ z(q7)$Lf*hrk6Ybb&Qn=NiqG3dK_7h3QXc%HC?lhy>S8CYY=_Wvw^Y{AcJkrkhjV+m zc%!UkWt`NdEd_yv3hIGl3)65BfN6T6bS-VPrEJ`lT)9=PtvzHdcAqJSd!Ezig0>)`Ir z&#xtHZ;fG)3xNcmK`O)DwYY@Az}CL-CIycOGVCE24Sti!*AhZ@5OKC#0xWU=Mof9 zlNWY!vEf1@T?92;oCT2r(%eoiFbkxh8$tlZ>#gVPqvYveV6G+OE1<8Vreq++XT@!1 z<>rn;S;-1L^IpE$oN@J{ks7MX9Q& zTd1IPP`*ko_OkNkJo=uzz^vpfEK%0(^6paLA|wbXFf&(vFC^04O4v&VrR-Fputd(k=55y?aZUXNvB`Gl3H z=o}#dO{CMnlEIXfL5a-|B;`UBXRD%{DrX6?DIX_l zJQqamYv@w^#+|c7Yo;bo))E%J*83fXgkr%1|NG|>Q|v>?;N2F7%>RA|4kgaUn1|6| zlKj^XS11N{0qfH-JiJ(ppFe@bSXZdTSlT1WlaG#2Wj)_^YupzVSBFbZZ+4UzzRC05 zUvWFyG8=nk9W&RJ@V@N0?kN_Wh7}VZQHuNL`0q)!v;&={8jO$kSD8N~a~}GX;^E;{ zdT*K(YQBh0mk2OuX=(8<{Xn1)h^4^<#q3U!f-pdT{!HsZHXt60y%{?zFUu4Ym{gFH zwVrMZA8*_cbV}DM)R3jR|HHM$Z9YQmbnkPe@4lUWB{G~O&wFDcuAqP=MZht2t|v{m zBkBhIJMYbFxWnVs_8)?=3Fg|uNq0sqV!HYy&+pvUDUQvNexzt*lumX>4`J1l%7~3m zMl`qoWBLIh@?dQYGBf_lT5Wmq-J98nRafIwK3lD{C3@+Hmw4<3SyK3H<0tC5UTNgY;4x|_C9yF+laJ9E`IseXYAlJ`x;s{7n?UkF)N3tvp(l;txW@l` zrPzHxS-4T-wVs+fP-^@x&-U|k#km@S{uEy8?sQ>y9S4iO-5eGy(C}kNNx#M|zwTh%UeTlB^x(_NtED9yJVL_c-V9Nt z3iI|n=c$I3!>ziO-5QT&jjm+QtV`4qMg~@OfXQwaqZohME7@ELHF&~Y`Bvk4njg#0cSkp~G7v`IBsFl+%%d9p8oFpP9-?~2YKiwZ$;q}Ih zK;Pxg{NP(Y}5LaE{*1cRBF+pHVm6ji_U0oo+thF0rV>NB4 zQ_d9ArwZ6pPa|G%fkRCpWZB0%Nc-Pr3xzO|^wU8r)JS2wFS72{I_G_RAEO&gJc6Dd z>dGwv<G_04>l4ne0izM8KYI6wKsKs_GV|4 z!{}|ljPF_m)<^JI>?GU|;6D!tI4Rkrn5Nh)@pzA~bHmo##qOWgC2|-Wte?oZ<@BTp zzQlrBypO)>3?a*EKB=)8pqoFHS-R8WO{>>1C@KepfR^axJDpDU+enKe%Z9A@C3Tha#I1t<09$Km;j-~NZD6Yq!( z8i&HY;X-?5Cd0$BS$38hSDJyk$e_AN7Zaa5Ts$8xmneDv-c^Y&#rnL<{W%X5jri_1 zVK5@&h{^R`PWG0cEw$Ql0m`};+p0u~EY1=8d6duo6CNSZ-BMBu|?{OKRS)%?esO$^`( zG6h)){`myauZdQ`SkeL5E@%J!&Rr7V=@Ou?ZvE-&&T$8ew3UWSjbE}|rVv6%{MeG- zKNsR9J-@O)bt0djn-D0DiwGQVy+VQghuK1TAhw_K)a6slf%nBC#KiBPu3)r={eHqy zvKX1F$s@c4OxLE7os7`HVkfkpRoZ^#{^#9Ewqh{EThF#&1V>wG3nhSet&pc2C}y&JYYw`i-!UR z*nRbn$uZnj0-N`mj{rv&f(hcwWnAjpe?F4yvVl3zSrZt^pXpG1;eP;5?Ty4SoZt5b zYhIEG%rwh@R^CtoyJHZXkrDsrBMlS)nz;en7r=v#->|1g|ACTL&zSDCTQ6fjaxUQffHpNG>x!Nf5kNKYlO;|38~8GmjI ziVi(`S1iHijsG|AzmDVo+wcF`PNtm;Yyk+d?8_972hKAF;XGN(IP_9Cx%Cm@OZ#L_ zQ;zj7Zw|X~-!@1+xahVsr;t^uww_BV>IL6j=$W5xeo$zfI_G}B=4eQWlL_+yu*I8~ zp7#7{i#GJrWiR^3F>Cz}oqmF{PtR3u17wo)%DV5E5Bd%lk#+GyB)sW4Fmy+|>sKOw zp95G1lm_Af-14U~Q%Qgkqh(}eyQ*wIGq^8&)M)Vc5q6yo5jz;Q&?vtvHIK=zUztb7 zZlHCO-;Ny{rVohcN&Sz*!eT2d-YX`tY0JFC;Ts&m zUfgs5r?B@mwY=IN4ppgX3Za>d!)ILUfks^VZ=5m`*|ZhqVyM{BknH>JA~SISIj4!% zOfBw*42gi++KVl2U+a8$+~(l6qvbi5fOp61pqGzknZd&pwd^Ld{qnj54GOG2KFwKS z*DHq}A8vBq5^zA!k>*ra+YQB2iTlCj23PB@X#NQ06$Ccf0@TUQH4e7}P= zRxJ; zQ}j)KMLaE(o;nU~Bg<7bEku^(hbJlt82cl<6l5Lq48adEuxOQ;(E(J0Q!Z zwe?c{!}+eMLG5wTHlTfdT}|@TV~Sz&(74tQ>N;azR{tD}sm1w?!x&wOL3R8!c7p;U zlaOI1HQ3yl^BSe>Tjw&9`uM%@sG6fwaE$7s+o|o3KD1}O_F>1nt^(Y=O^ZX;Z}Wyj zNipwBs~A)D?=62;TK<$j^i`SRR!f6Y$Y*oT7w<4a0jt%U9-K8ZGsEM)pgf->$uE_`~LAQX7I%8 zId|{0IL6I;ff=sAuOT~RUmJ$W;*LaIybzhd~PWlS+;YH=N)Q^e7S8l{;*)O&hN2&g$RX} zC#=@^DBhZ4``~h?VMT0C2&*@*?H)hB4CS^DnX76!n;kAq}uQL1e zT#nLKuQYeT%fHYj)Os}8b^9jC7CI~EOhjG&V+<&QSjhjN%D>j7Aw8ynBz8;r3O0<- z`!&UNz7TaA6cN6o4e&y&=8Qs#!LE-ZJMUWded(gXlM4qro&J>E|6JGkc?{%fa2gn?k`PmXoo>{tH$w~PYmbkAei2R z@w#-1Keh#fS$42 zq-(N0{_)56-F)?Da`7OYgT8b3IEBJg$itmFLhL2J58=!)yh{)LVn+wj~Dklh`o3-?8WBe z@T{)z$VJBwWlawVyU30G#=ZP0uH%7>1Tx4{>5%+$D4IMx<#ZwShU1mnL1FlWgybj* zzrzi7f@^nT$xTpFAg0rT$d#=B%1UA3V+x~1M--3uhLPvw7w5*3kd+bB`|Mv{J0=j* z%N6vswzMEWENaglIE025P3j&b}n zF({n0Gl4l~ZK~1Khg-MIgdGqaTCi8Zu~%BWztVoO1%%Mvl~*S9z6v?8&_Oj_W87Z+ zt8AV3&vvQ9qoTARo`B5Z0T=+>6<)Bs+j_?6O}Y`0S3z6kM`zpmsXO@W0smzVB92rM zBqK?G9JqZn*BIH)+Xsr{{gb)cuS`#`|CmoU%>(GK`Kt+;FY>^1d4E~Yg9V{Rf~Ojppx z1l)QNy|RGpPqVnI2GJ2OrxtdTdBSeU^sVD@0-A;cNRkwLY3lwtJHP#KBJkcy(G) zBrYJJgQfs4T)c$vgG!$6X$t%7YA!O^r%b)U<$Qhvk8x*zK0OT_F zEk^Q*|M#7)gY_>%Uk3o2I0k4VG7clX@Ucj8PDSv@tAkwhN`Ub`@FBFJF`{^lB{V?; z)_5~SZa(ps^Nc6qG7QZ14(m(0tqY0|Av+GL86w)D!(jS4Zi~GoEjF}rWu}cBfW(aV zS|9H+bgs`QA)GNlcAuV|Y|bwYJcBd`^?v~di3Zh)ESEz`!X}?$3zF&JE-uM{Fs)j3 z^Zl=2I87b|4T&+GDB`VSwIp93FfG>=O4AyR{fCm0+U|0;mjZ0%cV0-+_JMGuQBV*o7&V7EN z#Ei|^);X#sp?=~|Z16aswKN#9E_rs;`pu`zsP8o|E6yAa2nv#Sw>5r+1#6P&3-5T> zuPqh$seU)Qk|j!M!&^HN)19GKBpz!h;<>^wQfjR9>E3IHF(fcgjXIQT%&D9BsuPKT z4ILPa)a=#mUxV4>ZUs%=+z2|jX;Z-3;_{-zgstL@6SfH1{g4_lic^ty9_|QHUJ{MB*Fp5RV2AYc}IkFNugNjfKndp8vZu=%dGwBvLD567t)*L-NqDOd+o4#AK8(5 z_4QV8%gJ}UlH*V1M(qsgG&~@e;XFCqOn$DC)(xC4sidU;Q8~%8BWK_y!(n9RRfrr| z?T(cN!`+Kumv~uT5&qgGCA=kC?o<`imQ^ zS7uj+UINU1RoHtY-Kf@swcXy_+&qzpQo{djx`?MD9zwTZ0T4HQQz3efJE5&tE@UXVfZbW%^Uy3dDUJ6O)WX?cZx$@l9UXVcjRb>x8Olwhi$vx#`EJ60Uaz zr%oGm7Mzb&%$-x1VXeHDiR8Dnlb>kYuULf)zNHBnsdgrl%tFnE%WT@D@K}1>!ns9< z$qr^yjj*8nHKsg(nx2U@BslgVlQgjVqg!dLnFy4`xw@?Bc42!wC`G`@(4@Kw*|YtX zq0gTvR%4-1G_GiQ-~9_$r~)t+>)KM6AtCo{?V-y=MdV^+91i1n%MG%x>2Mlu zj8;ffZbJH&K%ppM?N(15{Uejfa!Qgd5J(dvAN+g>G)s8b8Ii$M>FuP-?1OxmC>BF{&pfLcCRA|YVKCIg{L2Xf{3{dQz1DUXTZiuS!@QN7wUGm$ z?!oLUZ$DTtvq|J54hRaj%fA3o2nQxT)u?o`l}j&$Sxuy%b3AEsuTpI}S53G>O{u%| z_UEgmky$LZGCbCuEZcZ^8Z?X2pYmXgpIaPz^!hsUR!Q=h)iRYyif&O+QKd%)zTyeR zkakgN{;2xvtQIre?YQtRX_sKQVIQA=|54w+6A4}OJrHt&u_TBxgy||3XT-kGg~QY} zn2N&YB0estkz<+ro(B*$oANjXy@$V9;?yYH1lb63tcEo4$!NZdBlKE{;XfGyP00wh{tKsYXxJNjOJR0^ea6m){NA+nh3;ZjTHL zL*&%_q+DKE^$a%BLKl0|m^ELh@=sn^Y1^*@_mq*s|IBYU2tNf_OFV4lKT=Lv!lx;eQIhH;BqR*~6r2ok5!MF0^Xgf`?s9nk$ut2ADb^(Q&lnXGxg3kEO8GxC zS++(4I?jgdO8C#?480HWe><2}c!NFFR?r)Byik+5IkO$W#z{dH(U9rX=C2VFG<(J{ zGD&)RdeU|q5OXe3#;4f#^oNhux#AvLM;O!``!hCPx2+zg))MrG5|DRi zOFhu)bQ~_$x7?Wn8KCqR0Bl`Z7`92x2g|Ne-`TzulFbK~g~7B~QjsVs6clq$&_8EUBSlryBy137d5 zN*;e^Sxom&`hIEwNL(Q32{bDn#>9EKKc6h7*grcxA#GpUY&_3IPA#<(O0K!u7}+oA zJ%$WY^sfD?>NlB#8B~p8+7w`RN@8~FI~^>wE@oEI@kN#t3HW@!>RkG4=>%I1Ih)j z*+C(z3urDHcceoFZQ+y^(u4)ua=94%ncRLqcafw#unF+KjDTTdyI=)sqjLbIuS^Lw z(1L{HR1}bD*P^0~kENk~h|eH9fcK>G=YwM&Er=HC< z*~G-eJYH*hg&O%0*XJ>Z5+WrJa2M>CwFTbSl4UQrSaJS3*w=`bM#{2CaKRZ?Sfezb z$?*OfH`Dsakc-Z1nG;i7wi&R9mZ!)2ih8|if@*Tn6s({upBQujhpibk*GbGM1|$-k zn$MNvUORs22EqzUkhm0m1(F4H22^R*dXbR_9RPm%xBrb&_MB{ffODE5QYM%Zm}%cL zmm^vvijR;_iG1*joY{JWQJ0O*I)JBV)&X4lGo!aIL0{GGLk&rM-A<6}2`oe1^^oq;AUlAK_wmkSb64S?#h!lNscd3qJ*p8&AhAB$&HLJk(F zD`tb|r7@(CCQx!*3=73ydF3!xsUB$X_TGQw*QXrvXCuLSm1O)(vvmf6{G2j2@k{{wC>T86YEte$C@e!vf&j;!L%iGIhP1I-a z-7`R9-SRvcw=ZK23g7wy)(i((ZJuo8wctE$+!B?{kj~SoQBi{;E!Jf~AIIOw0BVEN zGZ4TIdj<={v&oc>IoWuIvRJ|B%74U?9^57Z{Gg!XHxnneyx*RRrGx0r*2mD|{62=d z?BE_yKImo!9O&N=LzU}d*1F8V(4~056w8IWSlnl+TY;N|9DEHpH1_#EpR64wh^C4U z?%k}wNiG%p4@u%@5Hx7pb|n}OWmHaVe&3;HhaJeKKfs867AOGWl0 zr5}USQiB}#gu&W6?&6mI5iGH;Vv0e_pGwmV_A%zMa9L+mkSmDYetIVg)LrfFn7&s1 zC+U|`2+VDwcookp8zg-DaeZK*`r(%cgc1euy0o-ORE&VZ=+@lK++)l45;tBPKXDiu zfRbhV7DHbDq=RYYnhJ2LiCUkocy#H@Y!Xdq<%rHf+ye0 zK9An{etI}H)R^VBA)J)GvOi*~+x4>HXqEubGEs^Tha2zT_q^O2FB@M4ykWt!KwyDP zfG^R`x=bmem7y8wIs|e%aB`o;oA~TFl_w>kwD? z9Vma;F7Q=Kwvt9Pxa<-2D#=Oh6n{+N{SZb>FLpqE<3?czkYG>~@IW6udURrW2_Mnu zvTgp$SK}jghXWf4+ZG;2iR-I?rQ9d5F^dS%L07RL*!b{J{Huf6G*DVHSGhG1bF?ej za2vp}E<|eQD<$htbpjX8s-7ECZ1_esTpGC|X9y;i3;-!hT*R{=myJ6-tkP=-O9S+> znD~869l?2e=qg-FCaoO0kfd8`q`o)_n`qekq!j+1Pzovt5@Unc4u$bmIb>{F@O?t{ z^}=%ih1jGBFpC(e0lYf1PiFt8x)1Fq?cd41wuiX%lt$Q)G}v#o4ZEhw>z&cD=L@4L zAGlLi{T)|sK^c;bh>-lGJz{+Zi%g36{jQeyR~-rQuNE}p3!CnX!>z79oL*5g_;7lF zR)_%-DIa9*pMbuE)uit2v^SyHR$!I=h-$caNNp(y2i;HDUtY|hm%lPr>-mx-6Vxc< zK)I`M>U~>VfiF4}|5Jj*i&B(-d^0~9L$T`uevTn<|81{q1}=)F5XBR{>)eYvh|3uZ zAY_qQo^6kue@}fs(ecY`R&IPV{GjE+`yX*U`?H?FFpENOuKt5`pkaS4dA5^JB`dw> z@}UI3eLh866~de_J&|v@WzOlNO3h-sTPJpg0NtyJNVV9l-0$;o9DQA_j;pqn{I~Vh zpkSRtx~5%Vmia)a*6oKE!_*rT3K#QOiuKWu|pIJprPlco0;W#v*;z$N9(J zIA?jR3@Qu-9g<>GsiL%KsWeHlw-OtG49snBX+S0x_wng!Sz{uoW|7UjVcCG}fjF!2 z*2PTN@1R)39QTxF?+qx~QJuU$5_Fnii4i||%JfV<(5|IJ@t?>?G>%mzRuC${?f8n> z+8t{t+*WCRIyS|PlS67Y*P{A{lFTRhhf1k?W*SJSC~JR3+=7?hz)gt3hG58Bx?&K{ ziZnpAPNvKZa5#A&Mr#0E9s?nRxPz)Igf_Jf8c28N4FKzh0Z-yFLTzN9vbQne*JS`s zR%I_xz@{I%KBCM4-QEPJx9Hy2`{w~E;(o6|nwFr5;kP2$pQlKcJzVU3@RO2`X3CzI z>KPz?ZL>A!;DGD2*DGDQ_*iXjp~5|$h;nu4%knBopf2AX9wQ#|ZuWeM`-XH`ch>B+ z8%r&4OHJP*K~Q32C4w0OWN~}`LvmvoX362Lx=f%xBxb;DIqL*5um~}NmS0Wz?F7PT z%q3aGUTiVV`Z7@76!j{;1FC_H9I#II(ljxj%5mL@JULdp{jzuk1(2U52z}ia5wj)t@e$|4RAs-Cq5h!2VH|$S+AD=jonu1o^b=lc zhicft1BOpahKb*fv+w(GO1HSx2>f`X+~EIR>zV=5H8dds zGO9JRe{@t&|D9Kf-ADpPdrfxfowwk5BVeTA5z{KNQt-8?8@yPvB;ir9cX8df60ym{ zBd5FCxNkqbMM5k!(Q<1=C6EqzeH~T~q@zN_#V^iSp{J&P#Xo&F0}%~o385XP*Dbf9 zuCeP0)KuR~D9+~m6vAq3m2sR(9?Bx06udEnYh~%w9<*1_NUAbGV7b|8g6H;!;LdCV zc$^F&Hdzt@i6{&1Cm@f1JpW<%AGc1!g10p@0wT++GupRAS@IHT%-aKkAGveG@*+Rd z#T+=4zQnb}by`_y{hloJjpT&S_`CWzHRDbpmKPMkUCBr^j7v!~y zvHL&A#==8fPXVna!&KK&;zNnN-j!`KXo_WZojhW$YLhp520yj3^_5ZU!2hNRejY_4l~Fl|%R40aFg+XH%wO=9)H@h=h-l>flyQDE0` z!dA4_r&uf`R~(BEo(MnO{iHPS`R$UDGV=SJ2L(p-g>5DdT0VDB=a(_6ja2y|d7xFB z43w=!dC)h`Q`acegV(CjO2b9V+^u_`2Leq}0iOPy83bk1HR15Pe^lr_4xM%?IDX!? zCgK2Zp*7XLKtd{;7#+{H`u@bA{hOyMVRO5LvNL2CROLjZz+)-gJ4Iq9KJ9WTqr80wAY|G8I4ye+97iq8qJB+^}b7lQ~J6OS-@j|Qz7MhS0n z$ey1XG%bdV2RItN^Xhro1ak2R8U}%ykx!>nPoHI4t_&8gq$@Gd!;Tzx%)OucR+d+? z<0Eb_zjFR3O@V6U_uXKvj`qLRKUZvWOQ^Or z4QCg4Nih59({jU#)WS*Bb$%**#Iw#yM(x<@B6oN6ewH{62j9V=ZLnkrjum@ z{$={s{L!smOhyJC5v)THqp7f&Laae1T92=ses#GDESw@z{mVlSL%qzW%EEH;t0 z-rZc>H-6N^{gk>M89=94^n>ci1R+_gTu0W0s`KI8zERogOAzxoDr)dkb;;`1J>K_X z|B*%zX<;2@TSW_Z33Flkg0QHsTl#9E-t#@hgYN@M1qBi(}TH`h#m4)UnS|R$sQOOK&y7pr1sk^zvUe}^ZU;Q zHyPv4LqbFI1=?ay(ht*wnO^*t4Q0Q7Fpj2j&d*MRKPGl5{Z=QPF;M#)*PqC4ooSVs z7+;~5C?3~q@UN+Io-!f5zh?0TsF=xEHDojcOc&=1UB!h~dA2Zaq62lM6i6x{HjMK{ zXx;OBI|j+k0nhzXRr>igZ+nM0f0pl;Vxw5pJ{at@4N4rUY=D5v>dR#eA(emH*3qE| zpz9`SDp7LR=B>$Q9NP!YE{(#?e4sW`PV)YzR91i z29$(Or2usY;0L?g{)+Yu$$NE!#rm3n=|C&eL9VJ9iVYGV19a(%DE_Nhw$hV3>Skd= z8?+!ZLL*Kh1`vA4Da_A-lD*LV;!MD8Ag;P&_*!}0Pg)qYm5nax89(||VkkH?1*nG7 zv&R=!X81$B9s9GTcG}5JJ3$IU5_+Xnq40i~aU!U_b%T5(5nf>Y?sY0*FXdkQgVhFv z|GY!n>@iTn5Bh|=y1JspOUvWcFUCXfA*J(Z4#xiU zLXBG3Wg41`kpUF>z8D}%)DD)Jnlb?idK*g}EECHH=;ZTc!bvKfCPi4aixO$bNMSZ0 zOH{nX@BGy$Tsed%_j1TQ6Xz1CCynB5t%7WchZv!Vj~SwG@Ci8#stQOn0lL(*;4rSO zA{iKcum~U?amytLI~^VDF?LIdd<<1IaW>#xU;bD%J&y;RXAz^vCJeX#3Z+Qr?Xo(zDgOJX`^oZ{ z$DnfA4SE5RpdLvemF)s$-@$uordTXkgCI^MK_4=cQK=vaWsm5^yr={A;_Rlx@#0pe zK)E>K{h6reO5RG-NqDe$#8v^UUxX2h971Md@&jPbN0)Bonp_X0UI>Y`!$p)gM_vrm z3Uf}z2mny-swq9ph5I#PKadAY_q@!+v~gr_Q@F|?592bspZn{7urnwS7wM2Bv~P&A;WV0nqLCqFsY~g!Tj{wF(ccg9OuUf0JG1t<95DY1=L!Iw z?M$D2uuu7iRUdq*+xHA3v}tcp5{soJ*l&Nub80l54oVyxiLebJmNtTE0c(b%yD19s z5Nj8DQg~nBH-|ReUnzW{C1+j&8T=C5P6gh0*y%kBRBvN+b7Q1awzbRw^*H?eRJlo= zgA}d*9bC}Qz??`SXu2Xq*A2u29`iO_PqB#Gc*HQF8OAhW_vd^7qn}cRTTb)ulLbc- zDXqN^vA*2(P{8?Xar{{nrKms`6#~~ZjP^&!BEL`22rqDUb}?+F(v@_`URl4RdS*fv ztjZT>6}lwhkwk8ht4wN{#VyfsgPp7pvUD1nxF$RZS=ZCp5L_4^KV}3u4$%Q>V7l^h zn+S>QvT=GQwqUNFl#kV}jEbKB|Ia!gRlp^q<)`{}sEQp8T0`~&qFU=_Ss@Owhld-J zq$na8FOm>?A8aFwld0qfg<-b_&Zkk9LxvhhR56OM%kU1bz>vk6~Sn z_Wq|PyJgxY8R3jS z(hWbd^A4lL3LW8_66<#rp(}w~^&RF{g7^-%YU6&9%z;=kiEpl)9K~M8#S0<2miD9h zBQ@rVBCgEaGt#Z3SLf`aFXU)0Ic2=1S?%_G^6t&Xs8fqAuVt*A;S2b4NR5M_z?Cqp zD9{gL$7lBXr^xmST_BbeF1M_lW=I`fTQvxEJF}6T=r}!ePsLPLYR5$n$E>^^5isu% z*?f~P3@WW6s1=}Dx+&t&zf>idwja(?xO0N)xs3HctC)iB(go&;f$pukZi7`e_$*Mz zQ11INB$;6(ZC?X2u5|7em-7`tS&(~Pa8`bC4V`>e%w$}^Z=4_R@4D{LK?2kYPwra3 z_~!`T<%DdZHUP6|S!@D``54w@D`qR_T%jQ z13oKSGkMo|l(Tp>>?pqrWOunbVui#cv4ia>YQ~tzE5=N1E!yg7*uMBJ04Zki(r-Ed zS&c1rNEo+FlFBjQ1JnhcRk*$7ZE6(sJ3w(#Bo}M>8t|<=*1Z&H2D=ZW3XIcHaq7sh zC1N{Y^KlK~t(jyJ=L?)RwVf`k$D1R0(D!nf>@e}SSbP{dOhFi+dT1B;UK00awr~Od zx_xv-=NLt=&OYqKKvu%G@r7!J;_Q$|cn=6l%=ju{DnA|tWNqZ%Z4M6RrJULKU(E0x z{2o&ZJ4v-Lb-Qwhp!8jmfY>g`6C@l8$;rO&G<+t7!u>1D=f6#3z?&4ogSxr2OBj67=1LKIUU%FC z9=+18?{DaclXRJG3NYybkdZAX#mE74yyBsha=#gZo5$P1uX(^|)hOwy54fIj{Jh#EE^CpritFfTW# zwnGc%ZvcYe$D-tU9MB6dJ$!@q@0Wmmt0-RyS$kC)0xiuV%8ZMPtKENDP0Ku{yGpT+ zh)I830g+jnQENyJd3tVz5~yt2y|M2_VBF5!@uAu*Z)o)O0D7fF3o2!psSk?!8y2od zruWP^XmhGomT^KnhKjTmX*fVd^9iSE=~-*zJ{t#SM8(^-1^O7VT;1K;H{TdeRwR5# zZ!uU0NV`m`JaU;3pOn1HaFJ&gbxrqOzQpF$exIz&C$~Uty^eS+X#;&Xnt5uh%l#Bu zp$Gi$_2wL|SikgjjSrZ_UIMj=dC*jr2r}1hkiH-R9%Z|s2C%TU+$iY4`vB@&v=tQ< zD|bEu@xs)jqwb{J8dWyxXw{a8DCs&G3j%s}cH)c{WJQ6dw^NBbca%XPfT2@h70_Dc zA~`}`4XV@2wo!8q#V-iM!w)>pWt^zQnehfHYH`(Z`^r0Ax_-c&Sohn%C=1Qa-HNpk zi~{83#i}0oDid(V-)A0C4}MPpWs7{R0)$lPCSBCEnb(om*yBNZp&4jUjd1*0maswp43XN5 zu^2){!K1m+&ImWcunv75b19i`xizmd&N&60}Pl z4B;W!dSd@F)pmsgPdr3e^VXINql7WGi88L-kHr{_(~WnVgP9r>W}hl8m24+#tLJ(% z4A*b5gC0~niGSpJw{=VNFskix#_LR(C*|dG_|VGRn)giXiK7>o8C7bzYHqoIeyL0L zwYQ2mW!}U;IE1{1GseTGH`e7#;%I7DJIj~K$$r4_7d0RJ&s&E>7Q6-ECkZie99Bjn z6@q~X?BUb3i5kt&ExIaipeguN@8=C*{D%N5Gu%?`+e2SZMV2Kh1r^Tay#LP$VJOGB z2;P=m>0=Dc%G!Li4Nfoq+u|!Ydk4kt{Xm&tGg{kk`<}c$UJVK#Xdyi?JlA-l3E+8e za|@fbV3najaf}nVE1fa6MnM0Ehliv4imM5$TxMD-9maTSP}{jHF_6KcB)iRf8E({L zY344`(f0I0>o?h6bso`T5+C|5h_jNh;OJU)4n=Je$+#YLEW4bS$I3z(d&SQ0l|{ zrhj>5jaQqX11dhGx3U949j6m22>>%f)04?7QAyA?;fcmTLY}SBg8MIj(y=nPN<=P|0jYjC(gPNEG7pUI8~8zcYZp99(d3%JI_b{g$LMn(DSAt!`%`U zAS9ujv{S@%())dYODpJYm6 zfEJHX8{kLkji==Lap9-nxu4y(vTU7y)K0)2I*;~fEWCUT&_w86P&yNLew zf@Tr8#MxlSTIwPA-UZD%Ld}<(>+UZm<%2#Zc>;blioR)+nO&1>j(cd{tF#gzIa$Z-mMSpnsp zO=pmbeEaro9u%(aoUF*vE3NsW9k>!eT|Zn^W-Z<960eRn|xjhl)Zfm5De z?*wWFKw);}DJ0|pY@5n{K4uNr`prg8RA-CyAQ0|n$b?WxlxE!tb*)7l-Zx(9+V%eL+e%0eS=w z)*C_c;UtVC;?HadO|<|=R>UzLWYa9>y*{3Qom#{LE^c$a)k?V2Ngsnk9<~s0GAu6w zu1A-!0fd{1bRZu%*qD4-r-m-4&SJQ~)PDKIj?nRZH|snRe3QbyC@KkPSCW@}nva^$ zZ4*0dulksLq4kf^Wq=*fRiTLHwLlk_9fclq5bDB(iZaQ_E3Z3_xHr+YOrVUunhY>x z7l>kLnY-PjnFP_wFRJe1CiT%ZK*<6+FMoWS&$^g?;`ZAq1mNMHnUtd2HX|s9$%dPN z>}ybD@WX|hO93I=o&e?aGy=L=SL{B)S-LZqpw=x<9l+2hwj1TK z`PEoFDh~*+!%atds^dSM25xex9p3Ihlo|=6vEOk6sH2sC1Ui}kL4Uxu?0{YV0HB#R zgAAa#kRWl37y|O+k`0BAcvBf=bvqHqnNkW0fhLXjCcpV_8PDBo1Vwef^ym7A>UQbh zh0)3N5v*z7ya$-+SFC~;xJal0f@a%;v1Xe#21 zfOhU71B~pniL@Xe3ZIs`N!WB;th!HY`|VyG3o$b4`tVc@9K0bTt^*)>J)(?Qk}%_K7z8Yv?)Q<#vwSgc zfZZc-Ldb>>-222|n7oB5qrb-l?@E|zokQ~kTEtR4>?#?^ZUULc4rT_*f|>1a?~^)P zsC6c+^_1=k_#e42PJk+YehBmUh*Ds%EzRKoXb7E4b8d(OJs~V|rQ|wYAPcEao(_w3 znh?#z7fVA)@dq4xv}2q?qk_9)B+pnL;1KGVlW?MpXy2lU_xhy?S&0vqp2e`K;;PHF zVVXqDG(d5;0DnHTNgLui<-?Y_f#cukQ)|l2x%Ytl3Q@e8*mekhQpX`~1AQ|#6#^Ax zW?h#UNPhE>zoBev7LW7r?xvc=cbS_44o`D2+6e^Jb`|b5`kh?i`zG;ehw`JyUvXZQ z=6hr{kmWKrG(B=}-9IIK-&t1;7dtTfWN83R$%K+H{?=Z%)f6WDZH5$b0h{h>rXH#VRFq$| z!GK*R!ab5eSKWA1@+_#UXkW$E5=+dzk5X^Oqq~l6!h^Dz5#H8{CqUp{s#?cCZ6&zy z5hs%|j0v|5Eq)Q8eQp#W1^sq28HddMp&*XmjhP<6ifZ0uHZg)GPUlc#n`4-DZe98Q zj=VRZ;YdTUw&}?hA0zq6mg;OnYkd&;<+s!{vE`+}o8^mrAfg}=KMIF%?*kX@LzX=a z+4corGPM6_57aMo%*npq4kOkL!nwtXTi!wIux0)}Y<4vugq&#WD|nnii|fD#9j;^U z>juKAEb41Fq(;PpJTJ_I?MHcr%{Jgv0a`%OXcn8`sQC-w08UOsGW+%0_zlloOlN7^ zG`Bq@Y0^J@K8@IvEhZo*^d|&CT+767y&8Nu2<+;&Hg6n!lcQ{vkxUbpwscyU=1!n* zn%d6ER#{q%P|3T$`C?MoNgemw5jUCz3H#wzSKhlt}sbI@*q(7;(y zs##KEYm`q|a>7yC#u=<1Zhxbvz4QO;?9BtI+`cz(=a>#s$CQkjDMdmkauS&`6iH^4 z3>7lZ9WpCYBt(YHnddQ6WsFS8JkMlSB)|3EbnpFr@8|c=uYd3Lp7-5*?Y-A}*7H0& zH~&6$&H!Oxu_B-X*I(eBdO-=V^aeD!-?(7c@gMsjkKw2%smD~@?p^a~!~Pq0N}5Z_NbL2Mkz@voo(ie)yU zKUE*)ZB{GlzO$)98;7a9d~)H)O}d9i*n`<(N76n7F;;x9X0#D>Si$#XTvyw@;>g`o z%UFAK4<{Ap6#vNK`S2q%CN%9lgW?nQ0b)Lb*b(kl!28}-<&V^mTwi|lv@VE-c7f@= zBb{tT%Gj(&wac>II~nyc6J5Z#H1n}hSr5ylmq)gooc(evX$NS`Xol(eW|+35>n)W4 z;&#g+8T{sGH6H`Ti4~y%$SI8`9ToqYx^!;k7ku&r2C?O_R={C>%7>#e@oM0v-J<_~ zhfF<-k=pt+s+pLNHF?xTPJyXH1Vb!~VU}6K_K<}z${ff|V;Undj_Qpd9`zy>@;){8 zM2a;qhnIZkDEqUK%yyj|M@j>3|`Tt{e8Kp=+^sAsFk zem7deu=x3%gI}=s5-Y;g_&miaoUDi*+N_jmNe~?1?H%_=uXyheaj>?)VV$xhQAuPL zw?!q(EIH826pR*>AaaU>9mcU>eieU{g`;3_5@r967v^i}nk>hN?Lz1KZQq&tD8H>C zI-k+zt>Ba$)FF}BrRxudJf)9@J&NN*y;e^@vpT%WiC+9GA>-#fv{9Z=&u-5g%c0VcYE7w(j)eh|r)5cY4hkx-ti%dy{GeFo*p7N-j z2<#@lcnex!r)mg+OXDYrp>>mBdrNa@#HsU8JjoT3kb~ZYn?y2U7AIXyzu6rnCE}Yp zAVjG~T!F=0^mW9NsgIyM#V8VJ4~<}LdU+{~@w?XFrK)HjoRlHfHw<&a`|^3__*C2N zvbv8qW4#U@dVyt7u>uNVu{k~E~_B($T1%kY#GFa(02WK;{j6)Z*3Ls_xwouJFOiE;p}0N zy#)+^^x-(C0K&s1AvwQtE{*DY%hTR%d`>_te2*0O{rKjin`+$T38K|)bjK+@{pv4i z566;MeY;wAG{>>Kb9ZiOMYeot?}IYN&9P*O7Ywr$P!*T!oL0t2bOg`pDhG?I8lZe zZ2{bdxK(>Zz_#s^p=o*^`Z!?QYPuG)v8~)WBt!06K9D~mqi|8PCdixA+3DfPdtB;q zS$dhLp({O7`rA8CBfNAXjPZ$0gR#>xS;v zMh5FV$Rv9c4JCid=*fuGB?i$S!0I&SkCd@ag`OhyVuPJ=BgBpt z=&oHKBMW!ZRf*Tv(D5J3w4ejrZRClwbaBE_-66{sR30JHi_gm;^A=zo8C`rNrgxXD zzDj53Kmlv5WA4@&=Nbx%gYnJULJqOmYam%+$ct5Qa8X7oCqr8PV-RSm= z4Oyrcj0|tHabtH#I;>K=xu-{p#21+cHYXPps!yP`KvMFFqMa1b*~!(l7Go80v0 zT3+9>K7+s$n-mi@O-M&CZba^K8e0J6W^`W?n{l zw;F-DMohaS;(g=u0!h#X*(7F+=}+^2qa9}(w3&~S^}fW14|6-EJt4B{9n*l4Cp^Dy z_k;t}t?_n+>K+8sdik+Q@FHcM37s@qdzA;e(@8?&F46Q;i-PQn2i_Y$!?%*1>Jl*_ z3=L|f7x0?~8?IDe;#yMv|AURkyJvE=dBBb*<4N%D*2S8pnsf3n6h)K&K`;xluT6Xv zBzuZuBeG7G2+pgGoN}Q}w0XFEPV3No4rK=NQEYAh0k@0{7b2B@8Cpyy>WCU_f*=h9 z*mwM*&dTAVTHnWJJW8}Vj*R__@kjgL|97*cx+?n2{b!IqasLQO=sF&hV@PeygOb|Q z0u{w2fCY~I4=bMysqQLw*B?-zB7(+_+*|VDc6f3^nbJLip(Y{xWw`dc zv7p#k1(o*}*;UE)D`7e&-0*puym8aRfwP?{&0swlp7Tn~D)uq6#8t@H`3Ol5YI?0e zsi@>M_IL%cm4m)*_ll*sDf7nR6WVM*^`U1*k%k7?4&Lof=LW=6efSpiC!Rw^-3)D} zBq)&5D?!Jy`qOboP&l~#Hu7%x`5z{!@vd4zF9{a!o-!g=wJ?gq7u@@L70hgS0b;rutNg`z2IMtY|B_au<>P|r z_<10Sy4D3fD2~KSPp+a1Z#OT90BAV&pSX4DkAUb=&NHOO2%xhoH>0RuV%`nrnFJMBbSoi*1cdw*U)cB@Te`>O zDv#J;f)ZZ|P0V1svXgN=dx*3d6c#QIKaxa*OE8uHwd=9}2S>c=>wZirGfx0;&o#^p zi^bW0+wu&xfkjmlaFO_-K-@fzqZiQQdqhGB9+*7}fIfHsQs-imS>-6$g7!%hf;z$f zpw3?P{U>PNeVuR{Q#WJfzS5ALA`49xHTv)P$$9lz}}|Bils*Cf>#^Y$hiK;XC%OO@xp zn?b6ZDvDt@5507~z|#V{BV~{xUzObikEphQoa}HU&n#^2wa&;?7_DS`r|1<=1%iW~`Z9{Yw z_rN4}v9ngf7f&wo*Km%QD8E%n$fvpBTzz*@@tM6hSII3EsB-Oc$gOIxA4{yv}l zOU}rA?5PpgfH>9q@BZpgEGRpI6dOc5AD}6FjT<-J63GcYSENVN1Pir0_S7pD{}U17 z6xJYre8#KB>ip}+bP~c_FS9cbm!pXkkfL+*T_Y^M-=8dSfn|LSdizQMNKIC54Lt`x z-SGMem=k&~#RoDN0janT!}<>ILLT#0vr1Bp^o{gEg#mBYGyb0y1aC$(11-}qG3xi+ zxY4BghYkJ#&-kwkgufv|ufN{=neuacK6`FU+&3PWZdYNq&?M%|0@~F_6VOuH>}=jI zG^CBa&P+`9G#iOyyI;6@hc?dNZ>y1GdK#n+9)+HHdW(xAUXXo8rYDU+W za{%kqefm8WI#^zHi-Qywt^b}FhWfWg*8hKce!|u{hp53T4YuGGv-fb68|MXHf}{MB zwTBj`skEoWf=zip5ZZU4rN;H}*H1KiVEx5qYu2-N4M3Dip&c#3v~Fl&KlEqmpF=u) z2!97;2jP5j{o)@`>?OT>KR(MJP1~D6+4p*hnU%m)9f)#xcBl0=Lbs7PV;*16Dt^qo z!>4iZW%SrdQX!iQDNhm-yRhW4=ziGBzweEMhV`O}kYsBoYw=|uCw;at?!V{Sd z6QH)WV_vxvO|~~LSQ@IK5j(Nf4rRf#i&MG0wku=r^K2%SBwW_b0$J6WaUr+mU<1?g z>vZJ@pdbQK3W3E}m&vm4*ehJYlf;`~{hRD9C>3{5f1Tt&Kg2sqqbgfbSVl$dvhHzG zzm_o92V~UrEE&!^Kyv0***|AW#yPN39`b^5o zjYBLX=DGKbcJ`2}p|0QmR91}lEmP177X8(pA{qTp=&Kff1DG%gl8`3OfE?}HsrO-y z(vKiQB(U*QTq*mUD9_J6Z}kHLT60QoYwsDw)t~+3SLQT#w(k;VWb>MipTJx9BeVs~ zB`f119r+Q9;;!k+17yv>39?}5$tOe>omRC<8}$H@Qw4rybMg;o=S?;-n?I<7ETZ_} zK(e^$J2W9YT9;i_w9&;V5p)EYIZhMBHs7Cr<7S^q9`!u{ysIl##ZObuXWA_`V+6_$ zK7A632gYEW9xH+iA}v65G&X7ZDGzItw(^i2$dN` zUW$mE>9V5L6(erJb6YB(w#n7K2bbp0HPW`3uG$l;nFWND9ia}sJ1yFh8;Vnc$CV1~ zKMcHlOJvP$tkT5P%%RKB&5+uF8Ay<$07GpZ4sLQz+rtMW=IOnHyf6Nnw4+Z=BSiGY zM#F{x1%4=$1gshRQZKv)*&7}uL4yiDgu+Y~dHQw(8@M$+gIymnp|(F1WOw{x;zt!! z`q5F!Yr|nL_5COn#V6+ZLN9P7*oA4{kRJMSoHcvA5A#3lxq1We<$NqatOcRjCCj0o zd$K1n%|6q_Up+SlB20Nn92hC&e9!k(OS~pM|MTh?q4>DQ>rfDd9EeER^Ef$TD^q%A zJrC2L>jfcC*Y%b6j~wG=HQ4eU12d^G5YzDnQ^%J8ti1|y%4TRUs-P^qcKJ@Plhx?2 znehjd)80?p>MYb7Ay)ar1f#3R4&aw_^WMX_o;*WP{>MlA9F=y_y+{#o4i_S$@-_uZ zarZZiFIgvn0|C+^MG`RIl_aD3w;JKhz8c}O!tDB-m`8i#`TYuaIn5f%V zm&?Id+q^qHoLSOoxr)IZnyPvGebg^s5nsy9Z^Pd<&K7jWT_6PV&xHGZ_U3Qz_mxbpr-a&U zmWviCoDXwz12^gvlatOjEsm<_c^Tg~ixTH2CI-8XxKo_Ge?WcLqLz zC|SF=H&LnkPElM;LxV!d^9>$aP~HeYKSaZ;VORz=Cz!iua5R-*Q0FOMY18n zfX);(X!hw(K!@nC;~^v&cmu5QJda(MW9o7EFtC-f94LA4+SIn?rBEI?x_Vm6@(hu( zI?sINxh z|42`(N!f98;YPr3i`Mkq-a*1`=u`8N{7Y4UC>Fq4Ci#=!V2<%4YImdH(h3ll&dsp8 zk*GO>2RUL2ng=HA-N9OiQi}^W|Fm_XWVtqw`)R;uMvz!XX`t{(|B%*fzFb2-Z&m*P zbX+4khO$av1G(|>M@x{}haYhUqQ`9z*x7(00G2Kp4G-T1zsKd`x&=@dDTQ$#s$qdE z2P{mXw?<7z$DM^8Kva1MJ&uTn2=SN?Cy=z6@L$aXs(=B!DF?zq1R_y02-m{Y!Dc9S zjiy8q39)P~0xOSjBJ zqxBlA1xDLj`Qkb>q8qTNz)D-}4k|^>>DRjLnmNg}>iqIVyh% zO#(TeDCR`;A6dQ;_tPi>pBy(JhK(L~H4SXwQX8}04r>@{-#B1uv zulVP_;J>cwEIe9y_iJ7fqSt*gtWmx^b$H)wWm^xq)}a|&1<<_at$!>9gkPo11rZe9 zq@)CP9#D3(WGh_}2V=JNwMf(0P>?Yw{rWzBsHU07Dbf}iOcl+`jPL$Z94LWgWU_eJ z<2iJjy9&~fE;HHWCw{A!Jt;lGbMk`Sn&IpvQ8~HKQ~*rrtH&~v?4JwbJ~#_BEqi3v zQ)SFKr*<|4eRKgZ6p#xQQrop?ojSo*E}NE^Px2l@{wVvzjbs1LMm|WxXwvfXigOB_wqiI!`c1WWmt4@HxJ8DI^lz;`?{G(`q^`E}-i~vm=tB~WT;DdD} zYlnLww4EageMged`~We6#aF)=Mr99G^c`fTgG6@rokKj?*kPu3L97x^Pep8S(c7e! zW{NtlR}ka*1ui78r#$cHq{!+8IuK(qSUS)ci`7{8>ivcGeW6t+_+oJ23|$vRxlghC z>hE0Bg?hfBAh5k}LeYO)kmW@F%TX+s4dV@PHOk>muHjoFP zSwWEC^b|DNX)@OABP(O0%cFH*+pvK0*<$vR?%Kp(#gjBfnER_fLW{^!wwdp1K=4{< zL(z}-;53a+~9gnE`q0HQxgf)b%{n&!qW=mh@_KhWQB z`F#GjC$Lh>*RO;YDU;LpGbMJJ+mDhDhvpO4UGqmMO5s?QU$6P!_N$&5)mC6rhXmJ? z`nLNYd8H6CEtm@nyTW-To0o2$zyhr_&^-u5(n5K<8vS=(Ch@5i_PFPy+{p}ccL!(a z78Q-fDP>b*s>mcz)lQ5;dOfL->f?Rm`ysEcjXkGh&-B8gBHL&8`pRi$jt9Dhj6Vbn z7pIP;W&nyx0%u9R-+6bMFc!_*>JOk+)1<`~Q#q5yo;VxG=g%LOZ#^RT_!4e9_;(ShdwZZH<0Yp0TZAZ@V3lo|V;7Sl@I$$yBbs z$^OU6SxlVAJ-Rt>0yKsVj}IX}P9AqwN84tUMNB}>Fd#wW;zmfYVzj1bGSe-X z+aWuE?YC+Sryt@WJ#)KXI<(F-)%}jI-g1qmvT3(zKw;awpuP-e()P6!4?lF;dDqN` zU#K*Tn#h)&GhRI(ti;4Q5KFNB{uy!FkxQ-7mp*>2%2%x$v>6s@Y;G)kc-rri`bkka zRcC9Xm33YI6v{7RO1_`lhc_Z8tlM79m0$fDOg92NtPnQ5%lPGEFhDR$^kPq zMV5wJqg82>Y26D|XWs{}50@#~mVEATy79`dEm}-jI9m`V>W*>77UbaXn2Sq(9t37Zp6%`IASUQ3oEiyH#y z2nhiKv=DdolPIy)fP7&M>%u~t9;@&WfwIA9U;dP^wc&>{SNjWc^h26dYYKJSp+F6x zNMgLYVJ{!=6KvO20CG`!*DO?)`V&_ z4e6f8#IQINAZL$*;Fss)iTCFk<+kIn=+r}up2zn;Py}V!RbejlD#r1X$L-*I(UNMb zwg)91uAcs+En1i?ad9`3|O?e=C|wO-Ddr%dB~&ETjvYxccM$yJp2 z3t1c_r>tU69D##oEU!G@l{PLL`r=c~bSt%PJt-BAlV_aZ_q<|}csq~Jw++IETMESl z@BDcBQI4L(W_;sB{95eFgthLso%Q4tXG0}bRn=Zo<<8E|5Q=2z7TUe_0)tC>mQc## z6=&PWr2{ln|0`U!(5Xdn0+-j}u@lRiN8hmVehFnuuMrj!m9r30c`h7VRu@X_S2274 z7at6G01Q6X1v?re%qH931w z_OAuX=0S`M&EkX2JLzdvyz9{q1Qo9scustEx^7n3MVof@I(onqrVx? zwN5|zU$N;ru2&Yud2X|*itcq|)x)W8&a7k+&Lw;K;BVzUbheA!ZerdY3&gA=71$Pc9GL*CEt+94 z=Zd~47<;J0zW%QMAUSc8)J3sQNJxdmrX~lqBj|DVqStnPEsrlmj=ZMY+TM z5x2LS!F2jK0|UcpxSL7>Ol4ZPK72SZvvPy*io4SW?yis3gr zEG*K~*1!_*1hDHwD9*{{!AAM{rs4iZiFt(|;+uW)a%g0>HCjNXme0&ON~l8}I~yKe z6{)JM%+HGIj<%+x1lzt->^Ez@$@1KH96l3lL*pVan8&f-OcG%X^ZEKWaQe|1A3pNtq03j zK?a;?SQRwOr|9{BaW87Dxl=;YlGpTQ{`0N3!K*a5#E!~iw0~u1D5451pTsu9@sKO( zf)kH4F3m>IsuOfGdBk+2CSE9;otc6q97vc-5(s{sQQuS|ProM$1oF4bVO*wn(nx`l zorT5wbr_y7@bXgD(t5#A*=49OBl0kYryS_;KY(|B4{g>c=P$F@qo$55hLp`>Qi4-0oDPT)UzbtsQn6aLm*p2h>)22w zJZtlI5k*^YpD4{{SfGCUa*pIF3h(jCgofSH@U*Ojg-BQ;lC4%m#nN(A7v~2Y+ z{q-yZQ{L`6iH#!B@VzB23Wnl0P5Rh5?&4SPY4dks#5&Tv+m0Gf5DUbA?$i5FkixhKy5 zJEz6ezr=S(Hy^o$8JH{?v@{`B*U!{^z1SJbt)H3-)+Kp>ZIKhs+%SzXIb7Po*h4qu8x;4pAha=BL);8UIW-w0Y7E5ax z9Nr*HI3KWp*6TYLB_N=GwM(tJ4PzCB|5r+c$AuA>ls>j0Bzo%<^CC6l(VWF=n5;7A zlRm1UU8erKIiFw0SP@^25u70KaWJp_~>S%!t1F;1-hsDPx==oXFGRV5UE?eI!3(z%?T%}TTQ~4s&B3s z6!#+nz7~G|6I$ipa}fPB;GwPay1hS7j@TQn75lmta^lNOn!3yuhx2m12frop$Q!G*UmdLTV!Hvm<-)R@y2n`CIs>a9>*j)lC z6E{D0ER-~coh#3vQ#Jq^V4dTf?Y@Exx+9pAup zY0=dT+s|LTxYM@^LL6Saj%nt4C{eqTMlR={fxCfc+GHz^6}E@F6Q(EOI2NfvE&xMB zJPJ_G0GKDlbIP*6sFkPDs2&YKFEW9XkmvSXS0Ca&+a7}IVfK3o`eb{-phGO<>eu~5}{HIivi80IE=YV08=5ZpC;Q`Oge zQe|;Lk)SZ~WA6SCVvnFK1qeGeKWcV0rX@3CKuK*WGm2;nUfF&UN{oa)TPTuzBA4#M z79z|UAt51=%nI(^r#KT3pvaki=2*%r4!}aQe+(ta1>FW(4^JFu=lhtSKYeOc6%iR} zUUfZ1mE}4*^|?UJ0>k8&vm%)e(52#WvuHoyqk2Eunn{|oVwUXxBbro;buV4=MGQ=l zjsc_UC+R&mq_~*b1OMeK$gQ5Y4znNsaoG&fae@5&&bo$}yL77Rup8K1zk$2UkW6$e zqouVq9LdYUR&rK}ps;}<-!EJ=^9-%z^XJd`D;!dGbbN?E%pN3a7r*Z$!1Py4dpgm; zih6l+$*z3Ru2Y4_Xl}h9S{TZRv<6VxFa+_voO!YmbEWa&e14+os|UUy?*4fMoPHdm z_J-d*+~?n2fjmW#%eS_5CI&((aX_>_%&SkUQHNKyZ-+RJW-g>iT#EdclshLHjTIHm#yx z{LkM?)o|O`+1{oTGpHLYP{z9Wg_{?K>d{Mca(vFK5=3>CvB+~EcnTY62#88b52)WN z)$WE%{$9$N+7;?j29+7LJ5R@$CXdf~$xJ@MTNHxtkx}fAmgeT?AnghexYw0qd@-LK z>2=7pWw&`ESf^#xk2_j5g`Z!jUe0*(#tAl%1QT~dbgKQi;NSHufhPf(wJk&3hlF%G z^7@+(1*~*31=zZP^hd70uz(Fpc90x6`+{KZ#fj;CgbHN{e=wUdm1<`)_8{Hd#QNrY zxs+~St@O+Vi^A}g+`L25oNrU*JPx2lY=nb(2sK zfSr#(R4X9;LzAXP%Es;jSWmq6S52YZC94e5cs<_I(qb1?o4d9mTr1RMqb5tR@gef+ z@4^np)&v{drBW1H^XJej!`u3sVtK}{MfV0Pl|9Zn;STP(Xp&PNe>J%o) zxm=XUzu{ZZYIbZxW8TTd$78O|1KvzAG4)uq82VZu{Hr(?_~-Eio->&5v%XSi(P28F z)z9qjPPD3BZwz*KePeMmfh>l)Ot~jJN=?RamHLeH+ zyGcjwN+@(%u+&U(1yf*gEWDH6f3{XJPqU$%omLKojg_0}v$}WBheqt>YLb&vaBVeE zHA^0AY<%|eDc_g9sz-KG(~`~8X)T@xC1G#n)l<5)K0I@;poaq0N;Tlm<8ld5UAL_L z0&VjL;mYo3AL&0uNJ&WdlgB0rUVCJw6H|72p@W}`voii7^-wI2R&uvX#wGJ`IyStP zR^aZR(ab?xl0;S~5XFsI7DlTU$E;5M+`F7%-#0egY45i=Gx@bZ?_5N6fzCM>W~MaA z^8a6TW(nPGG$~aMdAU&cdiK$`Pv2#_sU4gZjuo&H&ERc3D)9Yi>0Ipzd1aegAM?WC zGi1C$I6MI(>Tvm=<#LJ`6IXLjEah(IsO_)xEv_y9ol#-Ut*{BM+3rtYu@*_v5u%w| zh9L>rN{Xtg4pGr->{u;4fzro6(fN`-$EJ>Tjqibj=h;Pv?J>F7$8Il@yF=SiF=gk@ zuoR!(jWjQO&{IQbS4z}QAbR-PpS5ro9bRf&>~JU8Mx#0)Em=oe-`cEqopj7N$UlD?K8<9tt~BYHCeurp^D*sEh%h0_6t7$G8mCL6{9q?YGHqlxth{u ztp1*Jc1luTuViYV!rC@(bJ&IAaOVW5=9<9e>m*{%Dq=NMQHze$(q*SQ_gx+Gl)6jt zN`L0`Enefohp##hiUNsVnTff#O}9*b6W>k`51iBQ+1j8xDOT`}c~dGL%Fi%_R6uX& z#tyZ0C^)OKI!3{zqs`zAYXS$s)I7j9a`IvqA4Z+IZ&7H$><71`5vjMO9>e^}Ex1Qy zp9W|Rj+g53CXX11nR`tzH*HYfvS_7^l)5<~w1Hf_X*b&;2Yr4kuW6t;2EIFLsgt=m z4ei%cnLT2eN^%@jZ1_tF@2mFT9;<*to3TZ=IwaYT#xp4IEd_mUYkxn(XSR26Fp!?B zBeW$}%2mm7>77ho@Err&R?$@;az;s;n*&&E)02AT;!ImnISH?S0*q?lciJcW`Q#AB zx^C%7{;-lzo5!!^*PmtIbqR82#$RR?mqjXNsF*Wp>FDULXTr7j9dOv%q;z+@-tBnS zRTl^=B#`mrZ@?7~m%x=6AJVj?yWaDOcA!$?UBgHUkD`G`)i;4Eo$hp-J08~}m#eiK zo14!idlaP97!VMU)hWte&_wRK0(`hxYHL8_PUT|B_k)kf-^aj9E=9efxbz!c{6&OB zvhBIWMVHKEVhrFzfThUH{t~%23~t)Z`P+`Jo z+3+1l|1AG0m^1zO{;DUlH~tYs7O9W~-TwCeU86dKV7o=QtpTuDGp%FYP&NLj0JWS6 z^mT3C(_L=>W*p)sirD%Kx1K(BOc83VyPM_X!RIckgDk#?fy{L#-mhhghK6PmSOnqV zAM>5&OG@FK@Pk(qtP%^IeF1N{zC4w1e3GXroxPo()bs$01|cSHmg7$-uA$*ZofJq} z0ZeG+ORh3VM8FEO&Dl=*b^VYs^BlNAAnwNyG%*pr_|)AQZOT^wt0a7Tcv(e76t(e0 zZ0ISVV-i#Sjg1r)Znb+##uwrh=y==a+Dijvsi|ph*EHn5P@TEQ=sB019?1%a0b{=CvV)ikq0{PG@yeZ6nnVDB$zC&jAQO8 z`$fbmITv+v{P@D-_4lioRLZLd<=E!XMD3|!D7rtNqm6Cv!irCo1Ks*WTfIa$2lSlrAHg1_uQZLYE3NSCSXvZ4G@>fr86mNuPu{% zz=!|qGpG)dKU*A;0xTG@kwg+{pbM1Wa8lUn4!40Cmq@r8*Nb}Dt9K$rc42*y!Uht2F8Ds$Z@;3bvA?t3C8^aWTcB)Q$G z!uxRz>0K#WxhFH!T={V0XrjD3?={Y2iX;vT0&w=cb0Pz<_X?DP{QJWR zZiyS)jx%?hw==B-yaU;cuHXqQsDkVEmp^P>19aAqi`YNsQTPD`@ zC#U$>cl{Xa+-sD5-*rbaMPE0->Gb_3t){%RmD%u(vRK>LlOpAu zbw%XUMnw1EYwn|M#Qz>V{5xXb^J3SE7IzJ}qO1hcIG%m3jx?1fi~F@?)s^LjLi0%x z?Vn9pTy+FmRYkOQs8ZQi<>BeMFXC_{H!4_}m9wgr_GV#tan)Y%N@zyEe}=h4aAZQ^ zwSa<%`wEA|ddAi_;>;UTiX?VMOguH*I(Sg|R>{6K6bdV;M<6)ldEoUULYT7v;!^%)J-?)O2 z<-idC@rxac-XyL=}{Mr$#;`V<{Q?0%x6#{JY_}$ z21FO$~RondUw1y=8Hb}* zv0Oihi91RR2xmjTehQy0%0k=P5GAm|2cXZ&{SzOWCq_&ND7)`ieG&w*2h-OaDcz!_ zA#Q0FNuydbIR3b}xL&s3DAu}f9y+A{>D~+W`@%X(yIp7L3=g=n;j7>;D2La>+N*J~NET6*jTnyBb-4vK=gr z%@^iT2-tVl=1`ED-4>ww6Kt3S0u7bAVn%@r3$0xeTsVRx!GEIa`&E4CdBZal;WE`P zAmR=mCEY)uVPThWL(56|#To}z3RdF!!ZC(FkHp0>qmrY1Ki2q!s62(=y>OcS3MNEUcl`c`b-#>#-@zjPS|APy~$IT5Y+^ur=(zE-iCXOu!eOZ)p z*sg@ANLk|n6+6Cz@}Ez4J65gmS}@v2iQPyUh9xNE(Lb>?K+T3P<8F4Q@{fg-#bq<> z@!v0^9*lhX_(<(Xo(32&wqjQMze5S@e-3rYcRZ`{)|?kvoKW{Gy#s&83hpgbHn%E$ zZtZNt+8C_TWBK!+u-eFXTbiW8{8|mYz)aM)UhzuN{`1`go~y(!Ef|ML>Ca1CI1D>-=|<|gzkiMt%~%4`jaylo x?}AyS`hH3o>`4P=1w21i(vxl69}=qf2)h-kPCnG)UnYQmigK#5Su#dm{|_U5$2R}~ diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 4074cbca63..aab17febbe 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -20,6 +20,9 @@ For details see the controller_manager section. * Pass URDF to controllers on init (`#1088 `_). * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * A method to get node options to setup the controller node #api-breaking (`#1169 `_) +* Export state interfaces from the chainable controller #api-breaking (`#1021 `_) + + * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. controller_manager ****************** From 2969460d9962c7b2497155071cf08946d11d6800 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 13 May 2024 17:44:59 +0200 Subject: [PATCH 57/61] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- controller_manager/doc/controller_chaining.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index 82c66bb3fc..7eadf602ae 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -58,11 +58,11 @@ A Controller Base-Class: ChainableController ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual std::vector export_reference_interfaces() = 0`` method as well as ``virtual std::vector export_state_interfaces() = 0`` method. -This method should implement for each controller that **can be preceded** by another controller exporting all the reference/state interfaces. -For simplicity reasons, it is assumed for now that controller's all reference interfaces are used by other controller. However, the state interfaces exported by the controller, can be used by multieple controllers at the same time and with the combination they want. +This method should be implemented for each controller that **can be preceded** by another controller exporting all the reference/state interfaces. +For simplicity reasons, it is assumed for now that controller's all reference interfaces are used by other controller. However, the state interfaces exported by the controller, can be used by multiple controllers at the same time and with the combination they want. Therefore, do not try to implement any exclusive combinations of reference interfaces, but rather write multiple controllers if you need exclusivity. -The ``ChainableController`` base class implements ``void set_chained_mode(bool activate)`` that sets an internal flag that a controller is used by another controller (in chained mode) and calls ``virtual void on_set_chained_mode(bool activate) = 0`` that implements controller's specific actions when chained modes is activated or deactivated, e.g., deactivating subscribers. +The ``ChainableController`` base class implements ``void set_chained_mode(bool activate)`` that sets an internal flag that a controller is used by another controller (in chained mode) and calls ``virtual void on_set_chained_mode(bool activate) = 0`` that implements controller's specific actions when chained mode is activated or deactivated, e.g., deactivating subscribers. As an example, PID controllers export one virtual interface ``pid_reference`` and stop their subscriber ``/pid_reference`` when used in chained mode. 'diff_drive_controller' controller exports list of virtual interfaces ``/v_x``, ``/v_y``, and ``/w_z``, and stops subscribers from topics ``/cmd_vel`` and ``/cmd_vel_unstamped``. Its publishers can continue running. From b01722ca9818969c248b30fc760387479a2533a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 13 May 2024 18:09:24 +0200 Subject: [PATCH 58/61] Add more documentation on different interfaces --- controller_manager/doc/controller_chaining.rst | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index 7eadf602ae..1103a7ae5a 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -66,6 +66,21 @@ The ``ChainableController`` base class implements ``void set_chained_mode(bool a As an example, PID controllers export one virtual interface ``pid_reference`` and stop their subscriber ``/pid_reference`` when used in chained mode. 'diff_drive_controller' controller exports list of virtual interfaces ``/v_x``, ``/v_y``, and ``/w_z``, and stops subscribers from topics ``/cmd_vel`` and ``/cmd_vel_unstamped``. Its publishers can continue running. +Nomenclature +^^^^^^^^^^^^^^ + +There are two types of interfaces within the context of ``ros2_control``: ``CommandInterface`` and ``StateInterface``. + +- The ``CommandInterface`` is a Read-Write type of interface that can be used to get and set values. Its typical use-case is to set command values to the hardware. +- The ``StateInterface`` is a Read-Only type of interface that can be used to get values. Its typical use-case is to get actual state values from the hardware. + +These interfaces are utilized in different places within the controller in order to have a functional controller or controller chain that commands the hardware. + +- The ``virtual InterfaceConfiguration command_interface_configuration() const`` method defined in the ``ControllerInterface`` class is used to define the command interfaces used by the controller. These interfaces are used to command the hardware or the exposed reference interfaces from another controller. The ``controller_manager`` uses this configuration to claim the command interfaces from the ``ResourceManager``. +- The ``virtual InterfaceConfiguration state_interface_configuration() const`` method defined in the ``ControllerInterface`` class is used to define the state interfaces used by the controller. These interfaces are used to get the actual state values from the hardware or the exposed state interfaces from another controller. The ``controller_manager`` uses this configuration to claim the state interfaces from the ``ResourceManager``. +- The ``std::vector export_reference_interfaces()`` method defined in the ``ChainableController`` class is used to define the reference interfaces exposed by the controller. These interfaces are used to command the controller from other controllers. +- The ``std::vector export_state_interfaces()`` method defined in the ``ChainableController`` class is used to define the state interfaces exposed by the controller. These interfaces are used to get the actual state values from the controller by other controllers. + Inner Resource Management ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From e64558b22312144e107560873bde4d5735cba567 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 21 May 2024 09:16:35 +0200 Subject: [PATCH 59/61] update the docs of set_chained_mode method Co-authored-by: Dr. Denis --- .../include/controller_interface/controller_interface_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index f67fb23fa0..1b5fd2e059 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -236,7 +236,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode - * usually involves the usage of the controller's reference or state interfaces by the other + * usually involves the usage of the controller's reference interfaces by the other * controllers * * \returns true if mode is switched successfully and false if not. From fa2340c3102dc84961ea7a9f0b418780b6c18021 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 14 Jun 2024 17:52:10 +0200 Subject: [PATCH 60/61] Add default implementation to the on_export_state_interfaces and on_export_reference_interfaces --- .../chainable_controller_interface.hpp | 7 +++-- .../src/chainable_controller_interface.cpp | 26 +++++++++++++++++++ 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index b62cee3ee8..2e39f038b1 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,7 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -77,7 +78,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \returns list of StateInterfaces that other controller can use as their inputs. */ - virtual std::vector on_export_state_interfaces() = 0; + virtual std::vector on_export_state_interfaces(); /// Virtual method that each chainable controller should implement to export its read/write /// chainable interfaces. @@ -88,7 +89,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \returns list of CommandInterfaces that other controller can use as their outputs. */ - virtual std::vector on_export_reference_interfaces() = 0; + virtual std::vector on_export_reference_interfaces(); /// Virtual method that each chainable controller should implement to switch chained mode. /** @@ -129,9 +130,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase const rclcpp::Time & time, const rclcpp::Duration & period) = 0; /// Storage of values for state interfaces + std::vector exported_state_interface_names_; std::vector state_interfaces_values_; /// Storage of values for reference interfaces + std::vector exported_reference_interface_names_; std::vector reference_interfaces_; private: diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 803a1e67fc..4bf9fe0101 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -149,4 +149,30 @@ bool ChainableControllerInterface::is_in_chained_mode() const { return in_chaine bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } +std::vector +ChainableControllerInterface::on_export_state_interfaces() +{ + state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); + std::vector state_interfaces; + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); + } + return state_interfaces; +} + +std::vector +ChainableControllerInterface::on_export_reference_interfaces() +{ + reference_interfaces_.resize(exported_reference_interface_names_.size(), 0.0); + std::vector reference_interfaces; + for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i) + { + reference_interfaces.emplace_back(hardware_interface::CommandInterface( + get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i])); + } + return reference_interfaces; +} + } // namespace controller_interface From 4f97b54fb8f6701e9c1acbd38b7f75ba6520ea93 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 20 Jun 2024 10:51:01 +0200 Subject: [PATCH 61/61] remove one of the redundant checks with the new approach --- .../src/chainable_controller_interface.cpp | 27 ------------------- .../test_chainable_controller_interface.cpp | 21 --------------- 2 files changed, 48 deletions(-) diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 4bf9fe0101..9f4a171ec3 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -48,18 +48,6 @@ std::vector ChainableControllerInterface::export_state_interfaces() { auto state_interfaces = on_export_state_interfaces(); - // check if the "state_interfaces_values_" variable is resized to number of interfaces - if (state_interfaces_values_.size() != state_interfaces.size()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "The internal storage for exported state values 'state_interfaces_values_' variable has size " - "'%zu', but it is expected to have the size '%zu' equal to the number of exported state " - "interfaces. No state interface will be exported. Please correct and recompile " - "the controller with name '%s' and try again.", - state_interfaces_values_.size(), state_interfaces.size(), get_node()->get_name()); - state_interfaces.clear(); - } // check if the names of the controller state interfaces begin with the controller's name for (const auto & interface : state_interfaces) @@ -85,21 +73,6 @@ ChainableControllerInterface::export_reference_interfaces() { auto reference_interfaces = on_export_reference_interfaces(); - // check if the "reference_interfaces_" variable is resized to number of interfaces - if (reference_interfaces_.size() != reference_interfaces.size()) - { - // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the - // framework - RCLCPP_FATAL( - get_node()->get_logger(), - "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', " - "but it is expected to have the size '%zu' equal to the number of exported reference " - "interfaces. No reference interface will be exported. Please correct and recompile " - "the controller with name '%s' and try again.", - reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name()); - reference_interfaces.clear(); - } - // check if the names of the reference interfaces begin with the controller's name for (const auto & interface : reference_interfaces) { diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 7380b222e1..2f04273f3e 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -74,27 +74,6 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); } -TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) -{ - TestableChainableControllerInterface controller; - - // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::OK); - ASSERT_NO_THROW(controller.get_node()); - - // expect empty return because storage is not resized - controller.reference_interfaces_.clear(); - auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_THAT(reference_interfaces, IsEmpty()); - // expect empty return because storage is not resized - controller.state_interfaces_values_.clear(); - auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_THAT(exported_state_interfaces, IsEmpty()); -} - TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) { TestableChainableControllerInterface controller;