00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00028
00029
00030
00031 #include "controller_manager/controller_manager.h"
00032 #include <algorithm>
00033 #include <boost/thread/thread.hpp>
00034 #include <boost/thread/condition.hpp>
00035 #include <sstream>
00036 #include <ros/console.h>
00037 #include <controller_manager/controller_loader.h>
00038 #include <controller_manager_msgs/ControllerState.h>
00039
00040 namespace controller_manager{
00041
00042
00043 ControllerManager::ControllerManager(hardware_interface::RobotHW *robot_hw, const ros::NodeHandle& nh) :
00044 robot_hw_(robot_hw),
00045 root_nh_(nh),
00046 cm_node_(nh, "controller_manager"),
00047 start_request_(0),
00048 stop_request_(0),
00049 please_switch_(false),
00050 current_controllers_list_(0),
00051 used_by_realtime_(-1)
00052 {
00053
00054 controller_loaders_.push_back(
00055 ControllerLoaderInterfaceSharedPtr(new ControllerLoader<controller_interface::ControllerBase>("controller_interface",
00056 "controller_interface::ControllerBase") ) );
00057
00058
00059 srv_list_controllers_ = cm_node_.advertiseService("list_controllers", &ControllerManager::listControllersSrv, this);
00060 srv_list_controller_types_ = cm_node_.advertiseService("list_controller_types", &ControllerManager::listControllerTypesSrv, this);
00061 srv_load_controller_ = cm_node_.advertiseService("load_controller", &ControllerManager::loadControllerSrv, this);
00062 srv_unload_controller_ = cm_node_.advertiseService("unload_controller", &ControllerManager::unloadControllerSrv, this);
00063 srv_switch_controller_ = cm_node_.advertiseService("switch_controller", &ControllerManager::switchControllerSrv, this);
00064 srv_reload_libraries_ = cm_node_.advertiseService("reload_controller_libraries", &ControllerManager::reloadControllerLibrariesSrv, this);
00065 }
00066
00067
00068 ControllerManager::~ControllerManager()
00069 {}
00070
00071
00072
00073
00074
00075 void ControllerManager::update(const ros::Time& time, const ros::Duration& period, bool reset_controllers)
00076 {
00077 used_by_realtime_ = current_controllers_list_;
00078 std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
00079
00080
00081 if (reset_controllers){
00082 for (size_t i=0; i<controllers.size(); i++){
00083 if (controllers[i].c->isRunning()){
00084 controllers[i].c->stopRequest(time);
00085 controllers[i].c->startRequest(time);
00086 }
00087 }
00088 }
00089
00090
00091
00092 for (size_t i=0; i<controllers.size(); i++)
00093 controllers[i].c->updateRequest(time, period);
00094
00095
00096 if (please_switch_)
00097 {
00098
00099 robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);
00100
00101
00102 for (unsigned int i=0; i<stop_request_.size(); i++)
00103 if (!stop_request_[i]->stopRequest(time))
00104 ROS_FATAL("Failed to stop controller in realtime loop. This should never happen.");
00105
00106
00107 for (unsigned int i=0; i<start_request_.size(); i++)
00108 if (!start_request_[i]->startRequest(time))
00109 ROS_FATAL("Failed to start controller in realtime loop. This should never happen.");
00110
00111 please_switch_ = false;
00112 }
00113 }
00114
00115 controller_interface::ControllerBase* ControllerManager::getControllerByName(const std::string& name)
00116 {
00117
00118 boost::recursive_mutex::scoped_lock guard(controllers_lock_);
00119
00120 std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
00121 for (size_t i = 0; i < controllers.size(); ++i)
00122 {
00123 if (controllers[i].info.name == name)
00124 return controllers[i].c.get();
00125 }
00126 return NULL;
00127 }
00128
00129 void ControllerManager::getControllerNames(std::vector<std::string> &names)
00130 {
00131 boost::recursive_mutex::scoped_lock guard(controllers_lock_);
00132 names.clear();
00133 std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
00134 for (size_t i = 0; i < controllers.size(); ++i)
00135 {
00136 names.push_back(controllers[i].info.name);
00137 }
00138 }
00139
00140
00141 bool ControllerManager::loadController(const std::string& name)
00142 {
00143 ROS_DEBUG("Will load controller '%s'", name.c_str());
00144
00145
00146 boost::recursive_mutex::scoped_lock guard(controllers_lock_);
00147
00148
00149 int free_controllers_list = (current_controllers_list_ + 1) % 2;
00150 while (ros::ok() && free_controllers_list == used_by_realtime_){
00151 if (!ros::ok())
00152 return false;
00153 usleep(200);
00154 }
00155 std::vector<ControllerSpec>
00156 &from = controllers_lists_[current_controllers_list_],
00157 &to = controllers_lists_[free_controllers_list];
00158 to.clear();
00159
00160
00161 for (size_t i = 0; i < from.size(); ++i)
00162 to.push_back(from[i]);
00163
00164
00165 for (size_t j = 0; j < to.size(); ++j)
00166 {
00167 if (to[j].info.name == name)
00168 {
00169 to.clear();
00170 ROS_ERROR("A controller named '%s' was already loaded inside the controller manager", name.c_str());
00171 return false;
00172 }
00173 }
00174
00175 ros::NodeHandle c_nh;
00176
00177 try{
00178 c_nh = ros::NodeHandle(root_nh_, name);
00179 }
00180 catch(std::exception &e) {
00181 ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name.c_str(), e.what());
00182 return false;
00183 }
00184 catch(...){
00185 ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s'", name.c_str());
00186 return false;
00187 }
00188 controller_interface::ControllerBaseSharedPtr c;
00189 std::string type;
00190 if (c_nh.getParam("type", type))
00191 {
00192 ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
00193 try
00194 {
00195
00196 std::list<ControllerLoaderInterfaceSharedPtr>::iterator it = controller_loaders_.begin();
00197 while (!c && it != controller_loaders_.end())
00198 {
00199 std::vector<std::string> cur_types = (*it)->getDeclaredClasses();
00200 for(size_t i=0; i < cur_types.size(); i++){
00201 if (type == cur_types[i]){
00202 c = (*it)->createInstance(type);
00203 }
00204 }
00205 ++it;
00206 }
00207 }
00208 catch (const std::runtime_error &ex)
00209 {
00210 ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
00211 }
00212 }
00213 else
00214 {
00215 ROS_ERROR("Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server (namespace: '%s')?", name.c_str(), c_nh.getNamespace().c_str());
00216 to.clear();
00217 return false;
00218 }
00219
00220
00221 if (!c)
00222 {
00223 ROS_ERROR("Could not load controller '%s' because controller type '%s' does not exist.", name.c_str(), type.c_str());
00224 ROS_ERROR("Use 'rosservice call controller_manager/list_controller_types' to get the available types");
00225 to.clear();
00226 return false;
00227 }
00228
00229
00230 ROS_DEBUG("Initializing controller '%s'", name.c_str());
00231 bool initialized;
00232 std::set<std::string> claimed_resources;
00233 try{
00234 initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
00235 }
00236 catch(std::exception &e){
00237 ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
00238 initialized = false;
00239 }
00240 catch(...){
00241 ROS_ERROR("Exception thrown while initializing controller %s", name.c_str());
00242 initialized = false;
00243 }
00244 if (!initialized)
00245 {
00246 to.clear();
00247 ROS_ERROR("Initializing controller '%s' failed", name.c_str());
00248 return false;
00249 }
00250 ROS_DEBUG("Initialized controller '%s' successful", name.c_str());
00251
00252
00253 to.resize(to.size() + 1);
00254 to[to.size()-1].info.type = type;
00255 to[to.size()-1].info.hardware_interface = c->getHardwareInterfaceType();
00256 to[to.size()-1].info.name = name;
00257 to[to.size()-1].info.resources = claimed_resources;
00258 to[to.size()-1].c = c;
00259
00260
00261 int former_current_controllers_list_ = current_controllers_list_;
00262 current_controllers_list_ = free_controllers_list;
00263 while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
00264 if (!ros::ok())
00265 return false;
00266 usleep(200);
00267 }
00268 from.clear();
00269
00270 ROS_DEBUG("Successfully load controller '%s'", name.c_str());
00271 return true;
00272 }
00273
00274
00275
00276
00277 bool ControllerManager::unloadController(const std::string &name)
00278 {
00279 ROS_DEBUG("Will unload controller '%s'", name.c_str());
00280
00281
00282 boost::recursive_mutex::scoped_lock guard(controllers_lock_);
00283
00284
00285 int free_controllers_list = (current_controllers_list_ + 1) % 2;
00286 while (ros::ok() && free_controllers_list == used_by_realtime_){
00287 if (!ros::ok())
00288 return false;
00289 usleep(200);
00290 }
00291 std::vector<ControllerSpec>
00292 &from = controllers_lists_[current_controllers_list_],
00293 &to = controllers_lists_[free_controllers_list];
00294 to.clear();
00295
00296
00297 bool removed = false;
00298 for (size_t i = 0; i < from.size(); ++i)
00299 {
00300 if (from[i].info.name == name){
00301 if (from[i].c->isRunning()){
00302 to.clear();
00303 ROS_ERROR("Could not unload controller with name %s because it is still running",
00304 name.c_str());
00305 return false;
00306 }
00307 removed = true;
00308 }
00309 else
00310 to.push_back(from[i]);
00311 }
00312
00313
00314 if (!removed)
00315 {
00316 to.clear();
00317 ROS_ERROR("Could not unload controller with name %s because no controller with this name exists",
00318 name.c_str());
00319 return false;
00320 }
00321
00322
00323 ROS_DEBUG("Realtime switches over to new controller list");
00324 int former_current_controllers_list_ = current_controllers_list_;
00325 current_controllers_list_ = free_controllers_list;
00326 while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
00327 if (!ros::ok())
00328 return false;
00329 usleep(200);
00330 }
00331 ROS_DEBUG("Destruct controller");
00332 from.clear();
00333 ROS_DEBUG("Destruct controller finished");
00334
00335 ROS_DEBUG("Successfully unloaded controller '%s'", name.c_str());
00336 return true;
00337 }
00338
00339
00340
00341 bool ControllerManager::switchController(const std::vector<std::string>& start_controllers,
00342 const std::vector<std::string>& stop_controllers,
00343 int strictness)
00344 {
00345 if (!stop_request_.empty() || !start_request_.empty())
00346 ROS_FATAL("The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen.");
00347
00348 if (strictness == 0){
00349 ROS_WARN("Controller Manager: To switch controllers you need to specify a strictness level of controller_manager_msgs::SwitchController::STRICT (%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",
00350 controller_manager_msgs::SwitchController::Request::STRICT,
00351 controller_manager_msgs::SwitchController::Request::BEST_EFFORT);
00352 strictness = controller_manager_msgs::SwitchController::Request::BEST_EFFORT;
00353 }
00354
00355 ROS_DEBUG("switching controllers:");
00356 for (unsigned int i=0; i<start_controllers.size(); i++)
00357 ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str());
00358 for (unsigned int i=0; i<stop_controllers.size(); i++)
00359 ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str());
00360
00361
00362 boost::recursive_mutex::scoped_lock guard(controllers_lock_);
00363
00364 controller_interface::ControllerBase* ct;
00365
00366 for (unsigned int i=0; i<stop_controllers.size(); i++)
00367 {
00368 ct = getControllerByName(stop_controllers[i]);
00369 if (ct == NULL){
00370 if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){
00371 ROS_ERROR("Could not stop controller with name %s because no controller with this name exists",
00372 stop_controllers[i].c_str());
00373 stop_request_.clear();
00374 return false;
00375 }
00376 else{
00377 ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists",
00378 stop_controllers[i].c_str());
00379 }
00380 }
00381 else{
00382 ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers",
00383 stop_controllers[i].c_str());
00384 stop_request_.push_back(ct);
00385 }
00386 }
00387 ROS_DEBUG("Stop request vector has size %i", (int)stop_request_.size());
00388
00389
00390 for (unsigned int i=0; i<start_controllers.size(); i++)
00391 {
00392 ct = getControllerByName(start_controllers[i]);
00393 if (ct == NULL){
00394 if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){
00395 ROS_ERROR("Could not start controller with name %s because no controller with this name exists",
00396 start_controllers[i].c_str());
00397 stop_request_.clear();
00398 start_request_.clear();
00399 return false;
00400 }
00401 else{
00402 ROS_DEBUG("Could not start controller with name %s because no controller with this name exists",
00403 start_controllers[i].c_str());
00404 }
00405 }
00406 else{
00407 ROS_DEBUG("Found controller %s that needs to be started in list of controllers",
00408 start_controllers[i].c_str());
00409 start_request_.push_back(ct);
00410 }
00411 }
00412 ROS_DEBUG("Start request vector has size %i", (int)start_request_.size());
00413
00414
00415 std::list<hardware_interface::ControllerInfo> info_list;
00416 switch_start_list_.clear();
00417 switch_stop_list_.clear();
00418
00419 std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
00420 for (size_t i = 0; i < controllers.size(); ++i)
00421 {
00422 bool in_stop_list = false;
00423 for(size_t j = 0; j < stop_request_.size(); j++)
00424 {
00425 if (stop_request_[j] == controllers[i].c.get())
00426 {
00427 in_stop_list = true;
00428 break;
00429 }
00430 }
00431
00432 bool in_start_list = false;
00433 for(size_t j = 0; j < start_request_.size(); j++)
00434 {
00435 if (start_request_[j] == controllers[i].c.get())
00436 {
00437 in_start_list = true;
00438 break;
00439 }
00440 }
00441
00442 const bool is_running = controllers[i].c->isRunning();
00443 hardware_interface::ControllerInfo &info = controllers[i].info;
00444
00445 if(!is_running && in_stop_list){
00446 if(strictness == controller_manager_msgs::SwitchController::Request::STRICT){
00447 ROS_ERROR_STREAM("Could not stop controller '" << info.name << "' since it is not running");
00448 stop_request_.clear();
00449 start_request_.clear();
00450 return false;
00451 } else {
00452 in_stop_list = false;
00453 }
00454 }
00455
00456 if(is_running && !in_stop_list && in_start_list){
00457 if(strictness == controller_manager_msgs::SwitchController::Request::STRICT){
00458 ROS_ERROR_STREAM("Controller '" << info.name << "' is already running");
00459 stop_request_.clear();
00460 start_request_.clear();
00461 return false;
00462 } else {
00463 in_start_list = false;
00464 }
00465 }
00466
00467 if(is_running && in_stop_list && !in_start_list){
00468 switch_stop_list_.push_back(info);
00469 }
00470 else if(!is_running && !in_stop_list && in_start_list){
00471 switch_start_list_.push_back(info);
00472 }
00473
00474 bool add_to_list = is_running;
00475 if (in_stop_list)
00476 add_to_list = false;
00477 if (in_start_list)
00478 add_to_list = true;
00479
00480 if (add_to_list)
00481 info_list.push_back(info);
00482 }
00483
00484 bool in_conflict = robot_hw_->checkForConflict(info_list);
00485 if (in_conflict)
00486 {
00487 ROS_ERROR("Could not switch controllers, due to resource conflict");
00488 stop_request_.clear();
00489 start_request_.clear();
00490 return false;
00491 }
00492
00493 if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_))
00494 {
00495 ROS_ERROR("Could not switch controllers. The hardware interface combination for the requested controllers is unfeasible.");
00496 stop_request_.clear();
00497 start_request_.clear();
00498 return false;
00499 }
00500
00501
00502 switch_strictness_ = strictness;
00503 please_switch_ = true;
00504
00505
00506 ROS_DEBUG("Request atomic controller switch from realtime loop");
00507 while (ros::ok() && please_switch_){
00508 if (!ros::ok())
00509 return false;
00510 usleep(100);
00511 }
00512 start_request_.clear();
00513 stop_request_.clear();
00514
00515 ROS_DEBUG("Successfully switched controllers");
00516 return true;
00517 }
00518
00519
00520
00521
00522
00523 bool ControllerManager::reloadControllerLibrariesSrv(
00524 controller_manager_msgs::ReloadControllerLibraries::Request &req,
00525 controller_manager_msgs::ReloadControllerLibraries::Response &resp)
00526 {
00527
00528 ROS_DEBUG("reload libraries service called");
00529 boost::mutex::scoped_lock guard(services_lock_);
00530 ROS_DEBUG("reload libraries service locked");
00531
00532
00533 std::vector<std::string> controllers;
00534 getControllerNames(controllers);
00535 if (!controllers.empty() && !req.force_kill){
00536 ROS_ERROR("Controller manager: Cannot reload controller libraries because there are still %i controllers running", (int)controllers.size());
00537 resp.ok = false;
00538 return true;
00539 }
00540
00541
00542 if (!controllers.empty()){
00543 ROS_INFO("Controller manager: Killing all running controllers");
00544 std::vector<std::string> empty;
00545 if (!switchController(empty,controllers, controller_manager_msgs::SwitchController::Request::BEST_EFFORT)){
00546 ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to stop running controllers");
00547 resp.ok = false;
00548 return true;
00549 }
00550 for (unsigned int i=0; i<controllers.size(); i++){
00551 if (!unloadController(controllers[i])){
00552 ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to unload controller %s",
00553 controllers[i].c_str());
00554 resp.ok = false;
00555 return true;
00556 }
00557 }
00558 getControllerNames(controllers);
00559 }
00560 assert(controllers.empty());
00561
00562
00563 for(std::list<ControllerLoaderInterfaceSharedPtr>::iterator it = controller_loaders_.begin(); it != controller_loaders_.end(); ++it)
00564 {
00565 (*it)->reload();
00566 ROS_INFO("Controller manager: reloaded controller libraries for %s", (*it)->getName().c_str());
00567 }
00568
00569 resp.ok = true;
00570
00571 ROS_DEBUG("reload libraries service finished");
00572 return true;
00573 }
00574
00575
00576 bool ControllerManager::listControllerTypesSrv(
00577 controller_manager_msgs::ListControllerTypes::Request &req,
00578 controller_manager_msgs::ListControllerTypes::Response &resp)
00579 {
00580
00581 (void) req;
00582
00583
00584 ROS_DEBUG("list types service called");
00585 boost::mutex::scoped_lock guard(services_lock_);
00586 ROS_DEBUG("list types service locked");
00587
00588 for(std::list<ControllerLoaderInterfaceSharedPtr>::iterator it = controller_loaders_.begin(); it != controller_loaders_.end(); ++it)
00589 {
00590 std::vector<std::string> cur_types = (*it)->getDeclaredClasses();
00591 for(size_t i=0; i < cur_types.size(); i++)
00592 {
00593 resp.types.push_back(cur_types[i]);
00594 resp.base_classes.push_back((*it)->getName());
00595 }
00596 }
00597
00598 ROS_DEBUG("list types service finished");
00599 return true;
00600 }
00601
00602
00603 bool ControllerManager::listControllersSrv(
00604 controller_manager_msgs::ListControllers::Request &req,
00605 controller_manager_msgs::ListControllers::Response &resp)
00606 {
00607
00608 (void) req;
00609
00610
00611 ROS_DEBUG("list controller service called");
00612 boost::mutex::scoped_lock services_guard(services_lock_);
00613 ROS_DEBUG("list controller service locked");
00614
00615
00616 boost::recursive_mutex::scoped_lock controller_guard(controllers_lock_);
00617 std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
00618 resp.controller.resize(controllers.size());
00619
00620 for (size_t i = 0; i < controllers.size(); ++i)
00621 {
00622 controller_manager_msgs::ControllerState& cs = resp.controller[i];
00623 cs.name = controllers[i].info.name;
00624 cs.type = controllers[i].info.type;
00625 cs.hardware_interface = controllers[i].info.hardware_interface;
00626 cs.resources.clear();
00627 cs.resources.reserve(controllers[i].info.resources.size());
00628 for (std::set<std::string>::iterator it = controllers[i].info.resources.begin(); it != controllers[i].info.resources.end(); ++it)
00629 cs.resources.push_back(*it);
00630
00631 if (controllers[i].c->isRunning())
00632 cs.state = "running";
00633 else
00634 cs.state = "stopped";
00635 }
00636
00637 ROS_DEBUG("list controller service finished");
00638 return true;
00639 }
00640
00641
00642 bool ControllerManager::loadControllerSrv(
00643 controller_manager_msgs::LoadController::Request &req,
00644 controller_manager_msgs::LoadController::Response &resp)
00645 {
00646
00647 ROS_DEBUG("loading service called for controller %s ",req.name.c_str());
00648 boost::mutex::scoped_lock guard(services_lock_);
00649 ROS_DEBUG("loading service locked");
00650
00651 resp.ok = loadController(req.name);
00652
00653 ROS_DEBUG("loading service finished for controller %s ",req.name.c_str());
00654 return true;
00655 }
00656
00657
00658 bool ControllerManager::unloadControllerSrv(
00659 controller_manager_msgs::UnloadController::Request &req,
00660 controller_manager_msgs::UnloadController::Response &resp)
00661 {
00662
00663 ROS_DEBUG("unloading service called for controller %s ",req.name.c_str());
00664 boost::mutex::scoped_lock guard(services_lock_);
00665 ROS_DEBUG("unloading service locked");
00666
00667 resp.ok = unloadController(req.name);
00668
00669 ROS_DEBUG("unloading service finished for controller %s ",req.name.c_str());
00670 return true;
00671 }
00672
00673
00674 bool ControllerManager::switchControllerSrv(
00675 controller_manager_msgs::SwitchController::Request &req,
00676 controller_manager_msgs::SwitchController::Response &resp)
00677 {
00678
00679 ROS_DEBUG("switching service called");
00680 boost::mutex::scoped_lock guard(services_lock_);
00681 ROS_DEBUG("switching service locked");
00682
00683 resp.ok = switchController(req.start_controllers, req.stop_controllers, req.strictness);
00684
00685 ROS_DEBUG("switching service finished");
00686 return true;
00687 }
00688
00689 void ControllerManager::registerControllerLoader(ControllerLoaderInterfaceSharedPtr controller_loader)
00690 {
00691 controller_loaders_.push_back(controller_loader);
00692 }
00693
00694 }