ros2_control - rolling
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
hardware_interface::ResourceManager Class Reference
Inheritance diagram for hardware_interface::ResourceManager:
Inheritance graph
[legend]

Public Member Functions

 ResourceManager (rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface)
 Default constructor for the Resource Manager.
 
 ResourceManager (const std::string &urdf, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, bool activate_all=false, const unsigned int update_rate=100)
 Constructor for the Resource Manager. More...
 
 ResourceManager (const ResourceManager &)=delete
 
virtual bool load_and_initialize_components (const std::string &urdf, const unsigned int update_rate=100)
 Load resources from on a given URDF. More...
 
bool are_components_initialized () const
 if the resource manager load_and_initialize_components(...) function has been called this returns true. We want to permit to loading the urdf later on, but we currently don't want to permit multiple calls to load_and_initialize_components (reloading/loading different urdf). More...
 
LoanedStateInterface claim_state_interface (const std::string &key)
 Claim a state interface given its key. More...
 
std::vector< std::string > state_interface_keys () const
 Returns all registered state interfaces keys. More...
 
std::vector< std::string > available_state_interfaces () const
 Returns all available state interfaces keys. More...
 
bool state_interface_is_available (const std::string &name) const
 Checks whether a state interface is available under the given key. More...
 
void import_controller_exported_state_interfaces (const std::string &controller_name, std::vector< StateInterface > &interfaces)
 Add controllers' exported state interfaces to resource manager. More...
 
std::vector< std::string > get_controller_exported_state_interface_names (const std::string &controller_name)
 Get list of exported tate interface of a controller. More...
 
void make_controller_exported_state_interfaces_available (const std::string &controller_name)
 Add controller's exported state interfaces to available list. More...
 
void make_controller_exported_state_interfaces_unavailable (const std::string &controller_name)
 Remove controller's exported state interface to available list. More...
 
void remove_controller_exported_state_interfaces (const std::string &controller_name)
 Remove controllers exported state interfaces from resource manager. More...
 
void import_controller_reference_interfaces (const std::string &controller_name, std::vector< CommandInterface > &interfaces)
 Add controllers' reference interfaces to resource manager. More...
 
std::vector< std::string > get_controller_reference_interface_names (const std::string &controller_name)
 Get list of reference interface of a controller. More...
 
void make_controller_reference_interfaces_available (const std::string &controller_name)
 Add controller's reference interface to available list. More...
 
void make_controller_reference_interfaces_unavailable (const std::string &controller_name)
 Remove controller's reference interface to available list. More...
 
void remove_controller_reference_interfaces (const std::string &controller_name)
 Remove controllers reference interfaces from resource manager. More...
 
void cache_controller_to_hardware (const std::string &controller_name, const std::vector< std::string > &interfaces)
 Cache mapping between hardware and controllers using it. More...
 
std::vector< std::string > get_cached_controllers_to_hardware (const std::string &hardware_name)
 Return cached controllers for a specific hardware. More...
 
bool command_interface_is_claimed (const std::string &key) const
 Checks whether a command interface is already claimed. More...
 
LoanedCommandInterface claim_command_interface (const std::string &key)
 Claim a command interface given its key. More...
 
std::vector< std::string > command_interface_keys () const
 Returns all registered command interfaces keys. More...
 
std::vector< std::string > available_command_interfaces () const
 Returns all available command interfaces keys. More...
 
bool command_interface_is_available (const std::string &interface) const
 Checks whether a command interface is available under the given name. More...
 
size_t actuator_components_size () const
 Return the number size_t of loaded actuator components. More...
 
size_t sensor_components_size () const
 Return the number of loaded sensor components. More...
 
size_t system_components_size () const
 Return the number of loaded system components. More...
 
void import_component (std::unique_ptr< ActuatorInterface > actuator, const HardwareInfo &hardware_info)
 Import a hardware component which is not listed in the URDF. More...
 
void import_component (std::unique_ptr< SensorInterface > sensor, const HardwareInfo &hardware_info)
 Import a hardware component which is not listed in the URDF. More...
 
void import_component (std::unique_ptr< SystemInterface > system, const HardwareInfo &hardware_info)
 Import a hardware component which is not listed in the URDF. More...
 
std::unordered_map< std::string, HardwareComponentInfoget_components_status ()
 Return status for all components. More...
 
bool prepare_command_mode_switch (const std::vector< std::string > &start_interfaces, const std::vector< std::string > &stop_interfaces)
 Prepare the hardware components for a new command interface mode. More...
 
