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 "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
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
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
00108 controller_loader_.reset(new pluginlib::ClassLoader<pr2_controller_interface::Controller>("pr2_controller_interface",
00109 "pr2_controller_interface::Controller"));
00110
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
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
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
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
00169 publishMechanismStatistics();
00170 publishJointState();
00171
00172
00173 if (please_switch_)
00174 {
00175
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
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
00224 boost::mutex::scoped_lock guard(controllers_lock_);
00225
00226
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
00239 for (size_t i = 0; i < from.size(); ++i)
00240 to.push_back(from[i]);
00241
00242
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
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
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
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
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
00333 to.resize(to.size() + 1);
00334 to[to.size()-1].name = name;
00335 to[to.size()-1].c = controller;
00336
00337
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
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
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
00373 boost::mutex::scoped_lock guard(controllers_lock_);
00374
00375
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
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
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
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
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
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
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
00485 boost::mutex::scoped_lock guard(controllers_lock_);
00486
00487 pr2_controller_interface::Controller* ct;
00488
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
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
00538 switch_strictness_ = strictness;
00539 please_switch_ = true;
00540
00541
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
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
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
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
00687 ROS_DEBUG("reload libraries service called");
00688 boost::mutex::scoped_lock guard(services_lock_);
00689 ROS_DEBUG("reload libraries service locked");
00690
00691
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
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
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
00737 (void) req;
00738
00739
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
00756 (void) req;
00757
00758
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
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
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
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
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