Cleanup Resource Manager a bit to increase clarity. (backport #816) (#1191)

Author: mergify[bot]

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -118,12 +118,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   std::vector<std::string> available_state_interfaces() const;
 
-  /// Checks whether a state interface is registered under the given key.
-  /**
-   * \return true if interface exist, false otherwise.
-   */
-  bool state_interface_exists(const std::string & key) const;
-
   /// Checks whether a state interface is available under the given key.
   /**
    * \return true if interface is available, false otherwise.
@@ -239,13 +233,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   std::vector<std::string> available_command_interfaces() const;
 
-  /// Checks whether a command interface is registered under the given key.
-  /**
-   * \param[in] key string identifying the interface to check.
-   * \return true if interface exist, false otherwise.
-   */
-  bool command_interface_exists(const std::string & key) const;
-
   /// Checks whether a command interface is available under the given name.
   /**
    * \param[in] name string identifying the interface to check.
@@ -401,6 +388,19 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   void activate_all_components();
 
+  /// Checks whether a command interface is registered under the given key.
+  /**
+   * \param[in] key string identifying the interface to check.
+   * \return true if interface exist, false otherwise.
+   */
+  bool command_interface_exists(const std::string & key) const;
+
+  /// Checks whether a state interface is registered under the given key.
+  /**
+   * \return true if interface exist, false otherwise.
+   */
+  bool state_interface_exists(const std::string & key) const;
+
 private:
   void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
@@ -411,6 +411,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   mutable std::recursive_mutex resource_interfaces_lock_;
   mutable std::recursive_mutex claimed_command_interfaces_lock_;
   mutable std::recursive_mutex resources_lock_;
+
   std::unique_ptr<ResourceStorage> resource_storage_;
 
   // Structure to store read and write status so it is not initialized in the real-time loop

hardware_interface/src/resource_manager.cpp

@@ -675,6 +675,7 @@ ResourceManager::ResourceManager(
   }
 }
 
+// CM API: Called in "callback/slow"-thread
 void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces)
 {
   is_urdf_loaded__ = true;
@@ -730,6 +731,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string &
   return LoanedStateInterface(resource_storage_->state_interface_map_.at(key));
 }
 
+// CM API: Called in "callback/slow"-thread
 std::vector<std::string> ResourceManager::state_interface_keys() const
 {
   std::vector<std::string> keys;
@@ -741,19 +743,14 @@ std::vector<std::string> ResourceManager::state_interface_keys() const
   return keys;
 }
 
+// CM API: Called in "update"-thread
 std::vector<std::string> ResourceManager::available_state_interfaces() const
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
   return resource_storage_->available_state_interfaces_;
 }
 
-bool ResourceManager::state_interface_exists(const std::string & key) const
-{
-  std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
-  return resource_storage_->state_interface_map_.find(key) !=
-         resource_storage_->state_interface_map_.end();
-}
-
+// CM API: Called in "update"-thread (indirectly through `claim_state_interface`)
 bool ResourceManager::state_interface_is_available(const std::string & name) const
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
@@ -763,6 +760,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con
            name) != resource_storage_->available_state_interfaces_.end();
 }
 
+// CM API: Called in "callback/slow"-thread
 void ResourceManager::import_controller_reference_interfaces(
   const std::string & controller_name, std::vector<CommandInterface> & interfaces)
 {
@@ -772,12 +770,14 @@ void ResourceManager::import_controller_reference_interfaces(
   resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names;
 }
 
+// CM API: Called in "callback/slow"-thread
 std::vector<std::string> ResourceManager::get_controller_reference_interface_names(
   const std::string & controller_name)
 {
   return resource_storage_->controllers_reference_interfaces_map_.at(controller_name);
 }
 
+// CM API: Called in "update"-thread
 void ResourceManager::make_controller_reference_interfaces_available(
   const std::string & controller_name)
 {
@@ -789,6 +789,7 @@ void ResourceManager::make_controller_reference_interfaces_available(
     interface_names.end());
 }
 
+// CM API: Called in "update"-thread
 void ResourceManager::make_controller_reference_interfaces_unavailable(
   const std::string & controller_name)
 {
@@ -811,6 +812,7 @@ void ResourceManager::make_controller_reference_interfaces_unavailable(
   }
 }
 
+// CM API: Called in "callback/slow"-thread
 void ResourceManager::remove_controller_reference_interfaces(const std::string & controller_name)
 {
   auto interface_names =
@@ -822,6 +824,7 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string &
   resource_storage_->remove_command_interfaces(interface_names);
 }
 
+// CM API: Called in "callback/slow"-thread
 void ResourceManager::cache_controller_to_hardware(
   const std::string & controller_name, const std::vector<std::string> & interfaces)
 {
@@ -861,6 +864,7 @@ void ResourceManager::cache_controller_to_hardware(
   }
 }
 
+// CM API: Called in "update"-thread
 std::vector<std::string> ResourceManager::get_cached_controllers_to_hardware(
   const std::string & hardware_name)
 {
@@ -908,6 +912,7 @@ void ResourceManager::release_command_interface(const std::string & key)
   resource_storage_->claimed_command_interface_map_[key] = false;
 }
 
+// CM API: Called in "callback/slow"-thread
 std::vector<std::string> ResourceManager::command_interface_keys() const
 {
   std::vector<std::string> keys;
@@ -919,20 +924,14 @@ std::vector<std::string> ResourceManager::command_interface_keys() const
   return keys;
 }
 
+// CM API: Called in "update"-thread
 std::vector<std::string> ResourceManager::available_command_interfaces() const
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
   return resource_storage_->available_command_interfaces_;
 }
 