bool perform_command_mode_switch (const std::vector< std::string > &start_interfaces, const std::vector< std::string > &stop_interfaces)
 Notify the hardware components that realtime hardware mode switching should occur. More...
 
return_type set_component_state (const std::string &component_name, rclcpp_lifecycle::State &target_state)
 Sets state of hardware component. More...
 
void shutdown_async_components ()
 Deletes all async components from the resource storage. More...
 
HardwareReadWriteStatus read (const rclcpp::Time &time, const rclcpp::Duration &period)
 Reads all loaded hardware components. More...
 
HardwareReadWriteStatus write (const rclcpp::Time &time, const rclcpp::Duration &period)
 Write all loaded hardware components. More...
 
bool command_interface_exists (const std::string &key) const
 Checks whether a command interface is registered under the given key. More...
 
bool state_interface_exists (const std::string &key) const
 Checks whether a state interface is registered under the given key. More...
 

Protected Member Functions

rclcpp::Logger get_logger () const
 Gets the logger for the resource manager. More...
 
rclcpp::Clock::SharedPtr get_clock () const
 Gets the clock for the resource manager. More...
 

Protected Attributes

bool components_are_loaded_and_initialized_ = false
 
std::recursive_mutex resource_interfaces_lock_
 
std::recursive_mutex claimed_command_interfaces_lock_
 
std::recursive_mutex resources_lock_
 

Constructor & Destructor Documentation

◆ ResourceManager()

hardware_interface::ResourceManager::ResourceManager ( const std::string &  urdf,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr  clock_interface,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr  logger_interface,
bool  activate_all = false,
const unsigned int  update_rate = 100 
)
explicit

Constructor for the Resource Manager.

The implementation loads the specified urdf and initializes the hardware components listed within as well as populate their respective state and command interfaces.

Parameters
[in]urdfstring containing the URDF.
[in]activate_allboolean argument indicating if all resources should be immediately activated. Currently used only in tests.
[in]update_rateUpdate rate of the controller manager to calculate calling frequency of async components.
[in]clock_interfacereference to the clock interface of the CM node for getting time used for triggering async components.

Member Function Documentation

◆ actuator_components_size()

size_t hardware_interface::ResourceManager::actuator_components_size ( ) const

Return the number size_t of loaded actuator components.

Returns
number of actuator components.

◆ are_components_initialized()

bool hardware_interface::ResourceManager::are_components_initialized ( ) const

if the resource manager load_and_initialize_components(...) function has been called this returns true. We want to permit to loading the urdf later on, but we currently don't want to permit multiple calls to load_and_initialize_components (reloading/loading different urdf).

Returns
true if the resource manager has successfully loaded and initialized the components
false if the resource manager doesn't have any components loaded and initialized.

◆ available_command_interfaces()

std::vector< std::string > hardware_interface::ResourceManager::available_command_interfaces ( ) const

Returns all available command interfaces keys.

The keys are collected from the available list.

Returns
vector of strings, containing all available command interface names.

◆ available_state_interfaces()

std::vector< std::string > hardware_interface::ResourceManager::available_state_interfaces ( ) const

Returns all available state interfaces keys.

The keys are collected from the available list.

Returns
Vector of strings, containing all available state interface names.

◆ cache_controller_to_hardware()

void hardware_interface::ResourceManager::cache_controller_to_hardware ( const std::string &  controller_name,
const std::vector< std::string > &  interfaces 
)

Cache mapping between hardware and controllers using it.

Find mapping between controller and hardware based on interfaces controller with controller_name is using and cache those for later usage.

Parameters
[in]controller_namename of the controller which interfaces are provided.
[in]interfaceslist of interfaces controller with controller_name is using.

◆ claim_command_interface()

LoanedCommandInterface hardware_interface::ResourceManager::claim_command_interface ( const std::string &  key)

Claim a command interface given its key.

The resource is claimed as long as being in scope. Once the resource is going out of scope, the destructor returns and thus frees the resource to claimed by others.

Parameters
[in]keyString identifier which command interface to claim
Returns
command interface

◆ claim_state_interface()

LoanedStateInterface hardware_interface::ResourceManager::claim_state_interface ( const std::string &  key)

Claim a state interface given its key.

The resource is claimed as long as being in scope. Once the resource is going out of scope, the destructor returns.

Parameters
[in]keyString identifier which state interface to claim
Returns
state interface

◆ command_interface_exists()

