[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