-bool ResourceManager::command_interface_exists(const std::string & key) const
-{
-  std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
-  return resource_storage_->command_interface_map_.find(key) !=
-         resource_storage_->command_interface_map_.end();
-}
-
-// CM API
+// CM API: Called in "callback/slow"-thread
 bool ResourceManager::command_interface_is_available(const std::string & name) const
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
@@ -942,16 +941,6 @@ bool ResourceManager::command_interface_is_available(const std::string & name) c
            name) != resource_storage_->available_command_interfaces_.end();
 }
 
-size_t ResourceManager::actuator_components_size() const
-{
-  return resource_storage_->actuators_.size();
-}
-
-size_t ResourceManager::sensor_components_size() const
-{
-  return resource_storage_->sensors_.size();
-}
-
 void ResourceManager::import_component(
   std::unique_ptr<ActuatorInterface> actuator, const HardwareInfo & hardware_info)
 {
@@ -982,12 +971,7 @@ void ResourceManager::import_component(
     resource_storage_->systems_.size());
 }
 
-size_t ResourceManager::system_components_size() const
-{
-  return resource_storage_->systems_.size();
-}
-// End of "used only in tests"
-
+// CM API: Called in "callback/slow"-thread
 std::unordered_map<std::string, HardwareComponentInfo> ResourceManager::get_components_status()
 {
   for (auto & component : resource_storage_->actuators_)
@@ -1006,6 +990,7 @@ std::unordered_map<std::string, HardwareComponentInfo> ResourceManager::get_comp
   return resource_storage_->hardware_info_map_;
 }
 
+// CM API: Called in "callback/slow"-thread
 bool ResourceManager::prepare_command_mode_switch(
   const std::vector<std::string> & start_interfaces,
   const std::vector<std::string> & stop_interfaces)
@@ -1051,6 +1036,7 @@ bool ResourceManager::prepare_command_mode_switch(
   return true;
 }
 
+// CM API: Called in "update"-thread
 bool ResourceManager::perform_command_mode_switch(
   const std::vector<std::string> & start_interfaces,
   const std::vector<std::string> & stop_interfaces)
@@ -1078,6 +1064,7 @@ bool ResourceManager::perform_command_mode_switch(
   return true;
 }
 
+// CM API: Called in "callback/slow"-thread
 return_type ResourceManager::set_component_state(
   const std::string & component_name, rclcpp_lifecycle::State & target_state)
 {
@@ -1161,6 +1148,7 @@ return_type ResourceManager::set_component_state(
   return result;
 }
 
+// CM API: Called in "update"-thread
 HardwareReadWriteStatus ResourceManager::read(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
@@ -1188,6 +1176,7 @@ HardwareReadWriteStatus ResourceManager::read(
   return read_write_status;
 }
 
+// CM API: Called in "update"-thread
 HardwareReadWriteStatus ResourceManager::write(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
@@ -1214,6 +1203,39 @@ HardwareReadWriteStatus ResourceManager::write(
   return read_write_status;
 }
 
+// BEGIN: "used only in tests and locally"
+size_t ResourceManager::actuator_components_size() const
+{
+  return resource_storage_->actuators_.size();
+}
+
+size_t ResourceManager::sensor_components_size() const
+{
+  return resource_storage_->sensors_.size();
+}
+
+size_t ResourceManager::system_components_size() const
+{
+  return resource_storage_->systems_.size();
+}
+
+bool ResourceManager::command_interface_exists(const std::string & key) const
+{
+  std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
+  return resource_storage_->command_interface_map_.find(key) !=
+         resource_storage_->command_interface_map_.end();
+}
+
+bool ResourceManager::state_interface_exists(const std::string & key) const
+{
+  std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
+  return resource_storage_->state_interface_map_.find(key) !=
+         resource_storage_->state_interface_map_.end();
+}
+// END: "used only in tests and locally"
+
+// BEGIN: private methods
+
 void ResourceManager::validate_storage(
   const std::vector<hardware_interface::HardwareInfo> & hardware_info) const
 {
@@ -1286,7 +1308,9 @@ void ResourceManager::validate_storage(
   }
 }
 
-// Temporary method to keep old interface and reduce framework changes in PRs
+// END: private methods
+
+// Temporary method to keep old interface and reduce framework changes in the PRs
 void ResourceManager::activate_all_components()
 {
   using lifecycle_msgs::msg::State;