Public Member Functions | |
ControllerManager (std::unique_ptr< hardware_interface::ResourceManager > resource_manager, std::shared_ptr< rclcpp::Executor > executor, const std::string &manager_node_name="controller_manager", const std::string &node_namespace="", const rclcpp::NodeOptions &options=get_cm_node_options()) | |
ControllerManager (std::shared_ptr< rclcpp::Executor > executor, const std::string &manager_node_name="controller_manager", const std::string &node_namespace="", const rclcpp::NodeOptions &options=get_cm_node_options()) | |
ControllerManager (std::shared_ptr< rclcpp::Executor > executor, const std::string &urdf, bool activate_all_hw_components, const std::string &manager_node_name="controller_manager", const std::string &node_namespace="", const rclcpp::NodeOptions &options=get_cm_node_options()) | |
bool | shutdown_controllers () |
Shutdown all controllers in the controller manager. | |
void | robot_description_callback (const std_msgs::msg::String &msg) |
void | init_resource_manager (const std::string &robot_description) |
controller_interface::ControllerInterfaceBaseSharedPtr | load_controller (const std::string &controller_name, const std::string &controller_type) |
controller_interface::ControllerInterfaceBaseSharedPtr | load_controller (const std::string &controller_name) |
load_controller loads a controller by name, the type must be defined in the parameter server. | |
controller_interface::return_type | unload_controller (const std::string &controller_name) |
std::vector< ControllerSpec > | get_loaded_controllers () const |
template<typename T , typename std::enable_if< std::is_convertible< T *, controller_interface::ControllerInterfaceBase * >::value, T >::type * = nullptr> | |
controller_interface::ControllerInterfaceBaseSharedPtr | add_controller (std::shared_ptr< T > controller, const std::string &controller_name, const std::string &controller_type) |
controller_interface::ControllerInterfaceBaseSharedPtr | add_controller (const ControllerSpec &controller_spec) |
controller_interface::return_type | configure_controller (const std::string &controller_name) |
configure_controller Configure controller by name calling their "configure" method. | |
controller_interface::return_type | switch_controller (const std::vector< std::string > &activate_controllers, const std::vector< std::string > &deactivate_controllers, int strictness, bool activate_asap=kWaitForAllResources, const rclcpp::Duration &timeout=rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)) |
switch_controller Deactivates some controllers and activates others. | |
controller_interface::return_type | switch_controller_cb (const std::vector< std::string > &activate_controllers, const std::vector< std::string > &deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration &timeout, std::string &message) |
switch_controller Deactivates some controllers and activates others. | |
void | read (const rclcpp::Time &time, const rclcpp::Duration &period) |
Read values to state interfaces. | |
controller_interface::return_type | update (const rclcpp::Time &time, const rclcpp::Duration &period) |
Run update on controllers. | |
void | write (const rclcpp::Time &time, const rclcpp::Duration &period) |
Write values from command interfaces. | |
bool | is_resource_manager_initialized () const |
Deterministic (real-time safe) callback group, e.g., update function. | |
unsigned int | get_update_rate () const |
Update rate of the main control loop in the controller manager. | |
rclcpp::Clock::SharedPtr | get_trigger_clock () const |
Get the trigger clock of the controller manager. | |
Static Public Attributes | |
static constexpr bool | kWaitForAllResources = false |
static constexpr auto | kInfiniteTimeout = 0 |
Protected Member Functions | |
void | init_services () |
controller_interface::ControllerInterfaceBaseSharedPtr | add_controller_impl (const ControllerSpec &controller) |
void | manage_switch () |
void | deactivate_controllers (const std::vector< ControllerSpec > &rt_controller_list, const std::vector< std::string > controllers_to_deactivate) |
Deactivate chosen controllers from real-time controller list. | |
void | switch_chained_mode (const std::vector< std::string > &chained_mode_switch_list, bool to_chained_mode) |
void | activate_controllers (const std::vector< ControllerSpec > &rt_controller_list, const std::vector< std::string > controllers_to_activate) |
Activate chosen controllers from real-time controller list. | |
void | activate_controllers_asap (const std::vector< ControllerSpec > &rt_controller_list, const std::vector< std::string > controllers_to_activate) |
Activate chosen controllers from real-time controller list. | |
void | list_controllers_srv_cb (const std::shared_ptr< controller_manager_msgs::srv::ListControllers::Request > request, std::shared_ptr< controller_manager_msgs::srv::ListControllers::Response > response) |
void | list_hardware_interfaces_srv_cb (const std::shared_ptr< controller_manager_msgs::srv::ListHardwareInterfaces::Request > request, std::shared_ptr< controller_manager_msgs::srv::ListHardwareInterfaces::Response > response) |
void | load_controller_service_cb (const std::shared_ptr< controller_manager_msgs::srv::LoadController::Request > request, std::shared_ptr< controller_manager_msgs::srv::LoadController::Response > response) |
void | configure_controller_service_cb (const std::shared_ptr< controller_manager_msgs::srv::ConfigureController::Request > request, std::shared_ptr< controller_manager_msgs::srv::ConfigureController::Response > response) |
void | reload_controller_libraries_service_cb (const std::shared_ptr< controller_manager_msgs::srv::ReloadControllerLibraries::Request > request, std::shared_ptr< controller_manager_msgs::srv::ReloadControllerLibraries::Response > response) |
void | switch_controller_service_cb (const std::shared_ptr< controller_manager_msgs::srv::SwitchController::Request > request, std::shared_ptr< controller_manager_msgs::srv::SwitchController::Response > response) |
void | unload_controller_service_cb (const std::shared_ptr< controller_manager_msgs::srv::UnloadController::Request > request, std::shared_ptr< controller_manager_msgs::srv::UnloadController::Response > response) |
void | list_controller_types_srv_cb (const std::shared_ptr< controller_manager_msgs::srv::ListControllerTypes::Request > request, std::shared_ptr< controller_manager_msgs::srv::ListControllerTypes::Response > response) |
void | list_hardware_components_srv_cb (const std::shared_ptr< controller_manager_msgs::srv::ListHardwareComponents::Request > request, std::shared_ptr< controller_manager_msgs::srv::ListHardwareComponents::Response > response) |
void | set_hardware_component_state_srv_cb (const std::shared_ptr< controller_manager_msgs::srv::SetHardwareComponentState::Request > request, std::shared_ptr< controller_manager_msgs::srv::SetHardwareComponentState::Response > response) |
Protected Attributes | |
unsigned int | update_loop_counter_ = 0 |
unsigned int | update_rate_ |
std::vector< std::vector< std::string > > | chained_controllers_configuration_ |
std::unique_ptr< hardware_interface::ResourceManager > | resource_manager_ |
|
protected |
Activate chosen controllers from real-time controller list.
Activate controllers with names controllers_to_activate
from list rt_controller_list
. The controller list will be iterated as many times as there are controller names.
[in] | rt_controller_list | controllers in the real-time list. |
[in] | controllers_to_activate | names of the controller that have to be activated. |
|
protected |
Activate chosen controllers from real-time controller list.
Activate controllers with names controllers_to_activate
from list rt_controller_list
. The controller list will be iterated as many times as there are controller names.
NOTE: There is currently not difference to activate_controllers
method. Check https://github.com/ros-controls/ros2_control/issues/263 for more information.
[in] | rt_controller_list | controllers in the real-time list. |
[in] | controllers_to_activate | names of the controller that have to be activated. |
controller_interface::return_type controller_manager::ControllerManager::configure_controller | ( | const std::string & | controller_name | ) |
configure_controller Configure controller by name calling their "configure" method.
[in] | controller_name | as a string. |
|
protected |
Deactivate chosen controllers from real-time controller list.
Deactivate controllers with names controllers_to_deactivate
from list rt_controller_list
. The controller list will be iterated as many times as there are controller names.
[in] | rt_controller_list | controllers in the real-time list. |
[in] | controllers_to_deactivate | names of the controller that have to be deactivated. |
rclcpp::Clock::SharedPtr controller_manager::ControllerManager::get_trigger_clock | ( | ) | const |
Get the trigger clock of the controller manager.
Get the trigger clock of the controller manager. The method is used to get the clock that is used for triggering the controllers and the hardware components.
unsigned int controller_manager::ControllerManager::get_update_rate | ( | ) | const |
Update rate of the main control loop in the controller manager.
Update rate of the main control loop in the controller manager. The method is used for per-controller update rate support.
|
inline |
Deterministic (real-time safe) callback group, e.g., update function.
Deterministic (real-time safe) callback group for the update function. Default behavior is read hardware, update controller and finally write new values to the hardware. Interface for external components to check if Resource Manager is initialized. Checks if components in Resource Manager are loaded and initialized.
controller_interface::ControllerInterfaceBaseSharedPtr controller_manager::ControllerManager::load_controller | ( | const std::string & | controller_name | ) |
load_controller loads a controller by name, the type must be defined in the parameter server.
[in] | controller_name | as a string. |
void controller_manager::ControllerManager::read | ( | const rclcpp::Time & | time, |
const rclcpp::Duration & | period | ||
) |
Read values to state interfaces.
Read current values from hardware to state interfaces. The method called in the (real-time) control loop.
[in] | time | The time at the start of this control loop iteration |
[in] | period | The measured period of the last control loop iteration |
bool controller_manager::ControllerManager::shutdown_controllers | ( | ) |
Shutdown all controllers in the controller manager.
|
protected |
Switch chained mode for all the controllers with respect to the following cases:
[in] | chained_mode_switch_list | list of controller to switch chained mode. |
[in] | to_chained_mode | flag if controller should be switched to or from chained mode. |
controller_interface::return_type controller_manager::ControllerManager::switch_controller | ( | const std::vector< std::string > & | activate_controllers, |
const std::vector< std::string > & | deactivate_controllers, | ||
int | strictness, | ||
bool | activate_asap = kWaitForAllResources , |
||
const rclcpp::Duration & | timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout) |
||
) |
switch_controller Deactivates some controllers and activates others.
[in] | activate_controllers | is a list of controllers to activate. |
[in] | deactivate_controllers | is a list of controllers to deactivate. |
[in] | set | level of strictness (BEST_EFFORT or STRICT) |
[in] | activate_asap | flag to activate controllers as soon as possible. |
[in] | timeout | to wait for the controllers to be switched. |
controller_interface::return_type controller_manager::ControllerManager::switch_controller_cb | ( | const std::vector< std::string > & | activate_controllers, |
const std::vector< std::string > & | deactivate_controllers, | ||
int | strictness, | ||
bool | activate_asap, | ||
const rclcpp::Duration & | timeout, | ||
std::string & | message | ||
) |
switch_controller Deactivates some controllers and activates others.
[in] | activate_controllers | is a list of controllers to activate. |
[in] | deactivate_controllers | is a list of controllers to deactivate. |
[in] | set | level of strictness (BEST_EFFORT or STRICT) |
[in] | activate_asap | flag to activate controllers as soon as possible. |
[in] | timeout | to wait for the controllers to be switched. |
[out] | message | describing the result of the switch. |
controller_interface::return_type controller_manager::ControllerManager::update | ( | const rclcpp::Time & | time, |
const rclcpp::Duration & | period | ||
) |
Run update on controllers.
Call update of all controllers. The method called in the (real-time) control loop.
[in] | time | The time at the start of this control loop iteration |
[in] | period | The measured period of the last control loop iteration |
void controller_manager::ControllerManager::write | ( | const rclcpp::Time & | time, |
const rclcpp::Duration & | period | ||
) |
Write values from command interfaces.
Write values from command interface into hardware. The method called in the (real-time) control loop.
[in] | time | The time at the start of this control loop iteration |
[in] | period | The measured period of the last control loop iteration |