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/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..c59aff0700 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(); + std::vector 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_; 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