Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 4 commits into from
Dec 11, 2023
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0ce33d9
Cleanup Resource Manager a bit to increase clarity. (#816)
destogl Sep 24, 2022
b518370
Merge branch 'humble' into mergify/bp/humble/pr-816
christophfroehlich Dec 10, 2023
68d2cc6
Fix code format
christophfroehlich Dec 10, 2023
8538a38
Fix merge conflicts
christophfroehlich Dec 10, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 14 additions & 13 deletions hardware_interface/include/hardware_interface/resource_manager.hpp
View file
Open in desktop
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
Open in desktop
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
Loading