Skip to content

Commit

Permalink
Cleanup Resource Manager a bit to increase clarity. (backport #816) (#…
Browse files Browse the repository at this point in the history 
…1191)
  • Loading branch information
@mergify
mergify[bot] authored Dec 11, 2023
1 parent fc7097b commit 429820d
Show file tree
Hide file tree
Showing 4 changed files with 178 additions and 186 deletions.
27 changes: 14 additions & 13 deletions hardware_interface/include/hardware_interface/resource_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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;

Expand All @@ -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
Expand Down
88 changes: 56 additions & 32 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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_);
Expand All @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -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 =
Expand All @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand All @@ -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_);
Expand All @@ -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)
{
Expand Down Expand Up @@ -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_)
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand All @@ -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
{
Expand Down Expand Up @@ -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;
Expand Down
Loading

0 comments on commit 429820d

Please sign in to comment.