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 
(cherry picked from commit 88c74ae)

# Conflicts:
#	hardware_interface/include/hardware_interface/resource_manager.hpp
#	hardware_interface/src/resource_manager.cpp
#	hardware_interface/test/mock_components/test_generic_system.cpp
#	hardware_interface/test/test_resource_manager.cpp
  • Loading branch information
@destogl @mergify
destogl authored and mergify[bot] committed Dec 4, 2023
1 parent 8695bdf commit 0ce33d9
Show file tree
Hide file tree
Showing 4 changed files with 557 additions and 168 deletions.
31 changes: 18 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 @@ -112,12 +112,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 @@ -212,13 +206,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 @@ -374,6 +361,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 @@ -383,6 +383,11 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager

mutable std::recursive_mutex resource_interfaces_lock_;
mutable std::recursive_mutex claimed_command_interfaces_lock_;
<<<<<<< HEAD
=======
mutable std::recursive_mutex resources_lock_;

>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816))
std::unique_ptr<ResourceStorage> resource_storage_;

bool is_urdf_loaded__ = false;
Expand Down
149 changes: 117 additions & 32 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -665,6 +665,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 @@ -701,8 +702,11 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac
}
}

<<<<<<< HEAD
bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; }

=======
>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816))
// CM API: Called in "update"-thread
LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key)
{
Expand All @@ -715,6 +719,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 @@ -726,19 +731,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 @@ -748,6 +748,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 @@ -757,12 +758,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 @@ -774,6 +777,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 @@ -796,6 +800,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 @@ -807,6 +812,56 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string &
resource_storage_->remove_command_interfaces(interface_names);
}

<<<<<<< HEAD
=======
// CM API: Called in "callback/slow"-thread
void ResourceManager::cache_controller_to_hardware(
const std::string & controller_name, const std::vector<std::string> & interfaces)
{
for (const auto & interface : interfaces)
{
bool found = false;
for (const auto & [hw_name, hw_info] : resource_storage_->hardware_info_map_)
{
auto cmd_itf_it =
std::find(hw_info.command_interfaces.begin(), hw_info.command_interfaces.end(), interface);
if (cmd_itf_it != hw_info.command_interfaces.end())
{
found = true;
}
auto state_itf_it =
std::find(hw_info.state_interfaces.begin(), hw_info.state_interfaces.end(), interface);
if (state_itf_it != hw_info.state_interfaces.end())
{
found = true;
}

if (found)
{
// check if controller exist already in the list and if not add it
auto controllers = resource_storage_->hardware_used_by_controllers_[hw_name];
auto ctrl_it = std::find(controllers.begin(), controllers.end(), controller_name);
if (ctrl_it == controllers.end())
{
// add because it does not exist
controllers.reserve(controllers.size() + 1);
controllers.push_back(controller_name);
}
resource_storage_->hardware_used_by_controllers_[hw_name] = controllers;
break;
}
}
}
}

// CM API: Called in "update"-thread
std::vector<std::string> ResourceManager::get_cached_controllers_to_hardware(
const std::string & hardware_name)
{
return resource_storage_->hardware_used_by_controllers_[hardware_name];
}

>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816))
// CM API: Called in "update"-thread
bool ResourceManager::command_interface_is_claimed(const std::string & key) const
{
Expand Down Expand Up @@ -848,6 +903,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 @@ -859,20 +915,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 @@ -882,16 +932,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 All @@ -910,12 +950,7 @@ void ResourceManager::import_component(
resource_storage_->initialize_system(std::move(system), hardware_info);
}

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 @@ -934,6 +969,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 @@ -979,6 +1015,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 @@ -1006,6 +1043,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 @@ -1089,7 +1127,13 @@ return_type ResourceManager::set_component_state(
return result;
}

<<<<<<< HEAD
void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
=======
// CM API: Called in "update"-thread
HardwareReadWriteStatus ResourceManager::read(
const rclcpp::Time & time, const rclcpp::Duration & period)
>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816))
{
for (auto & component : resource_storage_->actuators_)
{
Expand All @@ -1105,7 +1149,13 @@ void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & p
}
}

<<<<<<< HEAD
void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration & period)
=======
// CM API: Called in "update"-thread
HardwareReadWriteStatus ResourceManager::write(
const rclcpp::Time & time, const rclcpp::Duration & period)
>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816))
{
for (auto & component : resource_storage_->actuators_)
{
Expand All @@ -1117,6 +1167,39 @@ void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration &
}
}

// 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 @@ -1189,7 +1272,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 0ce33d9

Please sign in to comment.