[RM/HW] Constify the exported state interfaces using ConstSharedPtr (#…

…1767)

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<hardware_interface::StateInterface::SharedPtr> export_state_interfaces() final;
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> export_state_interfaces() final;
 
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<hardware_interface::CommandInterface::SharedPtr> export_reference_interfaces() final;

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<hardware_interface::StateInterface::SharedPtr> export_state_interfaces() final;
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> export_state_interfaces() final;
 
   /**
    * Controller has no reference interfaces.

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<hardware_interface::StateInterface::SharedPtr> export_state_interfaces() = 0;
+  virtual std::vector<hardware_interface::StateInterface::ConstSharedPtr>
+  export_state_interfaces() = 0;
 
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch

controller_interface/src/chainable_controller_interface.cpp

@@ -44,11 +44,11 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
-std::vector<hardware_interface::StateInterface::SharedPtr>
+std::vector<hardware_interface::StateInterface::ConstSharedPtr>
 ChainableControllerInterface::export_state_interfaces()
 {
   auto state_interfaces = on_export_state_interfaces();
-  std::vector<hardware_interface::StateInterface::SharedPtr> state_interfaces_ptrs_vec;
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> 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<const hardware_interface::StateInterface>(state_interface));
   }
 
   return state_interfaces_ptrs_vec;

controller_interface/src/controller_interface.cpp

@@ -22,7 +22,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}
 
 bool ControllerInterface::is_chainable() const { return false; }
 
-std::vector<hardware_interface::StateInterface::SharedPtr>
+std::vector<hardware_interface::StateInterface::ConstSharedPtr>
 ControllerInterface::export_state_interfaces()
 {
   return {};

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<hardware_interface::StateInterface::SharedPtr> exported_state_interfaces;
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> exported_state_interfaces;
   EXPECT_THROW(
     { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error);
   ASSERT_THAT(exported_state_interfaces, IsEmpty());

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<hardware_interface::StateInterface::SharedPtr> state_interfaces;
+    std::vector<hardware_interface::StateInterface::ConstSharedPtr> state_interfaces;
     std::vector<hardware_interface::CommandInterface::SharedPtr> ref_interfaces;
     try
     {

hardware_interface/include/hardware_interface/actuator.hpp

@@ -71,7 +71,7 @@ class Actuator final
   const rclcpp_lifecycle::State & error();
 
   HARDWARE_INTERFACE_PUBLIC
-  std::vector<StateInterface::SharedPtr> export_state_interfaces();
+  std::vector<StateInterface::ConstSharedPtr> export_state_interfaces();
 
   HARDWARE_INTERFACE_PUBLIC
   std::vector<CommandInterface::SharedPtr> export_command_interfaces();

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<StateInterface::SharedPtr> on_export_state_interfaces() method. "
+    "Replaced by vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() method. "
     "Exporting is "
     "handled "
     "by the Framework.")]] virtual std::vector<StateInterface>
@@ -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<StateInterface::SharedPtr> on_export_state_interfaces()
+  virtual std::vector<StateInterface::ConstSharedPtr> on_export_state_interfaces()
   {
     // import the unlisted interfaces
     std::vector<hardware_interface::InterfaceDescription> unlisted_interface_descriptions =
       export_unlisted_state_interface_descriptions();
 
-    std::vector<StateInterface::SharedPtr> state_interfaces;
+    std::vector<StateInterface::ConstSharedPtr> state_interfaces;
     state_interfaces.reserve(
       unlisted_interface_descriptions.size() + joint_state_interfaces_.size());
 
@@ -220,15 +220,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
       auto state_interface = std::make_shared<StateInterface>(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<const StateInterface>(state_interface));
     }
 
     for (const auto & [name, descr] : joint_state_interfaces_)
     {
       auto state_interface = std::make_shared<StateInterface>(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<const StateInterface>(state_interface));
     }
     return state_interfaces;
   }

hardware_interface/include/hardware_interface/handle.hpp

@@ -139,6 +139,7 @@ class StateInterface : public Handle
   using Handle::Handle;
 
   using SharedPtr = std::shared_ptr<StateInterface>;
+  using ConstSharedPtr = std::shared_ptr<const StateInterface>;
 };
 
 class CommandInterface : public Handle

hardware_interface/include/hardware_interface/loaned_state_interface.hpp

@@ -29,23 +29,23 @@ class LoanedStateInterface
   using Deleter = std::function<void(void)>;
 
   [[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>(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>(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_;
 };
 

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<StateInterface::SharedPtr> & interfaces);
+    const std::string & controller_name, std::vector<StateInterface::ConstSharedPtr> & interfaces);
 
   /// Get list of exported tate interface of a controller.
   /**

hardware_interface/include/hardware_interface/sensor.hpp

@@ -71,7 +71,7 @@ class Sensor final
   const rclcpp_lifecycle::State & error();
 
   HARDWARE_INTERFACE_PUBLIC
-  std::vector<StateInterface::SharedPtr> export_state_interfaces();
+  std::vector<StateInterface::ConstSharedPtr> export_state_interfaces();
 
   HARDWARE_INTERFACE_PUBLIC
   std::string get_name() const;

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<StateInterface::SharedPtr> on_export_state_interfaces() method. "
+    "Replaced by vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() method. "
     "Exporting is handled "
     "by the Framework.")]] virtual std::vector<StateInterface>
   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<StateInterface::SharedPtr> on_export_state_interfaces()
+  virtual std::vector<StateInterface::ConstSharedPtr> on_export_state_interfaces()
   {
     // import the unlisted interfaces
     std::vector<hardware_interface::InterfaceDescription> unlisted_interface_descriptions =
       export_unlisted_state_interface_descriptions();
 
-    std::vector<StateInterface::SharedPtr> state_interfaces;
+    std::vector<StateInterface::ConstSharedPtr> 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<StateInterface>(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<const StateInterface>(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<StateInterface>(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<const StateInterface>(state_interface));
     }
 
     return state_interfaces;

hardware_interface/include/hardware_interface/system.hpp

@@ -71,7 +71,7 @@ class System final
   const rclcpp_lifecycle::State & error();
 
   HARDWARE_INTERFACE_PUBLIC
-  std::vector<StateInterface::SharedPtr> export_state_interfaces();
+  std::vector<StateInterface::ConstSharedPtr> export_state_interfaces();
 
   HARDWARE_INTERFACE_PUBLIC
   std::vector<CommandInterface::SharedPtr> export_command_interfaces();

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<StateInterface::SharedPtr> on_export_state_interfaces() method. "
+    "Replaced by vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() method. "
     "Exporting is handled "
     "by the Framework.")]] virtual std::vector<StateInterface>
   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<StateInterface::SharedPtr> on_export_state_interfaces()
+  std::vector<StateInterface::ConstSharedPtr> on_export_state_interfaces()
   {
     // import the unlisted interfaces
     std::vector<hardware_interface::InterfaceDescription> unlisted_interface_descriptions =
       export_unlisted_state_interface_descriptions();
 
-    std::vector<StateInterface::SharedPtr> state_interfaces;
+    std::vector<StateInterface::ConstSharedPtr> state_interfaces;
     state_interfaces.reserve(
       unlisted_interface_descriptions.size() + joint_state_interfaces_.size() +
       sensor_state_interfaces_.size() + gpio_state_interfaces_.size());
@@ -241,29 +241,29 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
       auto state_interface = std::make_shared<StateInterface>(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<const StateInterface>(state_interface));
     }
 
     for (const auto & [name, descr] : joint_state_interfaces_)
     {
       auto state_interface = std::make_shared<StateInterface>(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<const StateInterface>(state_interface));
     }
     for (const auto & [name, descr] : sensor_state_interfaces_)
     {
       auto state_interface = std::make_shared<StateInterface>(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<const StateInterface>(state_interface));
     }
     for (const auto & [name, descr] : gpio_state_interfaces_)
     {
       auto state_interface = std::make_shared<StateInterface>(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<const StateInterface>(state_interface));
     }
     return state_interfaces;
   }

hardware_interface/src/actuator.cpp

@@ -204,7 +204,7 @@ const rclcpp_lifecycle::State & Actuator::error()
   return impl_->get_lifecycle_state();
 }
 
-std::vector<StateInterface::SharedPtr> Actuator::export_state_interfaces()
+std::vector<StateInterface::ConstSharedPtr> 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<StateInterface::SharedPtr> Actuator::export_state_interfaces()
 
   // BEGIN (Handle export change): for backward compatibility, can be removed if
   // export_command_interfaces() method is removed
-  std::vector<StateInterface::SharedPtr> interface_ptrs;
+  std::vector<StateInterface::ConstSharedPtr> interface_ptrs;
   interface_ptrs.reserve(interfaces.size());
   for (auto const & interface : interfaces)
   {
-    interface_ptrs.emplace_back(std::make_shared<StateInterface>(interface));
+    interface_ptrs.emplace_back(std::make_shared<const StateInterface>(interface));
   }
   return interface_ptrs;
   // END: for backward compatibility

hardware_interface/src/resource_manager.cpp

@@ -608,7 +608,7 @@ class ResourceStorage
   template <class HardwareT>
   void import_state_interfaces(HardwareT & hardware)
   {
-    std::vector<StateInterface::SharedPtr> 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<std::string> add_state_interfaces(std::vector<StateInterface::SharedPtr> & interfaces)
+  std::vector<std::string> add_state_interfaces(
+    std::vector<StateInterface::ConstSharedPtr> & interfaces)
   {
     std::vector<std::string> interface_names;
     interface_names.reserve(interfaces.size());
@@ -1068,7 +1069,7 @@ class ResourceStorage
   std::unordered_map<std::string, std::vector<std::string>> controllers_reference_interfaces_map_;
 
   /// Storage of all available state interfaces
-  std::map<std::string, StateInterface::SharedPtr> state_interface_map_;
+  std::map<std::string, StateInterface::ConstSharedPtr> state_interface_map_;
   /// Storage of all available command interfaces
   std::map<std::string, CommandInterface::SharedPtr> 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<StateInterface::SharedPtr> & interfaces)
+  const std::string & controller_name, std::vector<StateInterface::ConstSharedPtr> & interfaces)
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
   auto interface_names = resource_storage_->add_state_interfaces(interfaces);

hardware_interface/src/sensor.cpp

@@ -203,7 +203,7 @@ const rclcpp_lifecycle::State & Sensor::error()
   return impl_->get_lifecycle_state();
 }
 
-std::vector<StateInterface::SharedPtr> Sensor::export_state_interfaces()
+std::vector<StateInterface::ConstSharedPtr> 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<StateInterface::SharedPtr> Sensor::export_state_interfaces()
 
   // BEGIN (Handle export change): for backward compatibility, can be removed if
   // export_command_interfaces() method is removed
-  std::vector<StateInterface::SharedPtr> interface_ptrs;
+  std::vector<StateInterface::ConstSharedPtr> interface_ptrs;
   interface_ptrs.reserve(interfaces.size());
   for (auto const & interface : interfaces)
   {
-    interface_ptrs.emplace_back(std::make_shared<StateInterface>(interface));
+    interface_ptrs.emplace_back(std::make_shared<const StateInterface>(interface));
   }
   return interface_ptrs;
   // END: for backward compatibility