ROS Controller Manager and Runner. More...
#include <controller_manager.h>
Public Member Functions | |
ControllerManager (hardware_interface::RobotHW *robot_hw, const ros::NodeHandle &nh=ros::NodeHandle()) | |
Constructor. | |
virtual | ~ControllerManager () |
Real-Time Safe Functions | |
void | update (const ros::Time &time, const ros::Duration &period, bool reset_controllers=false) |
Update all active controllers. | |
Non Real-Time Safe Functions | |
bool | loadController (const std::string &name) |
Load a new controller by name. | |
bool | unloadController (const std::string &name) |
Unload a controller by name. | |
bool | switchController (const std::vector< std::string > &start_controllers, const std::vector< std::string > &stop_controllers, const int strictness) |
Switch multiple controllers simultaneously. | |
virtual controller_interface::ControllerBase * | getControllerByName (const std::string &name) |
Get a controller by name. | |
void | registerControllerLoader (ControllerLoaderInterfaceSharedPtr controller_loader) |
Register a controller loader. | |
Private Member Functions | |
void | getControllerNames (std::vector< std::string > &v) |
Private Attributes | |
ros::NodeHandle | cm_node_ |
std::list < ControllerLoaderInterfaceSharedPtr > | controller_loaders_ |
hardware_interface::RobotHW * | robot_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::ControllerInfo > | switch_start_list_ |
std::list < hardware_interface::ControllerInfo > | switch_stop_list_ |
bool | please_switch_ |
int | switch_strictness_ |
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. | |
boost::recursive_mutex | controllers_lock_ |
Mutex protecting the current controllers list. | |
std::vector< ControllerSpec > | controllers_lists_ [2] |
Double-buffered controllers list. | |
int | current_controllers_list_ |
The index of the current controllers list. | |
int | used_by_realtime_ |
The index of the controllers list being used in the real-time thread. | |
ROS Service API | |
boost::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) |
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 69 of file controller_manager.h.
controller_manager::ControllerManager::ControllerManager | ( | hardware_interface::RobotHW * | robot_hw, |
const ros::NodeHandle & | nh = ros::NodeHandle() |
||
) |
Constructor.
robot_hw | A pointer to a robot hardware interface |
nh | The ros::NodeHandle in whose namespace all ROS interfaces should operate. |
Definition at line 43 of file controller_manager.cpp.
Definition at line 68 of file controller_manager.cpp.
controller_interface::ControllerBase * controller_manager::ControllerManager::getControllerByName | ( | const std::string & | name | ) | [virtual] |
Get a controller by name.
name | The name of a controller |
name
Definition at line 115 of file controller_manager.cpp.
void controller_manager::ControllerManager::getControllerNames | ( | std::vector< std::string > & | v | ) | [private] |
Definition at line 129 of file controller_manager.cpp.
bool controller_manager::ControllerManager::listControllersSrv | ( | controller_manager_msgs::ListControllers::Request & | req, |
controller_manager_msgs::ListControllers::Response & | resp | ||
) | [private] |
Definition at line 603 of file controller_manager.cpp.
bool controller_manager::ControllerManager::listControllerTypesSrv | ( | controller_manager_msgs::ListControllerTypes::Request & | req, |
controller_manager_msgs::ListControllerTypes::Response & | resp | ||
) | [private] |
Definition at line 576 of file controller_manager.cpp.
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.
name | The name of the controller as well as the ROS namespace under which the controller should be loaded |
Definition at line 141 of file controller_manager.cpp.
bool controller_manager::ControllerManager::loadControllerSrv | ( | controller_manager_msgs::LoadController::Request & | req, |
controller_manager_msgs::LoadController::Response & | resp | ||
) | [private] |
Definition at line 642 of file controller_manager.cpp.
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.
controller_loader | A pointer to the loader to be registered |
Definition at line 689 of file controller_manager.cpp.
bool controller_manager::ControllerManager::reloadControllerLibrariesSrv | ( | controller_manager_msgs::ReloadControllerLibraries::Request & | req, |
controller_manager_msgs::ReloadControllerLibraries::Response & | resp | ||
) | [private] |
Definition at line 523 of file controller_manager.cpp.
bool controller_manager::ControllerManager::switchController | ( | const std::vector< std::string > & | start_controllers, |
const std::vector< std::string > & | stop_controllers, | ||
const int | strictness | ||
) |
Switch multiple controllers simultaneously.
start_controllers | A vector of controller names to be started |
stop_controllers | A vector of controller names to be stopped |
strictness | How 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. |
Definition at line 341 of file controller_manager.cpp.
bool controller_manager::ControllerManager::switchControllerSrv | ( | controller_manager_msgs::SwitchController::Request & | req, |
controller_manager_msgs::SwitchController::Response & | resp | ||
) | [private] |
Definition at line 674 of file controller_manager.cpp.
bool controller_manager::ControllerManager::unloadController | ( | const std::string & | name | ) |
Unload a controller by name.
name | The name of the controller to unload. (The same as the one used in loadController ) |
Definition at line 277 of file controller_manager.cpp.
bool controller_manager::ControllerManager::unloadControllerSrv | ( | controller_manager_msgs::UnloadController::Request & | req, |
controller_manager_msgs::UnloadController::Response & | resp | ||
) | [private] |
Definition at line 658 of file controller_manager.cpp.
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.
time | The current time |
period | The change in time since the last call to update |
reset_controllers | If true , stop and start all running controllers before updating |
Definition at line 75 of file controller_manager.cpp.
Definition at line 174 of file controller_manager.h.
std::list<ControllerLoaderInterfaceSharedPtr> controller_manager::ControllerManager::controller_loaders_ [private] |
Definition at line 176 of file controller_manager.h.
std::vector<ControllerSpec> controller_manager::ControllerManager::controllers_lists_[2] [private] |
Double-buffered controllers list.
Definition at line 193 of file controller_manager.h.
boost::recursive_mutex controller_manager::ControllerManager::controllers_lock_ [private] |
Mutex protecting the current controllers list.
Definition at line 191 of file controller_manager.h.
The index of the current controllers list.
Definition at line 195 of file controller_manager.h.
bool controller_manager::ControllerManager::please_switch_ [private] |
Definition at line 182 of file controller_manager.h.
Definition at line 172 of file controller_manager.h.
Definition at line 174 of file controller_manager.h.
boost::mutex controller_manager::ControllerManager::services_lock_ [private] |
Definition at line 215 of file controller_manager.h.
Definition at line 216 of file controller_manager.h.
Definition at line 216 of file controller_manager.h.
Definition at line 216 of file controller_manager.h.
Definition at line 217 of file controller_manager.h.
Definition at line 217 of file controller_manager.h.
Definition at line 217 of file controller_manager.h.
std::vector<controller_interface::ControllerBase*> controller_manager::ControllerManager::start_request_ [private] |
Definition at line 180 of file controller_manager.h.
std::vector<controller_interface::ControllerBase*> controller_manager::ControllerManager::stop_request_ [private] |
Definition at line 180 of file controller_manager.h.
std::list<hardware_interface::ControllerInfo> controller_manager::ControllerManager::switch_start_list_ [private] |
Definition at line 181 of file controller_manager.h.
std::list<hardware_interface::ControllerInfo> controller_manager::ControllerManager::switch_stop_list_ [private] |
Definition at line 181 of file controller_manager.h.
Definition at line 183 of file controller_manager.h.
int controller_manager::ControllerManager::used_by_realtime_ [private] |
The index of the controllers list being used in the real-time thread.
Definition at line 197 of file controller_manager.h.