controller_manager.cpp
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2008, Willow Garage, Inc.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of Willow Garage, Inc. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 /*
00028  * Author: Stuart Glaser, Wim Meeussen
00029  */
00030 
00031 #include "pr2_controller_manager/controller_manager.h"
00032 #include "pr2_controller_manager/scheduler.h"
00033 #include <algorithm>
00034 #include <boost/thread/thread.hpp>
00035 #include <boost/thread/condition.hpp>
00036 #include <sstream>
00037 #include "ros/console.h"
00038 
00039 using namespace pr2_mechanism_model;
00040 using namespace pr2_controller_manager;
00041 using namespace pr2_hardware_interface;
00042 using namespace pr2_controller_interface;
00043 using namespace boost::accumulators;
00044 using namespace ros;
00045 
00046 
00047 ControllerManager::ControllerManager(HardwareInterface *hw, const ros::NodeHandle& nh) :
00048   model_(hw),
00049   state_(NULL),
00050   controller_node_(nh),
00051   cm_node_(nh, "pr2_controller_manager"),
00052   start_request_(0),
00053   stop_request_(0),
00054   please_switch_(false),
00055   current_controllers_list_(0),
00056   used_by_realtime_(-1),
00057   pub_joint_state_(nh, "joint_states", 1),
00058   pub_mech_stats_(nh, "mechanism_statistics", 1),
00059   last_published_joint_state_(ros::Time::now()),
00060   last_published_mechanism_stats_(ros::Time::now())
00061 {}
00062 
00063 ControllerManager::~ControllerManager()
00064 {
00065   if (state_)
00066     delete state_;
00067 }
00068 
00069 
00070 bool ControllerManager::initXml(TiXmlElement* config)
00071 {
00072   if (!model_.initXml(config)){
00073     ROS_ERROR("Failed to initialize pr2 mechanism model");
00074     return false;
00075   }
00076   state_ = new RobotState(&model_);
00077   motors_previously_halted_ = state_->isHalted();
00078 
00079   // pre-allocate for realtime publishing
00080   pub_mech_stats_.msg_.controller_statistics.resize(0);
00081   pub_mech_stats_.msg_.actuator_statistics.resize(model_.hw_->actuators_.size());
00082   int joints_size = 0;
00083   for (unsigned int i = 0; i < state_->joint_states_.size(); ++i)
00084   {
00085     int type = state_->joint_states_[i].joint_->type;
00086     if (type != urdf::Joint::REVOLUTE &&
00087         type != urdf::Joint::CONTINUOUS &&
00088         type != urdf::Joint::PRISMATIC){
00089       ROS_ERROR("Joint state contains joint '%s' of unknown type", state_->joint_states_[i].joint_->name.c_str());
00090       return false;
00091     }
00092     ++joints_size;
00093   }
00094   pub_mech_stats_.msg_.joint_statistics.resize(joints_size);
00095   pub_joint_state_.msg_.name.resize(joints_size);
00096   pub_joint_state_.msg_.position.resize(joints_size);
00097   pub_joint_state_.msg_.velocity.resize(joints_size);
00098   pub_joint_state_.msg_.effort.resize(joints_size);
00099 
00100   // get the publish rate for mechanism state
00101   double publish_rate_joint_state, publish_rate_mechanism_stats;
00102   cm_node_.param("mechanism_statistics_publish_rate", publish_rate_mechanism_stats, 1.0);
00103   cm_node_.param("joint_state_publish_rate", publish_rate_joint_state, 100.0);
00104   publish_period_mechanism_stats_ = Duration(1.0/fmax(0.000001, publish_rate_mechanism_stats));
00105   publish_period_joint_state_ = Duration(1.0/fmax(0.000001, publish_rate_joint_state));
00106 
00107   // create controller loader
00108   controller_loader_.reset(new pluginlib::ClassLoader<pr2_controller_interface::Controller>("pr2_controller_interface",
00109                                                                                            "pr2_controller_interface::Controller"));
00110   // Advertise services (this should be the last thing we do in init)
00111   srv_list_controllers_ = cm_node_.advertiseService("list_controllers", &ControllerManager::listControllersSrv, this);
00112   srv_list_controller_types_ = cm_node_.advertiseService("list_controller_types", &ControllerManager::listControllerTypesSrv, this);
00113   srv_load_controller_ = cm_node_.advertiseService("load_controller", &ControllerManager::loadControllerSrv, this);
00114   srv_unload_controller_ = cm_node_.advertiseService("unload_controller", &ControllerManager::unloadControllerSrv, this);
00115   srv_switch_controller_ = cm_node_.advertiseService("switch_controller", &ControllerManager::switchControllerSrv, this);
00116   srv_reload_libraries_ = cm_node_.advertiseService("reload_controller_libraries", &ControllerManager::reloadControllerLibrariesSrv, this);
00117 
00118   return true;
00119 }
00120 
00121 
00122 
00123 
00124 
00125 // Must be realtime safe.
00126 void ControllerManager::update()
00127 {
00128   used_by_realtime_ = current_controllers_list_;
00129   std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
00130   std::vector<size_t> &scheduling = controllers_scheduling_[used_by_realtime_];
00131 
00132   ros::Time start = ros::Time::now();
00133   state_->propagateActuatorPositionToJointPosition();
00134   state_->zeroCommands();
00135   ros::Time start_update = ros::Time::now();
00136   pre_update_stats_.acc((start_update - start).toSec());
00137 
00138   // Restart all running controllers if motors are re-enabled
00139   if (!state_->isHalted() && motors_previously_halted_){
00140     for (size_t i=0; i<controllers.size(); i++){
00141       if (controllers[scheduling[i]].c->isRunning()){
00142         controllers[scheduling[i]].c->stopRequest();
00143         controllers[scheduling[i]].c->startRequest();
00144       }
00145     }
00146   }
00147   motors_previously_halted_ = state_->isHalted();
00148 
00149   // Update all controllers in scheduling order
00150   for (size_t i=0; i<controllers.size(); i++){
00151     ros::Time start = ros::Time::now();
00152     controllers[scheduling[i]].c->updateRequest();
00153     ros::Time end = ros::Time::now();
00154     controllers[scheduling[i]].stats->acc((end - start).toSec());
00155     if (end - start > ros::Duration(0.001)){
00156       controllers[scheduling[i]].stats->num_control_loop_overruns++;
00157       controllers[scheduling[i]].stats->time_last_control_loop_overrun = end;
00158     }
00159   }
00160   ros::Time end_update = ros::Time::now();
00161   update_stats_.acc((end_update - start_update).toSec());
00162 
00163   state_->enforceSafety();
00164   state_->propagateJointEffortToActuatorEffort();
00165   ros::Time end = ros::Time::now();
00166   post_update_stats_.acc((end - end_update).toSec());
00167 
00168   // publish state
00169   publishMechanismStatistics();
00170   publishJointState();
00171 
00172   // there are controllers to start/stop
00173   if (please_switch_)
00174   {
00175     // stop controllers
00176     for (unsigned int i=0; i<stop_request_.size(); i++)
00177       if (!stop_request_[i]->stopRequest())
00178         ROS_FATAL("Failed to stop controller in realtime loop. This should never happen.");
00179 
00180     // start controllers
00181     for (unsigned int i=0; i<start_request_.size(); i++)
00182       if (!start_request_[i]->startRequest())
00183         ROS_FATAL("Failed to start controller in realtime loop. This should never happen.");
00184 
00185     start_request_.clear();
00186     stop_request_.clear();
00187     please_switch_ = false;
00188   }
00189 }
00190 
00191 pr2_controller_interface::Controller* ControllerManager::getControllerByName(const std::string& name)
00192 {
00193   std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
00194   for (size_t i = 0; i < controllers.size(); ++i)
00195   {
00196     if (controllers[i].name == name)
00197       return controllers[i].c.get();
00198   }
00199   return NULL;
00200 }
00201 
00202 void ControllerManager::getControllerNames(std::vector<std::string> &names)
00203 {
00204   boost::mutex::scoped_lock guard(controllers_lock_);
00205   std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
00206   for (size_t i = 0; i < controllers.size(); ++i)
00207   {
00208     names.push_back(controllers[i].name);
00209   }
00210 }
00211 
00212 void ControllerManager::getControllerSchedule(std::vector<size_t> &schedule)
00213 {
00214   boost::mutex::scoped_lock guard(controllers_lock_);
00215   schedule = controllers_scheduling_[current_controllers_list_];
00216 }
00217 
00218 
00219 bool ControllerManager::loadController(const std::string& name)
00220 {
00221   ROS_DEBUG("Will load controller '%s'", name.c_str());
00222 
00223   // lock controllers
00224   boost::mutex::scoped_lock guard(controllers_lock_);
00225 
00226   // get reference to controller list
00227   int free_controllers_list = (current_controllers_list_ + 1) % 2;
00228   while (ros::ok() && free_controllers_list == used_by_realtime_){
00229     if (!ros::ok())
00230       return false;
00231     usleep(200);
00232   }
00233   std::vector<ControllerSpec>
00234     &from = controllers_lists_[current_controllers_list_],
00235     &to = controllers_lists_[free_controllers_list];
00236   to.clear();
00237 
00238   // Copy all controllers from the 'from' list to the 'to' list
00239   for (size_t i = 0; i < from.size(); ++i)
00240     to.push_back(from[i]);
00241 
00242   // Checks that we're not duplicating controllers
00243   for (size_t j = 0; j < to.size(); ++j)
00244   {
00245     if (to[j].name == name)
00246     {
00247       to.clear();
00248       ROS_ERROR("A controller named '%s' was already loaded inside the controller manager", name.c_str());
00249       return false;
00250     }
00251   }
00252 
00253   NodeHandle c_node;
00254   // Constructs the controller
00255   try{
00256     c_node = NodeHandle(controller_node_, name);
00257   }
00258   catch(std::exception &e) {
00259     ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name.c_str(), e.what());
00260     return false;
00261   }
00262   catch(...){
00263     ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s'", name.c_str());
00264     return false;
00265   }
00266   boost::shared_ptr<pr2_controller_interface::Controller> controller;
00267   std::string type;
00268   if (c_node.getParam("type", type))
00269   {
00270     ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
00271     try {
00272       // Backwards compatibility for using non-namespaced controller types
00273       if (!controller_loader_->isClassAvailable(type))
00274       {
00275         std::vector<std::string> classes = controller_loader_->getDeclaredClasses();
00276         for(unsigned int i = 0; i < classes.size(); ++i)
00277         {
00278           if(type == controller_loader_->getName(classes[i]))
00279           {
00280             ROS_WARN("The deprecated controller type %s was not found.  Using the namespaced version %s instead.  "
00281                      "Please update your configuration to use the namespaced version.",
00282                      type.c_str(), classes[i].c_str());
00283             type = classes[i];
00284             break;
00285           }
00286         }
00287       }
00288 
00289       controller = controller_loader_->createInstance(type);
00290     }
00291     catch (const std::runtime_error &ex)
00292     {
00293       ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
00294     }
00295   }
00296 
00297   // checks if controller was constructed
00298   if (controller == NULL)
00299   {
00300     to.clear();
00301     if (type == "")
00302       ROS_ERROR("Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server?",
00303               name.c_str());
00304     else
00305       ROS_ERROR("Could not load controller '%s' because controller type '%s' does not exist",
00306                 name.c_str(), type.c_str());
00307     return false;
00308   }
00309 
00310   // Initializes the controller
00311   ROS_DEBUG("Initializing controller '%s'", name.c_str());
00312   bool initialized;
00313   try{
00314     initialized = controller->initRequest(this, state_, c_node);
00315   }
00316   catch(std::exception &e){
00317     ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
00318     initialized = false;
00319   }
00320   catch(...){
00321     ROS_ERROR("Exception thrown while initializing controller %s", name.c_str());
00322     initialized = false;
00323   }
00324   if (!initialized)
00325   {
00326     to.clear();
00327     ROS_ERROR("Initializing controller '%s' failed", name.c_str());
00328     return false;
00329   }
00330   ROS_DEBUG("Initialized controller '%s' succesful", name.c_str());
00331 
00332   // Adds the controller to the new list
00333   to.resize(to.size() + 1);
00334   to[to.size()-1].name = name;
00335   to[to.size()-1].c = controller;
00336 
00337   //  Do the controller scheduling
00338   if (!scheduleControllers(to, controllers_scheduling_[free_controllers_list])){
00339     to.clear();
00340     ROS_ERROR("Scheduling controller '%s' failed", name.c_str());
00341     return false;
00342   }
00343 
00344   // Resize controller state vector
00345   pub_mech_stats_.lock();
00346   pub_mech_stats_.msg_.controller_statistics.resize(to.size());
00347   for (size_t i=0; i<to.size(); i++)
00348     pub_mech_stats_.msg_.controller_statistics[i].name = to[i].name;
00349 
00350   // Destroys the old controllers list when the realtime thread is finished with it.
00351   int former_current_controllers_list_ = current_controllers_list_;
00352   current_controllers_list_ = free_controllers_list;
00353   while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
00354     if (!ros::ok())
00355       return false;
00356     usleep(200);
00357   }
00358   from.clear();
00359   pub_mech_stats_.unlock();
00360 
00361   ROS_DEBUG("Successfully load controller '%s'", name.c_str());
00362   return true;
00363 }
00364 
00365 
00366 
00367 
00368 bool ControllerManager::unloadController(const std::string &name)
00369 {
00370   ROS_DEBUG("Will unload controller '%s'", name.c_str());
00371 
00372   // lock the controllers
00373   boost::mutex::scoped_lock guard(controllers_lock_);
00374 
00375   // get reference to controller list
00376   int free_controllers_list = (current_controllers_list_ + 1) % 2;
00377   while (ros::ok() && free_controllers_list == used_by_realtime_){
00378     if (!ros::ok())
00379       return false;
00380     usleep(200);
00381   }
00382   std::vector<ControllerSpec>
00383     &from = controllers_lists_[current_controllers_list_],
00384     &to = controllers_lists_[free_controllers_list];
00385   to.clear();
00386 
00387   // check if no other controller depends on this controller
00388   for (size_t i = 0; i < from.size(); ++i){
00389     for (size_t b=0; b<from[i].c->before_list_.size(); b++){
00390       if (name == from[i].c->before_list_[b]){
00391         ROS_ERROR("Cannot unload controller %s because controller %s still depends on it",
00392                   name.c_str(), from[i].name.c_str());
00393         return false;
00394       }
00395     }
00396     for (size_t a=0; a<from[i].c->after_list_.size(); a++){
00397       if (name == from[i].c->after_list_[a]){
00398         ROS_ERROR("Cannot unload controller %s because controller %s still depends on it",
00399                   name.c_str(), from[i].name.c_str());
00400         return false;
00401       }
00402     }
00403   }
00404 
00405   // Transfers the running controllers over, skipping the one to be removed and the running ones.
00406   bool removed = false;
00407   for (size_t i = 0; i < from.size(); ++i)
00408   {
00409     if (from[i].name == name){
00410       if (from[i].c->isRunning()){
00411         to.clear();
00412         ROS_ERROR("Could not unload controller with name %s because it is still running",
00413                   name.c_str());
00414         return false;
00415       }
00416       removed = true;
00417     }
00418     else
00419       to.push_back(from[i]);
00420   }
00421 
00422   // Fails if we could not remove the controllers
00423   if (!removed)
00424   {
00425     to.clear();
00426     ROS_ERROR("Could not unload controller with name %s because no controller with this name exists",
00427               name.c_str());
00428     return false;
00429   }
00430 
00431   //  Do the controller scheduling
00432   ROS_DEBUG("Rescheduling controller execution order");
00433   if (!scheduleControllers(to, controllers_scheduling_[free_controllers_list])){
00434     to.clear();
00435     ROS_ERROR("Scheduling controllers failed when removing controller '%s' failed", name.c_str());
00436     return false;
00437   }
00438 
00439   // Resize controller state vector
00440   ROS_DEBUG("Resizing controller state vector");
00441   pub_mech_stats_.lock();
00442   pub_mech_stats_.msg_.controller_statistics.resize(to.size());
00443   for (size_t i=0; i<to.size(); i++)
00444     pub_mech_stats_.msg_.controller_statistics[i].name = to[i].name;
00445 
00446   // Destroys the old controllers list when the realtime thread is finished with it.
00447   ROS_DEBUG("Realtime switches over to new controller list");
00448   int former_current_controllers_list_ = current_controllers_list_;
00449   current_controllers_list_ = free_controllers_list;
00450   while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
00451     if (!ros::ok())
00452       return false;
00453     usleep(200);
00454   }
00455   ROS_DEBUG("Destruct controller");
00456   from.clear();
00457   ROS_DEBUG("Destruct controller finished");
00458   pub_mech_stats_.unlock();
00459 
00460   ROS_DEBUG("Successfully unloaded controller '%s'", name.c_str());
00461   return true;
00462 }
00463 
00464 
00465 
00466 bool ControllerManager::switchController(const std::vector<std::string>& start_controllers,
00467                                          const std::vector<std::string>& stop_controllers,
00468                                          int strictness)
00469 {
00470   if (!stop_request_.empty() || !start_request_.empty())
00471     ROS_FATAL("The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen.");
00472 
00473   if (strictness == 0){
00474     ROS_WARN("Controller Manager: To switch controllers you need to specify a strictness level of STRICT or BEST_EFFORT. Defaulting to BEST_EFFORT.");
00475     strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
00476   }
00477 
00478   ROS_DEBUG("switching controllers:");
00479   for (unsigned int i=0; i<start_controllers.size(); i++)
00480     ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str());
00481   for (unsigned int i=0; i<stop_controllers.size(); i++)
00482     ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str());
00483 
00484   // lock controllers
00485   boost::mutex::scoped_lock guard(controllers_lock_);
00486 
00487   pr2_controller_interface::Controller* ct;
00488   // list all controllers to stop
00489   for (unsigned int i=0; i<stop_controllers.size(); i++)
00490   {
00491     ct = getControllerByName(stop_controllers[i]);
00492     if (ct == NULL){
00493       if (strictness ==  pr2_mechanism_msgs::SwitchController::Request::STRICT){
00494         ROS_ERROR("Could not stop controller with name %s because no controller with this name exists",
00495                   stop_controllers[i].c_str());
00496         stop_request_.clear();
00497         return false;
00498       }
00499       else{
00500         ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists",
00501                   stop_controllers[i].c_str());
00502       }
00503     }
00504     else{
00505       ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers",
00506                 stop_controllers[i].c_str());
00507       stop_request_.push_back(ct);
00508     }
00509   }
00510   ROS_DEBUG("Stop request vector has size %i", (int)stop_request_.size());
00511 
00512   // list all controllers to start
00513   for (unsigned int i=0; i<start_controllers.size(); i++)
00514   {
00515     ct = getControllerByName(start_controllers[i]);
00516     if (ct == NULL){
00517       if (strictness ==  pr2_mechanism_msgs::SwitchController::Request::STRICT){
00518         ROS_ERROR("Could not start controller with name %s because no controller with this name exists",
00519                   start_controllers[i].c_str());
00520         stop_request_.clear();
00521         start_request_.clear();
00522         return false;
00523       }
00524       else{
00525         ROS_DEBUG("Could not start controller with name %s because no controller with this name exists",
00526                   start_controllers[i].c_str());
00527       }
00528     }
00529     else{
00530       ROS_DEBUG("Found controller %s that needs to be started in list of controllers",
00531                 start_controllers[i].c_str());
00532       start_request_.push_back(ct);
00533     }
00534   }
00535   ROS_DEBUG("Start request vector has size %i", (int)start_request_.size());
00536 
00537   // start the atomic controller switching
00538   switch_strictness_ = strictness;
00539   please_switch_ = true;
00540 
00541   // wait until switch is finished
00542   ROS_DEBUG("Request atomic controller switch from realtime loop");
00543   while (ros::ok() && please_switch_){
00544     if (!ros::ok())
00545       return false;
00546     usleep(100);
00547   }
00548   ROS_DEBUG("Successfully switched controllers");
00549   return true;
00550 }
00551 
00552 
00553 void ControllerManager::publishJointState()
00554 {
00555   ros::Time now = ros::Time::now();
00556   if (now > last_published_joint_state_ + publish_period_joint_state_)
00557   {
00558     if (pub_joint_state_.trylock())
00559     {
00560       while (last_published_joint_state_ + publish_period_joint_state_ < now)
00561         last_published_joint_state_ = last_published_joint_state_ + publish_period_joint_state_;
00562 
00563       unsigned int j = 0;
00564       for (unsigned int i = 0; i < state_->joint_states_.size(); ++i)
00565       {
00566         int type = state_->joint_states_[i].joint_->type;
00567         if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
00568         {
00569           assert(j < pub_joint_state_.msg_.name.size());
00570           assert(j < pub_joint_state_.msg_.position.size());
00571           assert(j < pub_joint_state_.msg_.velocity.size());
00572           assert(j < pub_joint_state_.msg_.effort.size());
00573           pr2_mechanism_model::JointState *in = &state_->joint_states_[i];
00574           pub_joint_state_.msg_.name[j] = state_->joint_states_[i].joint_->name;
00575           pub_joint_state_.msg_.position[j] = in->position_;
00576           pub_joint_state_.msg_.velocity[j] = in->velocity_;
00577           pub_joint_state_.msg_.effort[j] = in->measured_effort_;
00578 
00579           j++;
00580         }
00581       }
00582       pub_joint_state_.msg_.header.stamp = ros::Time::now();
00583       pub_joint_state_.unlockAndPublish();
00584     }
00585   }
00586 }
00587 
00588 
00589 void ControllerManager::publishMechanismStatistics()
00590 {
00591   ros::Time now = ros::Time::now();
00592   if (now > last_published_mechanism_stats_ + publish_period_mechanism_stats_)
00593   {
00594     if (pub_mech_stats_.trylock())
00595     {
00596       while (last_published_mechanism_stats_ + publish_period_mechanism_stats_ < now)
00597         last_published_mechanism_stats_ = last_published_mechanism_stats_ + publish_period_mechanism_stats_;
00598 
00599       // joint state
00600       unsigned int j = 0;
00601       for (unsigned int i = 0; i < state_->joint_states_.size(); ++i)
00602       {
00603         int type = state_->joint_states_[i].joint_->type;
00604         if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
00605         {
00606           assert(j < pub_mech_stats_.msg_.joint_statistics.size());
00607           pr2_mechanism_model::JointState *in = &state_->joint_states_[i];
00608           pr2_mechanism_msgs::JointStatistics *out = &pub_mech_stats_.msg_.joint_statistics[j];
00609           out->timestamp = now;
00610           out->name = state_->joint_states_[i].joint_->name;
00611           out->position = in->position_;
00612           out->velocity = in->velocity_;
00613           out->measured_effort = in->measured_effort_;
00614           out->commanded_effort = in->commanded_effort_;
00615           out->is_calibrated = in->calibrated_;
00616           out->violated_limits = in->joint_statistics_.violated_limits_;
00617           out->odometer = in->joint_statistics_.odometer_;
00618           out->min_position = in->joint_statistics_.min_position_;
00619           out->max_position = in->joint_statistics_.max_position_;
00620           out->max_abs_velocity = in->joint_statistics_.max_abs_velocity_;
00621           out->max_abs_effort = in->joint_statistics_.max_abs_effort_;
00622           in->joint_statistics_.reset();
00623           j++;
00624         }
00625       }
00626 
00627       // actuator state
00628       unsigned int i = 0;
00629       for (ActuatorMap::const_iterator it = model_.hw_->actuators_.begin(); it != model_.hw_->actuators_.end(); ++i, ++it)
00630       {
00631         pr2_mechanism_msgs::ActuatorStatistics *out = &pub_mech_stats_.msg_.actuator_statistics[i];
00632         ActuatorState *in = &(it->second->state_);
00633         out->timestamp = now;
00634         out->name = it->first;
00635         out->encoder_count = in->encoder_count_;
00636         out->encoder_offset = in->zero_offset_;
00637         out->position = in->position_;
00638         out->timestamp = ros::Time(in->timestamp_);
00639         out->device_id = in->device_id_;
00640         out->encoder_velocity = in->encoder_velocity_;
00641         out->velocity = in->velocity_;
00642         out->calibration_reading = in->calibration_reading_;
00643         out->calibration_rising_edge_valid = in->calibration_rising_edge_valid_;
00644         out->calibration_falling_edge_valid = in->calibration_falling_edge_valid_;
00645         out->last_calibration_rising_edge = in->last_calibration_rising_edge_;
00646         out->last_calibration_falling_edge = in->last_calibration_falling_edge_;
00647         out->is_enabled = in->is_enabled_;
00648         out->halted = in->halted_;
00649         out->last_commanded_current = in->last_commanded_current_;
00650         out->last_executed_current = in->last_executed_current_;
00651         out->last_measured_current = in->last_measured_current_;
00652         out->last_commanded_effort = in->last_commanded_effort_;
00653         out->last_executed_effort = in->last_executed_effort_;
00654         out->last_measured_effort = in->last_measured_effort_;
00655         out->motor_voltage = in->motor_voltage_;
00656         out->num_encoder_errors = in->num_encoder_errors_;
00657       }
00658 
00659       // controller state
00660       std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
00661       for (unsigned int i = 0; i < controllers.size(); ++i)
00662       {
00663         pr2_mechanism_msgs::ControllerStatistics *out = &pub_mech_stats_.msg_.controller_statistics[i];
00664         out->timestamp = now;
00665         out->running = controllers[i].c->isRunning();
00666         out->max_time = ros::Duration(max(controllers[i].stats->acc));
00667         out->mean_time = ros::Duration(mean(controllers[i].stats->acc));
00668         out->variance_time = ros::Duration(sqrt(variance(controllers[i].stats->acc)));
00669         out->num_control_loop_overruns = controllers[i].stats->num_control_loop_overruns;
00670         out->time_last_control_loop_overrun = controllers[i].stats->time_last_control_loop_overrun;
00671       }
00672 
00673       pub_mech_stats_.msg_.header.stamp = ros::Time::now();
00674 
00675       pub_mech_stats_.unlockAndPublish();
00676     }
00677   }
00678 }
00679 
00680 
00681 
00682 bool ControllerManager::reloadControllerLibrariesSrv(
00683   pr2_mechanism_msgs::ReloadControllerLibraries::Request &req,
00684   pr2_mechanism_msgs::ReloadControllerLibraries::Response &resp)
00685 {
00686   // lock services
00687   ROS_DEBUG("reload libraries service called");
00688   boost::mutex::scoped_lock guard(services_lock_);
00689   ROS_DEBUG("reload libraries service locked");
00690 
00691   // only reload libraries if no controllers are running
00692   std::vector<std::string> controllers;
00693   getControllerNames(controllers);
00694   if (!controllers.empty() && !req.force_kill){
00695     ROS_ERROR("Controller manager: Cannot reload controller libraries because there are still %i controllers running", (int)controllers.size());
00696     resp.ok = false;
00697     return true;
00698   }
00699 
00700   // kill running controllers if requested
00701   if (!controllers.empty()){
00702     ROS_INFO("Controller manager: Killing all running controllers");
00703     std::vector<std::string> empty;
00704     if (!switchController(empty,controllers, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT)){
00705       ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to stop running controllers");
00706       resp.ok = false;
00707       return true;
00708     }
00709     for (unsigned int i=0; i<controllers.size(); i++){
00710       if (!unloadController(controllers[i])){
00711         ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to unload controller %s",
00712                   controllers[i].c_str());
00713         resp.ok = false;
00714         return true;
00715       }
00716     }
00717     getControllerNames(controllers);
00718   }
00719   assert(controllers.empty());
00720 
00721   // create new controller loader
00722   controller_loader_.reset(new pluginlib::ClassLoader<pr2_controller_interface::Controller>("pr2_controller_interface",
00723                                                                                             "pr2_controller_interface::Controller"));
00724   ROS_INFO("Controller manager: reloaded controller libraries");
00725   resp.ok = true;
00726 
00727   ROS_DEBUG("reload libraries service finished");
00728   return true;
00729 }
00730 
00731 
00732 bool ControllerManager::listControllerTypesSrv(
00733   pr2_mechanism_msgs::ListControllerTypes::Request &req,
00734   pr2_mechanism_msgs::ListControllerTypes::Response &resp)
00735 {
00736   // pretend to use the request
00737   (void) req;
00738 
00739   // lock services
00740   ROS_DEBUG("list types service called");
00741   boost::mutex::scoped_lock guard(services_lock_);
00742   ROS_DEBUG("list types service locked");
00743 
00744   resp.types = controller_loader_->getDeclaredClasses();
00745 
00746   ROS_DEBUG("list types service finished");
00747   return true;
00748 }
00749 
00750 
00751 bool ControllerManager::listControllersSrv(
00752   pr2_mechanism_msgs::ListControllers::Request &req,
00753   pr2_mechanism_msgs::ListControllers::Response &resp)
00754 {
00755   // pretend to use the request
00756   (void) req;
00757 
00758   // lock services
00759   ROS_DEBUG("list controller service called");
00760   boost::mutex::scoped_lock guard(services_lock_);
00761   ROS_DEBUG("list controller service locked");
00762 
00763   std::vector<std::string> controllers;
00764   std::vector<size_t> schedule;
00765 
00766   getControllerNames(controllers);
00767   getControllerSchedule(schedule);
00768   assert(controllers.size() == schedule.size());
00769   resp.controllers.resize(controllers.size());
00770   resp.state.resize(controllers.size());
00771 
00772   for (size_t i=0; i<schedule.size(); i++){
00773     // add controller state
00774     Controller* c = getControllerByName(controllers[schedule[i]]);
00775     assert(c);
00776     resp.controllers[i] = controllers[schedule[i]];
00777     if (c->isRunning())
00778       resp.state[i] = "running";
00779     else
00780       resp.state[i] = "stopped";
00781   }
00782 
00783   ROS_DEBUG("list controller service finished");
00784   return true;
00785 }
00786 
00787 
00788 bool ControllerManager::loadControllerSrv(
00789   pr2_mechanism_msgs::LoadController::Request &req,
00790   pr2_mechanism_msgs::LoadController::Response &resp)
00791 {
00792   // lock services
00793   ROS_DEBUG("loading service called for controller %s ",req.name.c_str());
00794   boost::mutex::scoped_lock guard(services_lock_);
00795   ROS_DEBUG("loading service locked");
00796 
00797   resp.ok = loadController(req.name);
00798 
00799   ROS_DEBUG("loading service finished for controller %s ",req.name.c_str());
00800   return true;
00801 }
00802 
00803 
00804 bool ControllerManager::unloadControllerSrv(
00805   pr2_mechanism_msgs::UnloadController::Request &req,
00806   pr2_mechanism_msgs::UnloadController::Response &resp)
00807 {
00808   // lock services
00809   ROS_DEBUG("unloading service called for controller %s ",req.name.c_str());
00810   boost::mutex::scoped_lock guard(services_lock_);
00811   ROS_DEBUG("unloading service locked");
00812 
00813   resp.ok = unloadController(req.name);
00814 
00815   ROS_DEBUG("unloading service finished for controller %s ",req.name.c_str());
00816   return true;
00817 }
00818 
00819 
00820 bool ControllerManager::switchControllerSrv(
00821   pr2_mechanism_msgs::SwitchController::Request &req,
00822   pr2_mechanism_msgs::SwitchController::Response &resp)
00823 {
00824   // lock services
00825   ROS_DEBUG("switching service called");
00826   boost::mutex::scoped_lock guard(services_lock_);
00827   ROS_DEBUG("switching service locked");
00828 
00829   resp.ok = switchController(req.start_controllers, req.stop_controllers, req.strictness);
00830 
00831   ROS_DEBUG("switching service finished");
00832   return true;
00833 }
00834 


pr2_controller_manager
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Wed Aug 26 2015 15:37:28