bool hardware_interface::ResourceManager::command_interface_exists ( const std::string &  key) const

Checks whether a command interface is registered under the given key.

Parameters
[in]keystring identifying the interface to check.
Returns
true if interface exist, false otherwise.

◆ command_interface_is_available()

bool hardware_interface::ResourceManager::command_interface_is_available ( const std::string &  interface) const

Checks whether a command interface is available under the given name.

Parameters
[in]namestring identifying the interface to check.
Returns
true if interface is available, false otherwise.

◆ command_interface_is_claimed()

bool hardware_interface::ResourceManager::command_interface_is_claimed ( const std::string &  key) const

Checks whether a command interface is already claimed.

Any command interface can only be claimed by a single instance.

Note
the equivalent function does not exist for state interfaces. These are solely read-only and can thus be used by multiple instances.
Parameters
[in]keystring identifying the interface to check.
Returns
true if interface is already claimed, false if available.

◆ command_interface_keys()

std::vector< std::string > hardware_interface::ResourceManager::command_interface_keys ( ) const

Returns all registered command interfaces keys.

The keys are collected from each loaded hardware component.

Returns
vector of strings, containing all registered keys.

◆ get_cached_controllers_to_hardware()

std::vector< std::string > hardware_interface::ResourceManager::get_cached_controllers_to_hardware ( const std::string &  hardware_name)

Return cached controllers for a specific hardware.

Return list of cached controller names that use the hardware with name hardware_name.

Parameters
[in]hardware_namethe name of the hardware for which cached controllers should be returned.
Returns
list of cached controller names that depend on hardware with name hardware_name.

◆ get_clock()

rclcpp::Clock::SharedPtr hardware_interface::ResourceManager::get_clock ( ) const
protected

Gets the clock for the resource manager.

Returns
clock of the resource manager

◆ get_components_status()

std::unordered_map< std::string, HardwareComponentInfo > hardware_interface::ResourceManager::get_components_status ( )

Return status for all components.

Returns
map of hardware names and their status.

◆ get_controller_exported_state_interface_names()

std::vector< std::string > hardware_interface::ResourceManager::get_controller_exported_state_interface_names ( const std::string &  controller_name)

Get list of exported tate interface of a controller.

Returns lists of stored exported state interfaces names for a controller.

Parameters
[in]controller_namefor which list of state interface names is returned.
Returns
list of reference interface names.

◆ get_controller_reference_interface_names()

std::vector< std::string > hardware_interface::ResourceManager::get_controller_reference_interface_names ( const std::string &  controller_name)

Get list of reference interface of a controller.

Returns lists of stored reference interfaces names for a controller.

Parameters
[in]controller_namefor which list of reference interface names is returned.
Returns
list of reference interface names.

◆ get_logger()

rclcpp::Logger hardware_interface::ResourceManager::get_logger ( ) const
protected

Gets the logger for the resource manager.

Returns
logger of the resource manager

◆ import_component() [1/3]

void hardware_interface::ResourceManager::import_component ( std::unique_ptr< ActuatorInterface actuator,
const HardwareInfo hardware_info 
)

Import a hardware component which is not listed in the URDF.

Components which are initialized outside a URDF can be added post initialization. Nevertheless, there should still be HardwareInfo available for this component, either parsed from a URDF string (easiest) or filled manually.

Note
this might invalidate existing state and command interfaces and should thus not be called when a controller is running.
given that no hardware_info is available, the component has to be configured externally and prior to the call to import.
Parameters
[in]actuatorpointer to the actuator interface.
[in]hardware_infohardware info

◆ import_component() [2/3]

void hardware_interface::ResourceManager::import_component ( std::unique_ptr< SensorInterface sensor,
const HardwareInfo hardware_info 
)

Import a hardware component which is not listed in the URDF.

Components which are initialized outside a URDF can be added post initialization. Nevertheless, there should still be HardwareInfo available for this component, either parsed from a URDF string (easiest) or filled manually.

Note
this might invalidate existing state and command interfaces and should thus not be called when a controller is running.
given that no hardware_info is available, the component has to be configured externally and prior to the call to import.
Parameters
[in]sensorpointer to the sensor interface.
[in]hardware_infohardware info

◆ import_component() [3/3]

void hardware_interface::ResourceManager::import_component ( std::unique_ptr< SystemInterface system,
const HardwareInfo hardware_info 
)

Import a hardware component which is not listed in the URDF.

