diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 912e224c74..e4e4ec662c 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -59,7 +59,7 @@ 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; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 5c321b0e42..3455227e1d 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. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 1f350abeef..2bd01cc326 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -245,7 +245,8 @@ 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 ce19eff9b9..a6d427fe2b 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,11 +44,11 @@ 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; + 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()); @@ -88,7 +88,8 @@ ChainableControllerInterface::export_state_interfaces() } ordered_exported_state_interfaces_.push_back(state_interface); exported_state_interface_names_.push_back(interface_name); - state_interfaces_ptrs_vec.push_back(state_interface); + state_interfaces_ptrs_vec.push_back( + std::const_pointer_cast(state_interface)); } return state_interfaces_ptrs_vec; diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 4a22d78666..b254da79d8 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -22,7 +22,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector +std::vector ControllerInterface::export_state_interfaces() { return {}; diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 5233fe0278..12d5599d45 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -95,7 +95,7 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) std::runtime_error); ASSERT_THAT(exported_reference_interfaces, IsEmpty()); // expect empty return because interface prefix is not equal to the node name - std::vector exported_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()); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 52bec445a3..a0d7586308 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -785,7 +785,7 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - std::vector state_interfaces; + std::vector state_interfaces; std::vector ref_interfaces; try { diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 6834b9a74b..3b16a65261 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -71,7 +71,7 @@ class Actuator final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 123576d686..4cdd81b60f 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -169,7 +169,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -201,13 +201,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_unlisted_state_interface_descriptions(); - std::vector state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); @@ -220,7 +220,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod auto state_interface = std::make_shared(description); actuator_states_.insert(std::make_pair(name, state_interface)); unlisted_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : joint_state_interfaces_) @@ -228,7 +228,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod auto state_interface = std::make_shared(descr); actuator_states_.insert(std::make_pair(name, state_interface)); joint_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } return state_interfaces; } diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index dddcfef6b7..6fe4f25663 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -139,6 +139,7 @@ class StateInterface : public Handle using Handle::Handle; using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; }; class CommandInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 8fcc5bdb0b..96cc3e89df 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -29,23 +29,23 @@ class LoanedStateInterface using Deleter = std::function; [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedStateInterface( - StateInterface & state_interface) + const StateInterface & state_interface) : LoanedStateInterface(state_interface, nullptr) { } [[deprecated("Replaced by the new version using shared_ptr")]] LoanedStateInterface( - StateInterface & state_interface, Deleter && deleter) + const StateInterface & state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } - explicit LoanedStateInterface(StateInterface::SharedPtr state_interface) + explicit LoanedStateInterface(StateInterface::ConstSharedPtr state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(StateInterface::SharedPtr state_interface, Deleter && deleter) + LoanedStateInterface(StateInterface::ConstSharedPtr state_interface, Deleter && deleter) : state_interface_(*state_interface), deleter_(std::forward(deleter)) { } @@ -78,7 +78,7 @@ class LoanedStateInterface double get_value() const { return state_interface_.get_value(); } protected: - StateInterface & state_interface_; + const StateInterface & state_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index ece45e3146..a5592fc492 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. /** diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index edcd30cf21..ca570b78aa 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -71,7 +71,7 @@ class Sensor final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 5d859cc4f1..50d79d1a45 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -154,7 +154,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -185,13 +185,13 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_unlisted_state_interface_descriptions(); - std::vector state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); @@ -203,7 +203,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto state_interface = std::make_shared(description); sensor_states_map_.insert(std::make_pair(name, state_interface)); unlisted_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : sensor_state_interfaces_) @@ -212,7 +212,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto state_interface = std::make_shared(descr); sensor_states_map_.insert(std::make_pair(name, state_interface)); sensor_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } return state_interfaces; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 432ccc1fc8..09adaa7190 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -71,7 +71,7 @@ class System final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 4814f6f559..6b0652d3fe 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -190,7 +190,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -221,13 +221,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - std::vector on_export_state_interfaces() + std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_unlisted_state_interface_descriptions(); - std::vector state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); @@ -241,7 +241,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto state_interface = std::make_shared(description); system_states_.insert(std::make_pair(name, state_interface)); unlisted_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : joint_state_interfaces_) @@ -249,21 +249,21 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); joint_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : sensor_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); sensor_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : gpio_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); gpio_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } return state_interfaces; } diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index aae16a79ee..b0ed1f7c80 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -204,7 +204,7 @@ const rclcpp_lifecycle::State & Actuator::error() return impl_->get_lifecycle_state(); } -std::vector Actuator::export_state_interfaces() +std::vector Actuator::export_state_interfaces() { // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed @@ -222,11 +222,11 @@ std::vector Actuator::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { - interface_ptrs.emplace_back(std::make_shared(interface)); + interface_ptrs.emplace_back(std::make_shared(interface)); } return interface_ptrs; // END: for backward compatibility diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4d0c41b242..e5445491c8 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -608,7 +608,7 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { - std::vector interfaces = hardware.export_state_interfaces(); + auto interfaces = hardware.export_state_interfaces(); const auto interface_names = add_state_interfaces(interfaces); RCLCPP_WARN( @@ -678,7 +678,7 @@ class ResourceStorage } } - std::string add_state_interface(StateInterface::SharedPtr interface) + std::string add_state_interface(StateInterface::ConstSharedPtr interface) { auto interface_name = interface->get_name(); const auto [it, success] = state_interface_map_.emplace(interface_name, interface); @@ -702,7 +702,8 @@ class ResourceStorage * \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 add_state_interfaces( + std::vector & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); @@ -1068,7 +1069,7 @@ class ResourceStorage std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces - std::map state_interface_map_; + std::map state_interface_map_; /// Storage of all available command interfaces std::map command_interface_map_; @@ -1241,7 +1242,7 @@ 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_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index f3dc7efba5..5da01368c9 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -203,7 +203,7 @@ const rclcpp_lifecycle::State & Sensor::error() return impl_->get_lifecycle_state(); } -std::vector Sensor::export_state_interfaces() +std::vector Sensor::export_state_interfaces() { // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed @@ -221,11 +221,11 @@ std::vector Sensor::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { - interface_ptrs.emplace_back(std::make_shared(interface)); + interface_ptrs.emplace_back(std::make_shared(interface)); } return interface_ptrs; // END: for backward compatibility diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 9f1a35d736..abae924dfc 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -202,7 +202,7 @@ const rclcpp_lifecycle::State & System::error() return impl_->get_lifecycle_state(); } -std::vector System::export_state_interfaces() +std::vector System::export_state_interfaces() { // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed @@ -220,11 +220,11 @@ std::vector System::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { - interface_ptrs.emplace_back(std::make_shared(interface)); + interface_ptrs.emplace_back(std::make_shared(interface)); } return interface_ptrs; // END: for backward compatibility