Classes | Public Member Functions | Static Public Attributes | Private Member Functions | Private Attributes | List of all members
controller_manager::ControllerManager Class Reference

ROS Controller Manager and Runner. More...

#include <controller_manager.h>

Classes

struct  SwitchParams
 

Public Member Functions

 ControllerManager (hardware_interface::RobotHW *robot_hw, const ros::NodeHandle &nh=ros::NodeHandle())
 Constructor. More...
 
virtual ~ControllerManager ()=default
 
Real-Time Safe Functions
void update (const ros::Time &time, const ros::Duration &period, bool reset_controllers=false)
 Update all active controllers. More...
 
Non Real-Time Safe Functions
bool loadController (const std::string &name)
 Load a new controller by name. More...
 
bool unloadController (const std::string &name)
 Unload a controller by name. More...
 
bool switchController (const std::vector< std::string > &start_controllers, const std::vector< std::string > &stop_controllers, const int strictness, bool start_asap=WAIT_FOR_ALL_RESOURCES, double timeout=INFINITE_TIMEOUT)
 Switch multiple controllers simultaneously. More...
 
virtual controller_interface::ControllerBasegetControllerByName (const std::string &name)
 Get a controller by name. More...
 
void registerControllerLoader (ControllerLoaderInterfaceSharedPtr controller_loader)
 Register a controller loader. More...
 

Static Public Attributes

static constexpr double INFINITE_TIMEOUT = 0.0
 
static constexpr bool WAIT_FOR_ALL_RESOURCES = false
 

Private Member Functions

void getControllerNames (std::vector< std::string > &v)
 
void manageSwitch (const ros::Time &time)
 
void startControllers (const ros::Time &time)
 
void startControllersAsap (const ros::Time &time)
 
void stopControllers (const ros::Time &time)
 

Private Attributes

ros::NodeHandle cm_node_
 
std::list< ControllerLoaderInterfaceSharedPtrcontroller_loaders_
 
hardware_interface::RobotHWrobot_hw_
 
ros::NodeHandle root_nh_
 
Controller Switching
std::vector< controller_interface::ControllerBase * > start_request_
 
std::vector< controller_interface::ControllerBase * > stop_request_
 
std::list< hardware_interface::ControllerInfoswitch_start_list_
 
std::list< hardware_interface::ControllerInfoswitch_stop_list_
 
SwitchParams switch_params_
 
Controllers List

The controllers list is double-buffered to avoid needing to lock the real-time thread when switching controllers in the non-real-time thread.

std::recursive_mutex controllers_lock_
 Mutex protecting the current controllers list. More...
 
std::vector< ControllerSpeccontrollers_lists_ [2]
 Double-buffered controllers list. More...
 
int current_controllers_list_ = {0}
 The index of the current controllers list. More...
 
int used_by_realtime_ = {-1}
 The index of the controllers list being used in the real-time thread. More...
 

ROS Service API

std::mutex services_lock_
 
ros::ServiceServer srv_list_controllers_
 
ros::ServiceServer srv_list_controller_types_
 
ros::ServiceServer srv_load_controller_
 
ros::ServiceServer srv_unload_controller_
 
ros::ServiceServer srv_switch_controller_
 
ros::ServiceServer srv_reload_libraries_
 
bool listControllerTypesSrv (controller_manager_msgs::ListControllerTypes::Request &req, controller_manager_msgs::ListControllerTypes::Response &resp)
 
bool listControllersSrv (controller_manager_msgs::ListControllers::Request &req, controller_manager_msgs::ListControllers::Response &resp)
 
bool switchControllerSrv (controller_manager_msgs::SwitchController::Request &req, controller_manager_msgs::SwitchController::Response &resp)
 
bool loadControllerSrv (controller_manager_msgs::LoadController::Request &req, controller_manager_msgs::LoadController::Response &resp)
 
bool unloadControllerSrv (controller_manager_msgs::UnloadController::Request &req, controller_manager_msgs::UnloadController::Response &resp)
 
bool reloadControllerLibrariesSrv (controller_manager_msgs::ReloadControllerLibraries::Request &req, controller_manager_msgs::ReloadControllerLibraries::Response &resp)
 

Detailed Description

ROS Controller Manager and Runner.

This class advertises a ROS interface for loading, unloading, starting, and stopping ros_control-based controllers. It also serializes execution of all running controllers in update.

Definition at line 64 of file controller_manager.h.

Constructor & Destructor Documentation

◆ ControllerManager()

controller_manager::ControllerManager::ControllerManager ( hardware_interface::RobotHW robot_hw,
const ros::NodeHandle nh = ros::NodeHandle() 
)

Constructor.

Parameters
robot_hwA pointer to a robot hardware interface
nhThe ros::NodeHandle in whose namespace all ROS interfaces should operate.

Definition at line 45 of file controller_manager.cpp.

◆ ~ControllerManager()

virtual controller_manager::ControllerManager::~ControllerManager ( )
virtualdefault

