Skip to content

Commit

Permalink
Cleanup Resource Manager a bit to increase clarity. (#816)
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl authored Sep 24, 2022
1 parent e748824 commit 88c74ae
Show file tree
Hide file tree
Showing 4 changed files with 258 additions and 201 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 @@ -108,12 +108,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 @@ -228,13 +222,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 @@ -390,6 +377,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 @@ -400,6 +400,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
89 changes: 57 additions & 32 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -591,6 +591,7 @@ ResourceManager::ResourceManager(
}
}

// CM API: Called in "callback/slow"-thread
void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces)
{
const std::string system_type = "system";
Expand Down Expand Up @@ -631,6 +632,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac
resource_storage_->systems_.size());
}

// CM API: Called in "update"-thread
LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key)
{
if (!state_interface_is_available(key))
Expand All @@ -642,6 +644,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 @@ -653,19 +656,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 @@ -675,6 +673,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 @@ -684,12 +683,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 @@ -701,6 +702,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 @@ -723,6 +725,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 @@ -734,6 +737,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 @@ -773,6 +777,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 @@ -820,6 +825,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 @@ -831,20 +837,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 @@ -854,16 +854,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 @@ -894,12 +884,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 @@ -918,6 +903,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 @@ -963,6 +949,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 @@ -990,6 +977,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 @@ -1073,6 +1061,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 @@ -1100,6 +1089,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 @@ -1126,6 +1116,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 @@ -1181,7 +1204,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 88c74ae

Please sign in to comment.