diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2bdccefdc5..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" @@ -55,6 +56,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,8 +69,19 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_in_chained_mode() const final; protected: - /// Virtual method that each chainable controller should implement to export its 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_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_state_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 @@ -74,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. /** @@ -114,7 +129,12 @@ 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 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/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index a3d006755f..17f39c8478 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; + /** + * A non-chainable controller doesn't export any 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 1c09f71e98..1b5fd2e059 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -224,11 +224,20 @@ 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 - * 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 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 2f7c67741e..9f4a171ec3 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,26 +44,35 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector -ChainableControllerInterface::export_reference_interfaces() +std::vector +ChainableControllerInterface::export_state_interfaces() { - auto reference_interfaces = on_export_reference_interfaces(); + auto state_interfaces = on_export_state_interfaces(); - // check if the "reference_interfaces_" variable is resized to number of interfaces - if (reference_interfaces_.size() != reference_interfaces.size()) + // check if the names of the controller state interfaces begin with the controller's name + for (const auto & interface : state_interfaces) { - // 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(); + 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() +{ + auto reference_interfaces = on_export_reference_interfaces(); + // check if the names of the reference interfaces begin with the controller's name for (const auto & interface : reference_interfaces) { @@ -113,4 +122,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 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.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 47487e019c..2f04273f3e 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; @@ -31,7 +34,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) EXPECT_FALSE(controller.is_in_chained_mode()); } -TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) +TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) { TestableChainableControllerInterface controller; @@ -42,16 +45,16 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - auto reference_interfaces = controller.export_reference_interfaces(); + auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_EQ(reference_interfaces.size(), 1u); - EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); + 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(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); } -TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) +TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) { TestableChainableControllerInterface controller; @@ -62,13 +65,16 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc 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_TRUE(reference_interfaces.empty()); + + 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"); + + EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); } -TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node_name) +TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) { TestableChainableControllerInterface controller; @@ -83,7 +89,10 @@ 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()); + ASSERT_THAT(reference_interfaces, IsEmpty()); + // expect empty return because interface prefix is not equal to the node name + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_state_interfaces, IsEmpty()); } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -98,12 +107,15 @@ 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 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(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -116,6 +128,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); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -147,6 +160,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); + 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 @@ -154,6 +168,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.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); @@ -161,6 +176,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.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); @@ -168,6 +184,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.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); controller.reference_interfaces_[0] = 0.0; @@ -181,6 +198,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.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); @@ -189,4 +208,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.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 28401f13d5..f9684c27fd 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -29,24 +29,28 @@ 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; 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() { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); + state_interfaces_values_.reserve(1); + state_interfaces_values_.push_back(EXPORTED_STATE_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; } @@ -63,13 +67,24 @@ 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_interfaces_, "test_state", &state_interfaces_values_[0])); + + return state_interfaces; + } + // Implementation of ChainableController virtual methods std::vector on_export_reference_interfaces() override { 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; } @@ -78,6 +93,7 @@ class TestableChainableControllerInterface { if (reference_interfaces_[0] == 0.0) { + state_interfaces_values_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; return true; } else @@ -107,13 +123,14 @@ class TestableChainableControllerInterface } reference_interfaces_[0] -= 1; + state_interfaces_values_[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) @@ -121,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; }; diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index c83ed97ac0..1103a7ae5a 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,19 +57,34 @@ 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 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. +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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -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 +96,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 +111,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 1ba49a116e..c21ab88d72 100644 Binary files a/controller_manager/doc/images/chaining_example2.png and b/controller_manager/doc/images/chaining_example2.png differ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 845fa2dee0..cebb85cb68 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -556,6 +556,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 3b84fc07e4..3e624e415a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -83,11 +83,11 @@ 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" - * 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 + * "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. @@ -95,7 +95,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) @@ -125,8 +125,8 @@ 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.", - 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; } @@ -169,7 +169,6 @@ void controller_chain_spec_cleanup( } ctrl_chain_spec.erase(controller); } - } // namespace namespace controller_manager @@ -740,16 +739,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 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); + resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); + resource_manager_->import_controller_exported_state_interfaces( + controller_name, state_interfaces); } // let's update the list of following and preceding controllers @@ -758,24 +761,28 @@ 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); 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 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); 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); } } @@ -835,6 +842,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_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); @@ -1064,6 +1072,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( @@ -1375,6 +1391,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); @@ -1420,6 +1438,7 @@ void ControllerManager::deactivate_controllers( // deactivation if (controller->is_chainable()) { + 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) @@ -1628,6 +1647,8 @@ void ControllerManager::activate_controllers( // if it is a chainable controller, make the reference interfaces available on activation if (controller->is_chainable()) { + // make all the exported interfaces of the controller available + resource_manager_->make_controller_exported_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } @@ -1722,13 +1743,23 @@ 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( + controllers[i].info.name); controller_state.reference_interfaces.reserve(references.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 & state_interface : exported_state_interfaces) + { + const std::string prefix_name = controllers[i].c->get_node()->get_name(); + const std::string interface_name = state_interface.substr(prefix_name.size() + 1); + controller_state.exported_state_interfaces.push_back(interface_name); + } } } response->controller.push_back(controller_state); @@ -2354,12 +2385,16 @@ 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 (command_interface_is_reference_interface_of_controller( - 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) @@ -2388,12 +2423,21 @@ 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) + 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 (!command_interface_is_reference_interface_of_controller( - cmd_itf_name, controllers, following_ctrl_it)) + if (!is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { continue; } @@ -2413,9 +2457,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 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; } @@ -2468,14 +2512,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) - resource_manager_->make_controller_reference_interfaces_available( + // make all the exported interfaces of the controller available + resource_manager_->make_controller_exported_state_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 @@ -2508,79 +2561,63 @@ 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 & ref_itf_name : - resource_manager_->get_controller_reference_interface_names(controller_it->info.name)) + 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) { - std::vector preceding_controllers_using_ref_itf; + 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)); - // 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 (found_it != controllers.end()) { - const auto preceding_ctrl_cmd_itfs = - preceding_ctrl_it->c->command_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()) - { - 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) != + 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' will be activated. ", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + "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; } - // 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) == + if ( + is_controller_active(found_it->c) && + std::find(deactivate_request_.begin(), deactivate_request_.end(), preceeding_controller) == 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()); + 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); - // } } } + + // 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; } 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..e43f2a13a1 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 @@ -96,6 +103,20 @@ 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(); + ++i) + { + 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 < exported_state_interface_names_.size() && i < state_interfaces_.size() && + command_interfaces_.empty(); + ++i) + { + state_interfaces_values_[i] = state_interfaces_[i].get_value(); + } return controller_interface::return_type::OK; } @@ -150,6 +171,20 @@ CallbackReturn TestChainableController::on_cleanup( return CallbackReturn::SUCCESS; } +std::vector +TestChainableController::on_export_state_interfaces() +{ + std::vector state_interfaces; + + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + { + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); + } + + return state_interfaces; +} + std::vector TestChainableController::on_export_reference_interfaces() { @@ -184,6 +219,31 @@ 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) +{ + exported_state_interface_names_ = state_interface_names; + + state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); +} + +void TestChainableController::set_imu_sensor_name(const std::string & name) +{ + if (!name.empty()) + { + 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 5925ed8d11..f4f59ad9df 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 @@ -35,6 +36,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: @@ -60,6 +62,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; @@ -80,10 +85,21 @@ 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_exported_state_interface_names(const std::vector & state_interface_names); + + 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_; std::vector reference_interface_names_; + std::vector exported_state_interface_names_; + std::unique_ptr imu_sensor_; 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 8f120bbd47..625d7ed90f 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -128,6 +128,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 368aeae4a8..bf183c7bad 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; diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 24cd30bc06..4fdfe3fb9f 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -980,34 +980,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); @@ -1233,40 +1236,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( 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..cd2485e60e 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,10 @@ 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(); // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { @@ -135,7 +139,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 +158,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"}); - // 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_exported_state_interface_names({"odom_x", "odom_y"}); // configure Position Tracking controller controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { @@ -168,16 +174,51 @@ 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 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"}}; + odom_publisher_controller->set_state_interface_configuration(odom_and_fusion_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_exported_state_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); + 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(5u, 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()); 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()); + EXPECT_EQ(2, position_tracking_controller_two.use_count()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, @@ -194,6 +235,18 @@ 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()); + 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 @@ -201,6 +254,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 +262,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 +275,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 +284,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 +300,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 +320,83 @@ 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(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 + 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 + 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 < 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 & exported_state_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 (!exported_state_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 (!exported_state_interfaces.empty()) + { + // 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)); + } + } } } @@ -303,11 +422,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)); } @@ -351,10 +472,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); @@ -382,10 +504,13 @@ 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(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( @@ -401,12 +526,25 @@ 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_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->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); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); + 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); 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_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); @@ -415,10 +553,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_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 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 +572,10 @@ 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"; + static constexpr char POSITION_TRACKING_CONTROLLER_TWO[] = "position_tracking_controller_two"; const std::vector PID_LEFT_WHEEL_REFERENCE_INTERFACES = { "pid_left_wheel_controller/velocity"}; @@ -434,6 +583,11 @@ 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 = { + "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"}; @@ -447,7 +601,11 @@ 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; + std::shared_ptr position_tracking_controller_two; testing::WithParamInterface::ParamType test_param; @@ -458,6 +616,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_STATE_ODOM_X = 0.0; + double EXP_STATE_ODOM_Y = 0.0; // Expected behaviors struct used in chaining activation/deactivation tests struct ExpectedBehaviorStruct @@ -492,17 +652,32 @@ 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( + 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); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + 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_STATE_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( @@ -512,17 +687,30 @@ 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(sensor_fusion_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 +718,30 @@ 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); + + // 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); + 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 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); + 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 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"}) { @@ -537,9 +749,13 @@ 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(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}; @@ -552,25 +768,43 @@ 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, 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 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 + 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_STATE_ODOM_X = + chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 / dt + 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_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_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)); - 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)); @@ -582,6 +816,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_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); @@ -589,8 +826,14 @@ 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(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); + // The state doesn't change wrt to any data from the hardware calculation + 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 @@ -624,6 +867,18 @@ 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( + 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); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -646,6 +901,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( @@ -653,6 +909,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( @@ -661,35 +918,153 @@ 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 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 exported 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 + 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_FALSE(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 exported 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()); + 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 + 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( + 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( + 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_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_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_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_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, 8u, true); + 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( @@ -713,6 +1088,18 @@ 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( + 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); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -726,6 +1113,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. @@ -746,6 +1134,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) @@ -759,11 +1158,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 @@ -775,6 +1174,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( @@ -802,6 +1206,18 @@ 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( + 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); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -818,17 +1234,69 @@ 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); + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 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()); + 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()); + + // 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}, test_param.strictness, - std::future_status::timeout, controller_interface::return_type::OK); + {DIFF_DRIVE_CONTROLLER_TWO}, + {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 EXPECT_EQ( @@ -841,6 +1309,55 @@ 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()); + 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_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()); + 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( @@ -864,6 +1381,18 @@ 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( + 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); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); CheckIfControllersAreAddedCorrectly(); @@ -877,6 +1406,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( @@ -899,11 +1429,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 @@ -913,6 +1460,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 @@ -979,6 +1534,261 @@ 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); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + 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_FALSE(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/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 + // the other controllers that rely on the DiffDriveController expected interfaces + DeactivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + 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_FALSE(diff_drive_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 + // 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_FALSE(diff_drive_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( + 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_FALSE(diff_drive_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( + 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()); + 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_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()); + 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_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, 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()); + + // 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(); @@ -987,6 +1797,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); @@ -999,17 +1812,27 @@ 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); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + 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_STATE_INTERFACES, + DIFF_DRIVE_REFERENCE_INTERFACES); EXPECT_THROW( cm_->resource_manager_->make_controller_reference_interfaces_available( @@ -1037,6 +1860,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"}) { @@ -1044,13 +1870,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_) { @@ -1059,14 +1889,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 @@ -1075,9 +1905,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)); diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index 3b8e032541..65d773bddc 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[] 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/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 ****************** diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index c557a4970f..d2f480e2f3 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -86,6 +86,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/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 31def4598b..654e472e23 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -134,6 +134,59 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + /// Add controllers' exported 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. + * + * \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( + const std::string & controller_name, std::vector & interfaces); + + /// Get list of exported tate interface of 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_exported_state_interface_names( + const std::string & controller_name); + + /// 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 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_exported_state_interfaces_available(const std::string & controller_name); + + /// 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 + * another controller anymore. + * + * \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); + + /// Remove controllers exported state interfaces from resource manager. + /** + * 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_exported_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 d77f915eee..f0ecb0008b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -656,6 +656,47 @@ class ResourceStorage } } + /// 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 @@ -938,7 +979,9 @@ 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_exported_state_interfaces_map_; std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -1075,6 +1118,68 @@ 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_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_exported_state_interfaces_map_[controller_name] = interface_names; +} + +// CM API: Called in "callback/slow"-thread +std::vector ResourceManager::get_controller_exported_state_interface_names( + const std::string & controller_name) +{ + return resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); +} + +// CM API: Called in "update"-thread +void ResourceManager::make_controller_exported_state_interfaces_available( + const std::string & controller_name) +{ + auto interface_names = + 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(), + interface_names.end()); +} + +// CM API: Called in "update"-thread +void ResourceManager::make_controller_exported_state_interfaces_unavailable( + const std::string & controller_name) +{ + auto interface_names = + resource_storage_->controllers_exported_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_exported_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); + + 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) 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..9997466106 --- /dev/null +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -0,0 +1,142 @@ +// 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) 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 31e630059b..7f9b467e78 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 @@ -932,6 +932,23 @@ const auto diffbot_urdf = + + + test_hardware_components/TestIMUSensor + + + + + + + + + + + + + + )";