Member Function Documentation

◆ getControllerByName()

controller_interface::ControllerBase * controller_manager::ControllerManager::getControllerByName ( const std::string &  name)
virtual

Get a controller by name.

Parameters
nameThe name of a controller
Returns
An up-casted pointer to the controller identified by name

Definition at line 92 of file controller_manager.cpp.

◆ getControllerNames()

void controller_manager::ControllerManager::getControllerNames ( std::vector< std::string > &  v)
private

Definition at line 105 of file controller_manager.cpp.

◆ listControllersSrv()

bool controller_manager::ControllerManager::listControllersSrv ( controller_manager_msgs::ListControllers::Request &  req,
controller_manager_msgs::ListControllers::Response &  resp 
)
private

Definition at line 715 of file controller_manager.cpp.

◆ listControllerTypesSrv()

bool controller_manager::ControllerManager::listControllerTypesSrv ( controller_manager_msgs::ListControllerTypes::Request &  req,
controller_manager_msgs::ListControllerTypes::Response &  resp 
)
private

Definition at line 688 of file controller_manager.cpp.

◆ loadController()

bool controller_manager::ControllerManager::loadController ( const std::string &  name)

Load a new controller by name.

This dynamically loads a controller called name and initializes the newly loaded controller.

It determines the controller type by accessing the ROS parameter "type" in the namespace given by name relative to the namespace of root_nh_. It then initializes the controller with the hardware_interface::RobotHW pointer robot_hw_, the ros::NodeHandle describing this namespace, and a reference to a std::set to retrieve the resources needed by this controller.

A controller cannot be loaded while already loaded. To re-load a controller, first unloadController and then loadController.

Parameters
nameThe name of the controller as well as the ROS namespace under which the controller should be loaded
Returns
True on success
False on failure

Definition at line 229 of file controller_manager.cpp.

◆ loadControllerSrv()

bool controller_manager::ControllerManager::loadControllerSrv ( controller_manager_msgs::LoadController::Request &  req,
controller_manager_msgs::LoadController::Response &  resp 
)
private

Definition at line 782 of file controller_manager.cpp.

◆ manageSwitch()

void controller_manager::ControllerManager::manageSwitch ( const ros::Time time)
private

Definition at line 115 of file controller_manager.cpp.

◆ registerControllerLoader()

void controller_manager::ControllerManager::registerControllerLoader ( ControllerLoaderInterfaceSharedPtr  controller_loader)

Register a controller loader.

By default, the pluginlib-based ControllerLoader is registered on construction of this class. To load controllers through alternate means, register alternate controller loaders here. Note, however, that when controllers are loaded by loadController the controller loaders are queried in the order that they were registered. This means that if a controller CAN be loaded by the pluginlib-based ControllerLoader, then it WILL, regardless of which other loaders are registered.

Parameters
controller_loaderA pointer to the loader to be registered

Definition at line 830 of file controller_manager.cpp.

◆ reloadControllerLibrariesSrv()

bool controller_manager::ControllerManager::reloadControllerLibrariesSrv ( controller_manager_msgs::ReloadControllerLibraries::Request &  req,
controller_manager_msgs::ReloadControllerLibraries::Response &  resp 
)
private

Definition at line 635 of file controller_manager.cpp.

◆ startControllers()

void controller_manager::ControllerManager::startControllers ( const ros::Time time)
private

Definition at line 150 of file controller_manager.cpp.

◆ startControllersAsap()

void controller_manager::ControllerManager::startControllersAsap ( const ros::Time time)
private

Definition at line 184 of file controller_manager.cpp.

◆ stopControllers()

void controller_manager::ControllerManager::stopControllers ( const ros::Time time)
private

Definition at line 138 of file controller_manager.cpp.

◆ switchController()

bool controller_manager::ControllerManager::switchController ( const std::vector< std::string > &  start_controllers,
const std::vector< std::string > &  stop_controllers,
const int  strictness,
bool  start_asap = WAIT_FOR_ALL_RESOURCES,
double  timeout = INFINITE_TIMEOUT 
)

Switch multiple controllers simultaneously.

Parameters
start_controllersA vector of controller names to be started
stop_controllersA vector of controller names to be stopped
strictnessHow important it is that the requested controllers are started and stopped. The levels are defined in the controller_manager_msgs/SwitchControllers service as either BEST_EFFORT or STRICT. BEST_EFFORT means that switchController can still succeed if a non-existent controller is requested to be stopped or started.
start_asapStart the controllers as soon as their resources are ready, will wait for all resources to be ready otherwise.
timeoutThe timeout in seconds before aborting pending controllers. Zero for infinite.

Definition at line 436 of file controller_manager.cpp.

◆ switchControllerSrv()

bool controller_manager::ControllerManager::switchControllerSrv ( controller_manager_msgs::SwitchController::Request &  req,
controller_manager_msgs::SwitchController::Response &  resp 
)
private

