diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index b62cee3ee8..2e39f038b1 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,7 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -77,7 +78,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \returns list of StateInterfaces that other controller can use as their inputs. */ - virtual std::vector on_export_state_interfaces() = 0; + virtual std::vector on_export_state_interfaces(); /// Virtual method that each chainable controller should implement to export its read/write /// chainable interfaces. @@ -88,7 +89,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \returns list of CommandInterfaces that other controller can use as their outputs. */ - virtual std::vector on_export_reference_interfaces() = 0; + virtual std::vector on_export_reference_interfaces(); /// Virtual method that each chainable controller should implement to switch chained mode. /** @@ -129,9 +130,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase const rclcpp::Time & time, const rclcpp::Duration & period) = 0; /// Storage of values for state interfaces + std::vector exported_state_interface_names_; std::vector state_interfaces_values_; /// Storage of values for reference interfaces + std::vector exported_reference_interface_names_; std::vector reference_interfaces_; private: diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 803a1e67fc..4bf9fe0101 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -149,4 +149,30 @@ bool ChainableControllerInterface::is_in_chained_mode() const { return in_chaine bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } +std::vector +ChainableControllerInterface::on_export_state_interfaces() +{ + state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); + std::vector state_interfaces; + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); + } + return state_interfaces; +} + +std::vector +ChainableControllerInterface::on_export_reference_interfaces() +{ + reference_interfaces_.resize(exported_reference_interface_names_.size(), 0.0); + std::vector reference_interfaces; + for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i) + { + reference_interfaces.emplace_back(hardware_interface::CommandInterface( + get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i])); + } + return reference_interfaces; +} + } // namespace controller_interface