Components which are initialized outside a URDF can be added post initialization. Nevertheless, there should still be HardwareInfo available for this component, either parsed from a URDF string (easiest) or filled manually.

Note
this might invalidate existing state and command interfaces and should thus not be called when a controller is running.
given that no hardware_info is available, the component has to be configured externally and prior to the call to import.
Parameters
[in]systempointer to the system interface.
[in]hardware_infohardware info

◆ import_controller_exported_state_interfaces()

void hardware_interface::ResourceManager::import_controller_exported_state_interfaces ( const std::string &  controller_name,
std::vector< StateInterface > &  interfaces 
)

Add controllers' exported state interfaces to resource manager.

Interface for transferring management of exported state interfaces to resource manager. When chaining controllers, state interfaces are used by the preceding controllers. Therefore, they should be managed in the same way as state interface of hardware.

Parameters
[in]controller_namename of the controller which state interfaces are imported.
[in]interfaceslist of controller's state interfaces as StateInterfaces.

◆ import_controller_reference_interfaces()

void hardware_interface::ResourceManager::import_controller_reference_interfaces ( const std::string &  controller_name,
std::vector< CommandInterface > &  interfaces 
)

Add controllers' reference interfaces to resource manager.

Interface for transferring management of reference interfaces to resource manager. When chaining controllers, reference interfaces are used as command interface of preceding controllers. Therefore, they should be managed in the same way as command interface of hardware.

Parameters
[in]controller_namename of the controller which reference interfaces are imported.
[in]interfaceslist of controller's reference interfaces as CommandInterfaces.

◆ load_and_initialize_components()

bool hardware_interface::ResourceManager::load_and_initialize_components ( const std::string &  urdf,
const unsigned int  update_rate = 100 
)
virtual

Load resources from on a given URDF.

The resource manager can be post-initialized with a given URDF. This is mainly used in conjunction with the default constructor in which the URDF might not be present at first initialization.

Parameters
[in]urdfstring containing the URDF.
[in]update_rateupdate rate of the main control loop, i.e., of the controller manager.
Returns
false if URDF validation has failed.

Reimplemented in gz_ros2_control::GZResourceManager, and gazebo_ros2_control::GazeboResourceManager.

◆ make_controller_exported_state_interfaces_available()

void hardware_interface::ResourceManager::make_controller_exported_state_interfaces_available ( const std::string &  controller_name)

Add controller's exported state interfaces to available list.

Adds state interfacess of a controller with given name to the available list. This method should be called when a controller gets activated with chained mode turned on. That means, the controller's exported state interfaces can be used by another controllers in chained architectures.

Parameters
[in]controller_namename of the controller which interfaces should become available.

◆ make_controller_exported_state_interfaces_unavailable()

void hardware_interface::ResourceManager::make_controller_exported_state_interfaces_unavailable ( const std::string &  controller_name)

Remove controller's exported state interface to available list.

Removes interfaces of a controller with given name from the available list. This method should be called when a controller gets deactivated and its reference interfaces cannot be used by another controller anymore.

Parameters
[in]controller_namename of the controller which interfaces should become unavailable.

◆ make_controller_reference_interfaces_available()

void hardware_interface::ResourceManager::make_controller_reference_interfaces_available ( const std::string &  controller_name)

Add controller's reference interface to available list.

Adds interfaces of a controller with given name to the available list. This method should be called when a controller gets activated with chained mode turned on. That means, the controller's reference interfaces can be used by another controller in chained architectures.

Parameters
[in]controller_namename of the controller which interfaces should become available.

◆ make_controller_reference_interfaces_unavailable()

void hardware_interface::ResourceManager::make_controller_reference_interfaces_unavailable ( const std::string &  controller_name)

Remove controller's reference interface to available list.

Removes interfaces of a controller with given name from the available list. This method should be called when a controller gets deactivated and its reference interfaces cannot be used by another controller anymore.

Parameters
[in]controller_namename of the controller which interfaces should become unavailable.

◆ perform_command_mode_switch()

bool hardware_interface::ResourceManager::perform_command_mode_switch ( const std::vector< std::string > &  start_interfaces,
const std::vector< std::string > &  stop_interfaces 
)

Notify the hardware components that realtime hardware mode switching should occur.

Hardware components are asked to perform the command interface mode switching.

Note
this is intended for mode-switching when a hardware interface needs to change control mode depending on which command interface is claimed.
this is for realtime switching of the command interface.
it is assumed that prepare_command_mode_switch is called just before this method with the same input arguments.
Parameters
[in]start_interfacesvector of string identifiers for the command interfaces starting.
[in]stop_interfacesvector of string identifiers for the command interfaces stopping.
Returns
true if switch is performed, false if a component rejects switching.