Definition at line 814 of file controller_manager.cpp.

◆ unloadController()

bool controller_manager::ControllerManager::unloadController ( const std::string &  name)

Unload a controller by name.

Parameters
nameThe name of the controller to unload. (The same as the one used in loadController )

Definition at line 367 of file controller_manager.cpp.

◆ unloadControllerSrv()

bool controller_manager::ControllerManager::unloadControllerSrv ( controller_manager_msgs::UnloadController::Request &  req,
controller_manager_msgs::UnloadController::Response &  resp 
)
private

Definition at line 798 of file controller_manager.cpp.

◆ update()

void controller_manager::ControllerManager::update ( const ros::Time time,
const ros::Duration period,
bool  reset_controllers = false 
)

Update all active controllers.

When controllers are started or stopped (or switched), those calls are made in this function.

Parameters
timeThe current time
periodThe change in time since the last call to update
reset_controllersIf true, stop and start all running controllers before updating

Definition at line 66 of file controller_manager.cpp.

Member Data Documentation

◆ cm_node_

ros::NodeHandle controller_manager::ControllerManager::cm_node_
private

Definition at line 182 of file controller_manager.h.

◆ controller_loaders_

std::list<ControllerLoaderInterfaceSharedPtr> controller_manager::ControllerManager::controller_loaders_
private

Definition at line 184 of file controller_manager.h.

◆ controllers_lists_

std::vector<ControllerSpec> controller_manager::ControllerManager::controllers_lists_[2]
private

Double-buffered controllers list.

Definition at line 214 of file controller_manager.h.

◆ controllers_lock_

std::recursive_mutex controller_manager::ControllerManager::controllers_lock_
private

Mutex protecting the current controllers list.

Definition at line 212 of file controller_manager.h.

◆ current_controllers_list_

int controller_manager::ControllerManager::current_controllers_list_ = {0}
private

The index of the current controllers list.

Definition at line 216 of file controller_manager.h.

◆ INFINITE_TIMEOUT

constexpr double controller_manager::ControllerManager::INFINITE_TIMEOUT = 0.0
static

Definition at line 68 of file controller_manager.h.

◆ robot_hw_

hardware_interface::RobotHW* controller_manager::ControllerManager::robot_hw_
private

Definition at line 180 of file controller_manager.h.

◆ root_nh_

ros::NodeHandle controller_manager::ControllerManager::root_nh_
private

Definition at line 182 of file controller_manager.h.

◆ services_lock_

std::mutex controller_manager::ControllerManager::services_lock_
private

Definition at line 236 of file controller_manager.h.

◆ srv_list_controller_types_

ros::ServiceServer controller_manager::ControllerManager::srv_list_controller_types_
private

Definition at line 237 of file controller_manager.h.

◆ srv_list_controllers_

ros::ServiceServer controller_manager::ControllerManager::srv_list_controllers_
private

Definition at line 237 of file controller_manager.h.

◆ srv_load_controller_

ros::ServiceServer controller_manager::ControllerManager::srv_load_controller_
private

Definition at line 237 of file controller_manager.h.

◆ srv_reload_libraries_

ros::ServiceServer controller_manager::ControllerManager::srv_reload_libraries_
private

Definition at line 238 of file controller_manager.h.

◆ srv_switch_controller_

ros::ServiceServer controller_manager::ControllerManager::srv_switch_controller_
private

Definition at line 238 of file controller_manager.h.

◆ srv_unload_controller_

ros::ServiceServer controller_manager::ControllerManager::srv_unload_controller_
private

Definition at line 238 of file controller_manager.h.

◆ start_request_

std::vector<controller_interface::ControllerBase*> controller_manager::ControllerManager::start_request_
private

Definition at line 188 of file controller_manager.h.

◆ stop_request_

std::vector<controller_interface::ControllerBase*> controller_manager::ControllerManager::stop_request_
private

Definition at line 188 of file controller_manager.h.

◆ switch_params_

SwitchParams controller_manager::ControllerManager::switch_params_
private

Definition at line 203 of file controller_manager.h.

◆ switch_start_list_

std::list<hardware_interface::ControllerInfo> controller_manager::ControllerManager::switch_start_list_
private

Definition at line 189 of file controller_manager.h.

◆ switch_stop_list_

std::list<hardware_interface::ControllerInfo> controller_manager::ControllerManager::switch_stop_list_
private

Definition at line 189 of file controller_manager.h.

◆ used_by_realtime_

int controller_manager::ControllerManager::used_by_realtime_ = {-1}
private

The index of the controllers list being used in the real-time thread.

Definition at line 218 of file controller_manager.h.

◆ WAIT_FOR_ALL_RESOURCES

constexpr bool controller_manager::ControllerManager::WAIT_FOR_ALL_RESOURCES = false
static

Definition at line 67 of file controller_manager.h.


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


controller_manager
Author(s): Wim Meeussen, Mathias Lüdtke
autogenerated on Mon Feb 28 2022 23:30:17