diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2e39f038b1..912e224c74 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,7 +15,9 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include #include +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -57,10 +59,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector export_reference_interfaces() final; CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; @@ -131,11 +133,21 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// Storage of values for state interfaces std::vector exported_state_interface_names_; + std::vector ordered_exported_state_interfaces_; + std::unordered_map + exported_state_interfaces_; + // BEGIN (Handle export change): for backward compatibility std::vector state_interfaces_values_; + // END /// Storage of values for reference interfaces std::vector exported_reference_interface_names_; + // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; + // END + std::vector ordered_reference_interfaces_; + std::unordered_map + reference_interfaces_ptrs_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 43fd269803..5c321b0e42 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -47,7 +47,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_state_interfaces() final; /** * Controller has no reference interfaces. @@ -55,7 +55,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector export_reference_interfaces() final; /** * Controller is not chainable, therefore no chained mode can be set. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 74077969d3..1f350abeef 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -235,7 +235,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of command interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_reference_interfaces() = 0; + virtual std::vector + export_reference_interfaces() = 0; /** * Export interfaces for a chainable controller that can be used as state interface by other @@ -244,7 +245,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_state_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index ae5cd326b6..ce19eff9b9 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,53 +44,134 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector +std::vector ChainableControllerInterface::export_state_interfaces() { auto state_interfaces = on_export_state_interfaces(); + std::vector state_interfaces_ptrs_vec; + state_interfaces_ptrs_vec.reserve(state_interfaces.size()); + ordered_exported_state_interfaces_.reserve(state_interfaces.size()); + exported_state_interface_names_.reserve(state_interfaces.size()); // check if the names of the controller state interfaces begin with the controller's name for (const auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory for state interfaces. No state interface will be exported. Please " - "correct and recompile the controller with name '%s' and try again.", - interface.get_name().c_str(), get_node()->get_name()); - state_interfaces.clear(); - break; + std::string error_msg = + "The prefix of the interface '" + interface.get_prefix_name() + + "' does not equal the controller's name '" + get_node()->get_name() + + "'. This is mandatory for state interfaces. No state interface will be exported. Please " + "correct and recompile the controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + auto state_interface = std::make_shared(interface); + const auto interface_name = state_interface->get_name(); + auto [it, succ] = exported_state_interfaces_.insert({interface_name, state_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert StateInterface<" + interface_name + + "> into exported_state_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + exported_state_interfaces_.clear(); + exported_state_interface_names_.clear(); + state_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); } + ordered_exported_state_interfaces_.push_back(state_interface); + exported_state_interface_names_.push_back(interface_name); + state_interfaces_ptrs_vec.push_back(state_interface); } - return state_interfaces; + return state_interfaces_ptrs_vec; } -std::vector +std::vector ChainableControllerInterface::export_reference_interfaces() { auto reference_interfaces = on_export_reference_interfaces(); + std::vector reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + exported_reference_interface_names_.reserve(reference_interfaces.size()); + ordered_reference_interfaces_.reserve(reference_interfaces.size()); + + // BEGIN (Handle export change): for backward compatibility + // check if the "reference_interfaces_" variable is resized to number of interfaces + if (reference_interfaces_.size() != reference_interfaces.size()) + { + std::string error_msg = + "The internal storage for reference values 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + // END // check if the names of the reference interfaces begin with the controller's name - for (const auto & interface : reference_interfaces) + const auto ref_interface_size = reference_interfaces.size(); + for (auto & interface : reference_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory " - " for reference interfaces. No reference 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()); - reference_interfaces.clear(); - break; + std::string error_msg = "The name of the interface " + interface.get_name() + + " does not begin with the controller's name. This is mandatory for " + "reference interfaces. Please " + "correct and recompile the controller with name " + + get_node()->get_name() + " and try again."; + throw std::runtime_error(error_msg); } + + hardware_interface::CommandInterface::SharedPtr reference_interface = + std::make_shared(std::move(interface)); + const auto inteface_name = reference_interface->get_name(); + // check the exported interface name is unique + auto [it, succ] = reference_interfaces_ptrs_.insert({inteface_name, reference_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert Reference interface<" + inteface_name + + "> into reference_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + reference_interfaces_.clear(); + exported_reference_interface_names_.clear(); + reference_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); + } + ordered_reference_interfaces_.push_back(reference_interface); + exported_reference_interface_names_.push_back(inteface_name); + reference_interfaces_ptrs_vec.push_back(reference_interface); } - return reference_interfaces; + if (reference_interfaces_ptrs_.size() != ref_interface_size) + { + std::string error_msg = + "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + + std::to_string(reference_interfaces_ptrs_.size()) + + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + + return reference_interfaces_ptrs_vec; } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) @@ -130,8 +211,8 @@ ChainableControllerInterface::on_export_state_interfaces() 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])); + state_interfaces.emplace_back( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]); } return state_interfaces; } diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index a039501aa1..4a22d78666 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -22,12 +22,14 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_state_interfaces() +std::vector +ControllerInterface::export_state_interfaces() { return {}; } -std::vector ControllerInterface::export_reference_interfaces() +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 2f04273f3e..5233fe0278 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -15,6 +15,7 @@ #include "test_chainable_controller_interface.hpp" #include +#include using ::testing::IsEmpty; using ::testing::SizeIs; @@ -48,10 +49,10 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) auto exported_state_interfaces = controller.export_state_interfaces(); 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(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -68,10 +69,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); 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_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -88,10 +89,15 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); // expect empty return because interface prefix is not equal to the node name - auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_THAT(reference_interfaces, IsEmpty()); + std::vector exported_reference_interfaces; + EXPECT_THROW( + { exported_reference_interfaces = controller.export_reference_interfaces(); }, + std::runtime_error); + ASSERT_THAT(exported_reference_interfaces, IsEmpty()); // expect empty return because interface prefix is not equal to the node name - auto exported_state_interfaces = controller.export_state_interfaces(); + std::vector exported_state_interfaces; + EXPECT_THROW( + { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error); ASSERT_THAT(exported_state_interfaces, IsEmpty()); } @@ -114,8 +120,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -124,11 +129,11 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Success setting chained mode - reference_interfaces[0].set_value(0.0); + reference_interfaces[0]->set_value(0.0); EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b4f18dd90e..bad55e2e1c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -771,15 +771,28 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto state_interfaces = controller->export_state_interfaces(); - auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && state_interfaces.empty()) + std::vector state_interfaces; + std::vector ref_interfaces; + try { - // TODO(destogl): Add test for this! - RCLCPP_ERROR( - get_logger(), - "Controller '%s' is chainable, but does not export any state or reference interfaces.", - controller_name.c_str()); + state_interfaces = controller->export_state_interfaces(); + 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. Did you " + "override the on_export_method() correctly?", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::runtime_error & e) + { + RCLCPP_FATAL( + get_logger(), "Creation of the reference interfaces failed with following error: %s", + e.what()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..aa306870a1 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -28,16 +28,23 @@ class LoanedCommandInterface public: using Deleter = std::function; - explicit LoanedCommandInterface(CommandInterface & command_interface) + [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedCommandInterface( + CommandInterface & command_interface) : LoanedCommandInterface(command_interface, nullptr) { } - LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) + [[deprecated("Replaced by the new version using shared_ptr")]] LoanedCommandInterface( + CommandInterface & command_interface, Deleter && deleter) : command_interface_(command_interface), deleter_(std::forward(deleter)) { } + LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter) + : command_interface_(*command_interface), deleter_(std::forward(deleter)) + { + } + LoanedCommandInterface(const LoanedCommandInterface & other) = delete; LoanedCommandInterface(LoanedCommandInterface && other) = default; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..8fcc5bdb0b 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -28,16 +28,28 @@ class LoanedStateInterface public: using Deleter = std::function; - explicit LoanedStateInterface(StateInterface & state_interface) + [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedStateInterface( + StateInterface & state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) + [[deprecated("Replaced by the new version using shared_ptr")]] LoanedStateInterface( + StateInterface & state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } + explicit LoanedStateInterface(StateInterface::SharedPtr state_interface) + : LoanedStateInterface(state_interface, nullptr) + { + } + + LoanedStateInterface(StateInterface::SharedPtr state_interface, Deleter && deleter) + : state_interface_(*state_interface), deleter_(std::forward(deleter)) + { + } + LoanedStateInterface(const LoanedStateInterface & other) = delete; LoanedStateInterface(LoanedStateInterface && other) = default; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 8f9f142c25..ece45e3146 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -141,7 +141,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \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); + const std::string & controller_name, std::vector & interfaces); /// Get list of exported tate interface of a controller. /** @@ -195,7 +195,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void import_controller_reference_interfaces( const std::string & controller_name, - std::vector & interfaces); + const std::vector & interfaces); /// Get list of reference interface of a controller. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index fe5e9ae904..4d0c41b242 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -656,10 +656,10 @@ class ResourceStorage { try { - std::vector interfaces = hardware.export_command_interfaces(); - + auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + // TODO(Manuel) END: for backward compatibility } catch (const std::exception & ex) { @@ -1207,7 +1207,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & } std::lock_guard guard(resource_interfaces_lock_); - return LoanedStateInterface(*(resource_storage_->state_interface_map_.at(key))); + return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); } // CM API: Called in "callback/slow"-thread @@ -1241,16 +1241,10 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto & interface : interfaces) - { - interface_ptrs.push_back(std::make_shared(interface)); - } - auto interface_names = resource_storage_->add_state_interfaces(interface_ptrs); + auto interface_names = resource_storage_->add_state_interfaces(interfaces); resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; } @@ -1310,16 +1304,10 @@ void ResourceManager::remove_controller_exported_state_interfaces( // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, - std::vector & interfaces) + const std::vector & interfaces) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto & interface : interfaces) - { - interface_ptrs.push_back(std::make_shared(std::move(interface))); - } - auto interface_names = resource_storage_->add_command_interfaces(interface_ptrs); + auto interface_names = resource_storage_->add_command_interfaces(interfaces); resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } @@ -1452,7 +1440,7 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin resource_storage_->claimed_command_interface_map_[key] = true; std::lock_guard guard(resource_interfaces_lock_); return LoanedCommandInterface( - *(resource_storage_->command_interface_map_.at(key)), + resource_storage_->command_interface_map_.at(key), std::bind(&ResourceManager::release_command_interface, this, key)); } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 51d81a90ab..e72a4a8214 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1230,12 +1230,12 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; - std::vector reference_interfaces; + std::vector reference_interfaces; std::vector reference_interface_values = {1.0, 2.0, 3.0}; for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( + reference_interfaces.push_back(std::make_shared( CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); }