ros2_control - iron
|
Public Member Functions | |
ResourceManager (unsigned int update_rate=100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface=nullptr) | |
Default constructor for the Resource Manager. | |
ResourceManager (const std::string &urdf, bool validate_interfaces=true, bool activate_all=false, unsigned int update_rate=100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface=nullptr) | |
Constructor for the Resource Manager. | |
ResourceManager (const ResourceManager &)=delete | |
void | load_urdf (const std::string &urdf, bool validate_interfaces=true, bool load_and_initialize_components=true) |
Load resources from on a given URDF. | |
bool | is_urdf_already_loaded () const |
if the resource manager load_urdf(...) function has been called this returns true. We want to permit to load the urdf later on but we currently don't want to permit multiple calls to load_urdf (reloading/loading different urdf). | |
LoanedStateInterface | claim_state_interface (const std::string &key) |
Claim a state interface given its key. | |
std::vector< std::string > | state_interface_keys () const |
Returns all registered state interfaces keys. | |
std::vector< std::string > | available_state_interfaces () const |
Returns all available state interfaces keys. | |
bool | state_interface_is_available (const std::string &name) const |
Checks whether a state interface is available under the given key. | |
void | import_controller_reference_interfaces (const std::string &controller_name, std::vector< CommandInterface > &interfaces) |
Add controllers' reference interfaces to resource manager. | |
std::vector< std::string > | get_controller_reference_interface_names (const std::string &controller_name) |
Get list of reference interface of a controller. | |
void | make_controller_reference_interfaces_available (const std::string &controller_name) |
Add controller's reference interface to available list. | |
void | make_controller_reference_interfaces_unavailable (const std::string &controller_name) |
Remove controller's reference interface to available list. | |
void | remove_controller_reference_interfaces (const std::string &controller_name) |
Remove controllers reference interfaces from resource manager. | |
void | cache_controller_to_hardware (const std::string &controller_name, const std::vector< std::string > &interfaces) |
Cache mapping between hardware and controllers using it. | |
std::vector< std::string > | get_cached_controllers_to_hardware (const std::string &hardware_name) |
Return cached controllers for a specific hardware. | |
bool | command_interface_is_claimed (const std::string &key) const |
Checks whether a command interface is already claimed. | |
LoanedCommandInterface | claim_command_interface (const std::string &key) |
Claim a command interface given its key. | |
std::vector< std::string > | command_interface_keys () const |
Returns all registered command interfaces keys. | |
std::vector< std::string > | available_command_interfaces () const |
Returns all available command interfaces keys. | |
bool | command_interface_is_available (const std::string &interface) const |
Checks whether a command interface is available under the given name. | |
size_t | actuator_components_size () const |
Return the number size_t of loaded actuator components. | |
size_t | sensor_components_size () const |
Return the number of loaded sensor components. | |
size_t | system_components_size () const |
Return the number of loaded system components. | |
void | import_component (std::unique_ptr< ActuatorInterface > actuator, const HardwareInfo &hardware_info) |
Import a hardware component which is not listed in the URDF. | |
void | import_component (std::unique_ptr< SensorInterface > sensor, const HardwareInfo &hardware_info) |
Import a hardware component which is not listed in the URDF. | |
void | import_component (std::unique_ptr< SystemInterface > system, const HardwareInfo &hardware_info) |
Import a hardware component which is not listed in the URDF. | |
std::unordered_map< std::string, HardwareComponentInfo > | get_components_status () |
Return status for all components. | |
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. | |
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. | |
return_type | set_component_state (const std::string &component_name, rclcpp_lifecycle::State &target_state) |
Sets state of hardware component. | |
HardwareReadWriteStatus | read (const rclcpp::Time &time, const rclcpp::Duration &period) |
Reads all loaded hardware components. | |
HardwareReadWriteStatus | write (const rclcpp::Time &time, const rclcpp::Duration &period) |
Write all loaded hardware components. | |
bool | command_interface_exists (const std::string &key) const |
Checks whether a command interface is registered under the given key. | |
bool | state_interface_exists (const std::string &key) const |
Checks whether a state interface is registered under the given key. | |
|
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.
If the interfaces ought to be validated, the constructor throws an exception in case the URDF lists interfaces which are not available.
[in] | urdf | string containing the URDF. |
[in] | validate_interfaces | boolean argument indicating whether the exported interfaces ought to be validated. Defaults to true. |
[in] | activate_all | boolean argument indicating if all resources should be immediately activated. Currently used only in tests. |
size_t hardware_interface::ResourceManager::actuator_components_size | ( | ) | const |
Return the number size_t of loaded actuator components.
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.
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.
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.
[in] | controller_name | name of the controller which interfaces are provided. |
[in] | interfaces | list of interfaces controller with controller_name is using. |
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.
[in] | key | String identifier which command interface to claim |
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.
[in] | key | String identifier which state interface to claim |
bool hardware_interface::ResourceManager::command_interface_exists | ( | const std::string & | key | ) | const |
Checks whether a command interface is registered under the given key.
[in] | key | string identifying the interface to check. |
bool hardware_interface::ResourceManager::command_interface_is_available | ( | const std::string & | interface | ) | const |
Checks whether a command interface is available under the given name.
[in] | name | string identifying the interface to check. |
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.
[in] | key | string identifying the interface to check. |
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.
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
.
[in] | hardware_name | the name of the hardware for which cached controllers should be returned. |
hardware_name
. std::unordered_map< std::string, HardwareComponentInfo > hardware_interface::ResourceManager::get_components_status | ( | ) |
Return status for all components.
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.
[in] | controller_name | for which list of reference interface names is returned. |
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.
[in] | actuator | pointer to the actuator interface. |
[in] | hardware_info | hardware info |
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.
[in] | sensor | pointer to the sensor interface. |
[in] | hardware_info | hardware info |
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.
[in] | system | pointer to the system interface. |
[in] | hardware_info | hardware info |
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.
[in] | controller_name | name of the controller which reference interfaces are imported. |
[in] | interfaces | list of controller's reference interfaces as CommandInterfaces. |
bool hardware_interface::ResourceManager::is_urdf_already_loaded | ( | ) | const |
if the resource manager load_urdf(...) function has been called this returns true. We want to permit to load the urdf later on but we currently don't want to permit multiple calls to load_urdf (reloading/loading different urdf).
void hardware_interface::ResourceManager::load_urdf | ( | const std::string & | urdf, |
bool | validate_interfaces = true , |
||
bool | load_and_initialize_components = true |
||
) |
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.
[in] | urdf | string containing the URDF. |
[in] | validate_interfaces | boolean argument indicating whether the exported interfaces ought to be validated. Defaults to true. |
[in] | load_and_initialize_components | boolean argument indicating whether to load and initialize the components present in the parsed URDF. Defaults to true. |
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.
[in] | controller_name | name of the controller which interfaces should become available. |
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.
[in] | controller_name | name of the controller which interfaces should become unavailable. |
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.
prepare_command_mode_switch
is called just before this method with the same input arguments. [in] | start_interfaces | vector of string identifiers for the command interfaces starting. |
[in] | stop_interfaces | vector of string identifiers for the command interfaces stopping. |
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.
[in] | start_interfaces | vector of string identifiers for the command interfaces starting. |
[in] | stop_interfaces | vector of string identifiers for the command interfaces stopping. |
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.
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.
[in] | controller_name | list of interface names that will be deleted from resource manager. |
size_t hardware_interface::ResourceManager::sensor_components_size | ( | ) | const |
Return the number of loaded sensor components.
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.
[in] | component_name | component name to change state. |
[in] | target_state | target state to set for a hardware component. |
bool hardware_interface::ResourceManager::state_interface_exists | ( | const std::string & | key | ) | const |
Checks whether a state interface is registered under the given key.
bool hardware_interface::ResourceManager::state_interface_is_available | ( | const std::string & | name | ) | const |
Checks whether a state interface is available under the given key.
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.
size_t hardware_interface::ResourceManager::system_components_size | ( | ) | const |
Return the number of loaded system components.
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.