◆ prepare_command_mode_switch()

bool hardware_interface::ResourceManager::prepare_command_mode_switch ( const std::vector< std::string > &  start_interfaces,
const std::vector< std::string > &  stop_interfaces 
)

Prepare the hardware components for a new command interface mode.

Hardware components are asked to prepare a new command interface claim.

Note
this is intended for mode-switching when a hardware interface needs to change control mode depending on which command interface is claimed.
this is for non-realtime preparing for and accepting new command resource combinations.
accept_command_resource_claim is called on all actuators and system components and hardware interfaces should return hardware_interface::return_type::OK by default
Parameters
[in]start_interfacesvector of string identifiers for the command interfaces starting.
[in]stop_interfacesvector of string identifiers for the command interfaces stopping.
Returns
true if switch can be prepared; false if a component rejects switch request, and if at least one of the input interfaces are not existing or not available (i.e., component is not in ACTIVE or INACTIVE state).

◆ read()

HardwareReadWriteStatus hardware_interface::ResourceManager::read ( const rclcpp::Time &  time,
const rclcpp::Duration &  period 
)

Reads all loaded hardware components.

Reads from all active hardware components.

Part of the real-time critical update loop. It is realtime-safe if used hardware interfaces are implemented adequately.

◆ remove_controller_exported_state_interfaces()

void hardware_interface::ResourceManager::remove_controller_exported_state_interfaces ( const std::string &  controller_name)

Remove controllers exported state interfaces from resource manager.

Remove exported state interfaces from resource manager, i.e., resource storage. The interfaces will be deleted from all internal maps and lists.

Parameters
[in]controller_namelist of interface names that will be deleted from resource manager.

◆ remove_controller_reference_interfaces()

void hardware_interface::ResourceManager::remove_controller_reference_interfaces ( const std::string &  controller_name)

Remove controllers reference interfaces from resource manager.

Remove reference interfaces from resource manager, i.e., resource storage. The interfaces will be deleted from all internal maps and lists.

Parameters
[in]controller_namelist of interface names that will be deleted from resource manager.

◆ sensor_components_size()

size_t hardware_interface::ResourceManager::sensor_components_size ( ) const

Return the number of loaded sensor components.

Returns
number of sensor components.

◆ set_component_state()

return_type hardware_interface::ResourceManager::set_component_state ( const std::string &  component_name,
rclcpp_lifecycle::State &  target_state 
)

Sets state of hardware component.

Set set of hardware component if possible. Takes care of all transitions needed to reach the target state. It implements the state machine from: https://design.ros2.org/articles/node_lifecycle.html

The method is not part of the real-time critical update loop.

Parameters
[in]component_namecomponent name to change state.
[in]target_statetarget state to set for a hardware component.
Returns
hardware_interface::retun_type::OK if component successfully switched its state and hardware_interface::return_type::ERROR any of state transitions has failed.

◆ shutdown_async_components()

void hardware_interface::ResourceManager::shutdown_async_components ( )

Deletes all async components from the resource storage.

Needed to join the threads immediately after the control loop is ended.

◆ state_interface_exists()

bool hardware_interface::ResourceManager::state_interface_exists ( const std::string &  key) const

Checks whether a state interface is registered under the given key.

Returns
true if interface exist, false otherwise.

◆ state_interface_is_available()

bool hardware_interface::ResourceManager::state_interface_is_available ( const std::string &  name) const

Checks whether a state interface is available under the given key.

Returns
true if interface is available, false otherwise.

◆ state_interface_keys()

std::vector< std::string > hardware_interface::ResourceManager::state_interface_keys ( ) const

Returns all registered state interfaces keys.

The keys are collected from each loaded hardware component.

Returns
Vector of strings, containing all registered keys.

◆ system_components_size()

size_t hardware_interface::ResourceManager::system_components_size ( ) const

Return the number of loaded system components.

Returns
size_t number of system components.

◆ write()

HardwareReadWriteStatus hardware_interface::ResourceManager::write ( const rclcpp::Time &  time,
const rclcpp::Duration &  period 
)

Write all loaded hardware components.

Writes to all active hardware components.

Part of the real-time critical update loop. It is realtime-safe if used hardware interfaces are implemented adequately.


The documentation for this class was generated from the following files: