controller_manager.cpp
Go to the documentation of this file.
1 // Copyright (C) 2008, Willow Garage, Inc.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Willow Garage, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 /*
28  * Author: Stuart Glaser, Wim Meeussen
29  */
30 
33 #include <algorithm>
34 #include <boost/thread/thread.hpp>
35 #include <boost/thread/condition.hpp>
36 #include <sstream>
37 #include "ros/console.h"
38 
39 using namespace pr2_mechanism_model;
40 using namespace pr2_controller_manager;
41 using namespace pr2_hardware_interface;
42 using namespace pr2_controller_interface;
43 using namespace boost::accumulators;
44 using namespace ros;
45 
46 
47 ControllerManager::ControllerManager(HardwareInterface *hw, const ros::NodeHandle& nh) :
48  model_(hw),
49  state_(NULL),
50  controller_node_(nh),
51  cm_node_(nh, "pr2_controller_manager"),
52  start_request_(0),
53  stop_request_(0),
54  please_switch_(false),
55  current_controllers_list_(0),
56  used_by_realtime_(-1),
57  pub_joint_state_(nh, "joint_states", 1),
58  pub_mech_stats_(nh, "mechanism_statistics", 1),
59  last_published_joint_state_(ros::Time::now()),
60  last_published_mechanism_stats_(ros::Time::now())
61 {}
62 
64 {
65  if (state_)
66  delete state_;
67 }
68 
69 
70 bool ControllerManager::initXml(TiXmlElement* config)
71 {
72  if (!model_.initXml(config)){
73  ROS_ERROR("Failed to initialize pr2 mechanism model");
74  return false;
75  }
76  state_ = new RobotState(&model_);
78 
79  // pre-allocate for realtime publishing
80  pub_mech_stats_.msg_.controller_statistics.resize(0);
81  pub_mech_stats_.msg_.actuator_statistics.resize(model_.hw_->actuators_.size());
82  int joints_size = 0;
83  for (unsigned int i = 0; i < state_->joint_states_.size(); ++i)
84  {
85  int type = state_->joint_states_[i].joint_->type;
86  if (type != urdf::Joint::REVOLUTE &&
87  type != urdf::Joint::CONTINUOUS &&
88  type != urdf::Joint::PRISMATIC){
89  ROS_ERROR("Joint state contains joint '%s' of unknown type", state_->joint_states_[i].joint_->name.c_str());
90  return false;
91  }
92  ++joints_size;
93  }
94  pub_mech_stats_.msg_.joint_statistics.resize(joints_size);
95  pub_joint_state_.msg_.name.resize(joints_size);
96  pub_joint_state_.msg_.position.resize(joints_size);
97  pub_joint_state_.msg_.velocity.resize(joints_size);
98  pub_joint_state_.msg_.effort.resize(joints_size);
99 
100  // get the publish rate for mechanism state
101  double publish_rate_joint_state, publish_rate_mechanism_stats;
102  cm_node_.param("mechanism_statistics_publish_rate", publish_rate_mechanism_stats, 1.0);
103  cm_node_.param("joint_state_publish_rate", publish_rate_joint_state, 100.0);
104  publish_period_mechanism_stats_ = Duration(1.0/fmax(0.000001, publish_rate_mechanism_stats));
105  publish_period_joint_state_ = Duration(1.0/fmax(0.000001, publish_rate_joint_state));
106 
107  // create controller loader
109  "pr2_controller_interface::Controller"));
110  // Advertise services (this should be the last thing we do in init)
117 
118  return true;
119 }
120 
121 
122 
123 
124 
125 // Must be realtime safe.
127 {
129  std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
130  std::vector<size_t> &scheduling = controllers_scheduling_[used_by_realtime_];
131 
134  state_->zeroCommands();
135  ros::Time start_update = ros::Time::now();
136  pre_update_stats_.acc((start_update - start).toSec());
137 
138  // Restart all running controllers if motors are re-enabled
140  for (size_t i=0; i<controllers.size(); i++){
141  if (controllers[scheduling[i]].c->isRunning()){
142  controllers[scheduling[i]].c->stopRequest();
143  controllers[scheduling[i]].c->startRequest();
144  }
145  }
146  }
148 
149  // Update all controllers in scheduling order
150  for (size_t i=0; i<controllers.size(); i++){
151  ros::Time start = ros::Time::now();
152  controllers[scheduling[i]].c->updateRequest();
153  ros::Time end = ros::Time::now();
154  controllers[scheduling[i]].stats->acc((end - start).toSec());
155  if (end - start > ros::Duration(0.001)){
156  controllers[scheduling[i]].stats->num_control_loop_overruns++;
157  controllers[scheduling[i]].stats->time_last_control_loop_overrun = end;
158  }
159  }
160  ros::Time end_update = ros::Time::now();
161  update_stats_.acc((end_update - start_update).toSec());
162 
165  ros::Time end = ros::Time::now();
166  post_update_stats_.acc((end - end_update).toSec());
167 
168  // publish state
171 
172  // there are controllers to start/stop
173  if (please_switch_)
174  {
175  // stop controllers
176  for (unsigned int i=0; i<stop_request_.size(); i++)
177  if (!stop_request_[i]->stopRequest())
178  ROS_FATAL("Failed to stop controller in realtime loop. This should never happen.");
179 
180  // start controllers
181  for (unsigned int i=0; i<start_request_.size(); i++)
182  if (!start_request_[i]->startRequest())
183  ROS_FATAL("Failed to start controller in realtime loop. This should never happen.");
184 
185  start_request_.clear();
186  stop_request_.clear();
187  please_switch_ = false;
188  }
189 }
190 
192 {
193  std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
194  for (size_t i = 0; i < controllers.size(); ++i)
195  {
196  if (controllers[i].name == name)
197  return controllers[i].c.get();
198  }
199  return NULL;
200 }
201 
202 void ControllerManager::getControllerNames(std::vector<std::string> &names)
203 {
204  boost::mutex::scoped_lock guard(controllers_lock_);
205  std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
206  for (size_t i = 0; i < controllers.size(); ++i)
207  {
208  names.push_back(controllers[i].name);
209  }
210 }
211 
212 void ControllerManager::getControllerSchedule(std::vector<size_t> &schedule)
213 {
214  boost::mutex::scoped_lock guard(controllers_lock_);
216 }
217 
218 
219 bool ControllerManager::loadController(const std::string& name)
220 {
221  ROS_DEBUG("Will load controller '%s'", name.c_str());
222 
223  // lock controllers
224  boost::mutex::scoped_lock guard(controllers_lock_);
225 
226  // get reference to controller list
227  int free_controllers_list = (current_controllers_list_ + 1) % 2;
228  while (ros::ok() && free_controllers_list == used_by_realtime_){
229  if (!ros::ok())
230  return false;
231  usleep(200);
232  }
233  std::vector<ControllerSpec>
235  &to = controllers_lists_[free_controllers_list];
236  to.clear();
237 
238  // Copy all controllers from the 'from' list to the 'to' list
239  for (size_t i = 0; i < from.size(); ++i)
240  to.push_back(from[i]);
241 
242  // Checks that we're not duplicating controllers
243  for (size_t j = 0; j < to.size(); ++j)
244  {
245  if (to[j].name == name)
246  {
247  to.clear();
248  ROS_ERROR("A controller named '%s' was already loaded inside the controller manager", name.c_str());
249  return false;
250  }
251  }
252 
253  NodeHandle c_node;
254  // Constructs the controller
255  try{
256  c_node = NodeHandle(controller_node_, name);
257  }
258  catch(std::exception &e) {
259  ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name.c_str(), e.what());
260  return false;
261  }
262  catch(...){
263  ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s'", name.c_str());
264  return false;
265  }
267  std::string type;
268  if (c_node.getParam("type", type))
269  {
270  ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
271  try {
272  // Backwards compatibility for using non-namespaced controller types
273  if (!controller_loader_->isClassAvailable(type))
274  {
275  std::vector<std::string> classes = controller_loader_->getDeclaredClasses();
276  for(unsigned int i = 0; i < classes.size(); ++i)
277  {
278  if(type == controller_loader_->getName(classes[i]))
279  {
280  ROS_WARN("The deprecated controller type %s was not found. Using the namespaced version %s instead. "
281  "Please update your configuration to use the namespaced version.",
282  type.c_str(), classes[i].c_str());
283  type = classes[i];
284  break;
285  }
286  }
287  }
288 
289  controller = controller_loader_->createInstance(type);
290  }
291  catch (const std::runtime_error &ex)
292  {
293  ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
294  }
295  }
296 
297  // checks if controller was constructed
298  if (controller == NULL)
299  {
300  to.clear();
301  if (type == "")
302  ROS_ERROR("Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server?",
303  name.c_str());
304  else
305  ROS_ERROR("Could not load controller '%s' because controller type '%s' does not exist",
306  name.c_str(), type.c_str());
307  return false;
308  }
309 
310  // Initializes the controller
311  ROS_DEBUG("Initializing controller '%s'", name.c_str());
312  bool initialized;
313  try{
314  initialized = controller->initRequest(this, state_, c_node);
315  }
316  catch(std::exception &e){
317  ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
318  initialized = false;
319  }
320  catch(...){
321  ROS_ERROR("Exception thrown while initializing controller %s", name.c_str());
322  initialized = false;
323  }
324  if (!initialized)
325  {
326  to.clear();
327  ROS_ERROR("Initializing controller '%s' failed", name.c_str());
328  return false;
329  }
330  ROS_DEBUG("Initialized controller '%s' succesful", name.c_str());
331 
332  // Adds the controller to the new list
333  to.resize(to.size() + 1);
334  to[to.size()-1].name = name;
335  to[to.size()-1].c = controller;
336 
337  // Do the controller scheduling
338  if (!scheduleControllers(to, controllers_scheduling_[free_controllers_list])){
339  to.clear();
340  ROS_ERROR("Scheduling controller '%s' failed", name.c_str());
341  return false;
342  }
343 
344  // Resize controller state vector
346  pub_mech_stats_.msg_.controller_statistics.resize(to.size());
347  for (size_t i=0; i<to.size(); i++)
348  pub_mech_stats_.msg_.controller_statistics[i].name = to[i].name;
349 
350  // Destroys the old controllers list when the realtime thread is finished with it.
351  int former_current_controllers_list_ = current_controllers_list_;
352  current_controllers_list_ = free_controllers_list;
353  while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
354  if (!ros::ok())
355  return false;
356  usleep(200);
357  }
358  from.clear();
360 
361  ROS_DEBUG("Successfully load controller '%s'", name.c_str());
362  return true;
363 }
364 
365 
366 
367 
368 bool ControllerManager::unloadController(const std::string &name)
369 {
370  ROS_DEBUG("Will unload controller '%s'", name.c_str());
371 
372  // lock the controllers
373  boost::mutex::scoped_lock guard(controllers_lock_);
374 
375  // get reference to controller list
376  int free_controllers_list = (current_controllers_list_ + 1) % 2;
377  while (ros::ok() && free_controllers_list == used_by_realtime_){
378  if (!ros::ok())
379  return false;
380  usleep(200);
381  }
382  std::vector<ControllerSpec>
384  &to = controllers_lists_[free_controllers_list];
385  to.clear();
386 
387  // check if no other controller depends on this controller
388  for (size_t i = 0; i < from.size(); ++i){
389  for (size_t b=0; b<from[i].c->before_list_.size(); b++){
390  if (name == from[i].c->before_list_[b]){
391  ROS_ERROR("Cannot unload controller %s because controller %s still depends on it",
392  name.c_str(), from[i].name.c_str());
393  return false;
394  }
395  }
396  for (size_t a=0; a<from[i].c->after_list_.size(); a++){
397  if (name == from[i].c->after_list_[a]){
398  ROS_ERROR("Cannot unload controller %s because controller %s still depends on it",
399  name.c_str(), from[i].name.c_str());
400  return false;
401  }
402  }
403  }
404 
405  // Transfers the running controllers over, skipping the one to be removed and the running ones.
406  bool removed = false;
407  for (size_t i = 0; i < from.size(); ++i)
408  {
409  if (from[i].name == name){
410  if (from[i].c->isRunning()){
411  to.clear();
412  ROS_ERROR("Could not unload controller with name %s because it is still running",
413  name.c_str());
414  return false;
415  }
416  removed = true;
417  }
418  else
419  to.push_back(from[i]);
420  }
421 
422  // Fails if we could not remove the controllers
423  if (!removed)
424  {
425  to.clear();
426  ROS_ERROR("Could not unload controller with name %s because no controller with this name exists",
427  name.c_str());
428  return false;
429  }
430 
431  // Do the controller scheduling
432  ROS_DEBUG("Rescheduling controller execution order");
433  if (!scheduleControllers(to, controllers_scheduling_[free_controllers_list])){
434  to.clear();
435  ROS_ERROR("Scheduling controllers failed when removing controller '%s' failed", name.c_str());
436  return false;
437  }
438 
439  // Resize controller state vector
440  ROS_DEBUG("Resizing controller state vector");
442  pub_mech_stats_.msg_.controller_statistics.resize(to.size());
443  for (size_t i=0; i<to.size(); i++)
444  pub_mech_stats_.msg_.controller_statistics[i].name = to[i].name;
445 
446  // Destroys the old controllers list when the realtime thread is finished with it.
447  ROS_DEBUG("Realtime switches over to new controller list");
448  int former_current_controllers_list_ = current_controllers_list_;
449  current_controllers_list_ = free_controllers_list;
450  while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
451  if (!ros::ok())
452  return false;
453  usleep(200);
454  }
455  ROS_DEBUG("Destruct controller");
456  from.clear();
457  ROS_DEBUG("Destruct controller finished");
459 
460  ROS_DEBUG("Successfully unloaded controller '%s'", name.c_str());
461  return true;
462 }
463 
464 
465 
466 bool ControllerManager::switchController(const std::vector<std::string>& start_controllers,
467  const std::vector<std::string>& stop_controllers,
468  int strictness)
469 {
470  if (!stop_request_.empty() || !start_request_.empty())
471  ROS_FATAL("The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen.");
472 
473  if (strictness == 0){
474  ROS_WARN("Controller Manager: To switch controllers you need to specify a strictness level of STRICT or BEST_EFFORT. Defaulting to BEST_EFFORT.");
475  strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
476  }
477 
478  ROS_DEBUG("switching controllers:");
479  for (unsigned int i=0; i<start_controllers.size(); i++)
480  ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str());
481  for (unsigned int i=0; i<stop_controllers.size(); i++)
482  ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str());
483 
484  // lock controllers
485  boost::mutex::scoped_lock guard(controllers_lock_);
486 
488  // list all controllers to stop
489  for (unsigned int i=0; i<stop_controllers.size(); i++)
490  {
491  ct = getControllerByName(stop_controllers[i]);
492  if (ct == NULL){
493  if (strictness == pr2_mechanism_msgs::SwitchController::Request::STRICT){
494  ROS_ERROR("Could not stop controller with name %s because no controller with this name exists",
495  stop_controllers[i].c_str());
496  stop_request_.clear();
497  return false;
498  }
499  else{
500  ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists",
501  stop_controllers[i].c_str());
502  }
503  }
504  else{
505  ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers",
506  stop_controllers[i].c_str());
507  stop_request_.push_back(ct);
508  }
509  }
510  ROS_DEBUG("Stop request vector has size %i", (int)stop_request_.size());
511 
512  // list all controllers to start
513  for (unsigned int i=0; i<start_controllers.size(); i++)
514  {
515  ct = getControllerByName(start_controllers[i]);
516  if (ct == NULL){
517  if (strictness == pr2_mechanism_msgs::SwitchController::Request::STRICT){
518  ROS_ERROR("Could not start controller with name %s because no controller with this name exists",
519  start_controllers[i].c_str());
520  stop_request_.clear();
521  start_request_.clear();
522  return false;
523  }
524  else{
525  ROS_DEBUG("Could not start controller with name %s because no controller with this name exists",
526  start_controllers[i].c_str());
527  }
528  }
529  else{
530  ROS_DEBUG("Found controller %s that needs to be started in list of controllers",
531  start_controllers[i].c_str());
532  start_request_.push_back(ct);
533  }
534  }
535  ROS_DEBUG("Start request vector has size %i", (int)start_request_.size());
536 
537  // start the atomic controller switching
538  switch_strictness_ = strictness;
539  please_switch_ = true;
540 
541  // wait until switch is finished
542  ROS_DEBUG("Request atomic controller switch from realtime loop");
543  while (ros::ok() && please_switch_){
544  if (!ros::ok())
545  return false;
546  usleep(100);
547  }
548  ROS_DEBUG("Successfully switched controllers");
549  return true;
550 }
551 
552 
554 {
555  ros::Time now = ros::Time::now();
557  {
559  {
562 
563  unsigned int j = 0;
564  for (unsigned int i = 0; i < state_->joint_states_.size(); ++i)
565  {
566  int type = state_->joint_states_[i].joint_->type;
567  if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
568  {
569  assert(j < pub_joint_state_.msg_.name.size());
570  assert(j < pub_joint_state_.msg_.position.size());
571  assert(j < pub_joint_state_.msg_.velocity.size());
572  assert(j < pub_joint_state_.msg_.effort.size());
574  pub_joint_state_.msg_.name[j] = state_->joint_states_[i].joint_->name;
575  pub_joint_state_.msg_.position[j] = in->position_;
576  pub_joint_state_.msg_.velocity[j] = in->velocity_;
577  pub_joint_state_.msg_.effort[j] = in->measured_effort_;
578 
579  j++;
580  }
581  }
582  pub_joint_state_.msg_.header.stamp = ros::Time::now();
584  }
585  }
586 }
587 
588 
590 {
591  ros::Time now = ros::Time::now();
593  {
594  if (pub_mech_stats_.trylock())
595  {
598 
599  // joint state
600  unsigned int j = 0;
601  for (unsigned int i = 0; i < state_->joint_states_.size(); ++i)
602  {
603  int type = state_->joint_states_[i].joint_->type;
604  if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
605  {
606  assert(j < pub_mech_stats_.msg_.joint_statistics.size());
608  pr2_mechanism_msgs::JointStatistics *out = &pub_mech_stats_.msg_.joint_statistics[j];
609  out->timestamp = now;
610  out->name = state_->joint_states_[i].joint_->name;
611  out->position = in->position_;
612  out->velocity = in->velocity_;
613  out->measured_effort = in->measured_effort_;
614  out->commanded_effort = in->commanded_effort_;
615  out->is_calibrated = in->calibrated_;
616  out->violated_limits = in->joint_statistics_.violated_limits_;
617  out->odometer = in->joint_statistics_.odometer_;
618  out->min_position = in->joint_statistics_.min_position_;
619  out->max_position = in->joint_statistics_.max_position_;
620  out->max_abs_velocity = in->joint_statistics_.max_abs_velocity_;
621  out->max_abs_effort = in->joint_statistics_.max_abs_effort_;
622  in->joint_statistics_.reset();
623  j++;
624  }
625  }
626 
627  // actuator state
628  unsigned int i = 0;
629  for (ActuatorMap::const_iterator it = model_.hw_->actuators_.begin(); it != model_.hw_->actuators_.end(); ++i, ++it)
630  {
631  pr2_mechanism_msgs::ActuatorStatistics *out = &pub_mech_stats_.msg_.actuator_statistics[i];
632  ActuatorState *in = &(it->second->state_);
633  out->timestamp = now;
634  out->name = it->first;
635  out->encoder_count = in->encoder_count_;
636  out->encoder_offset = in->zero_offset_;
637  out->position = in->position_;
638  out->timestamp = ros::Time(in->timestamp_);
639  out->device_id = in->device_id_;
640  out->encoder_velocity = in->encoder_velocity_;
641  out->velocity = in->velocity_;
642  out->calibration_reading = in->calibration_reading_;
643  out->calibration_rising_edge_valid = in->calibration_rising_edge_valid_;
644  out->calibration_falling_edge_valid = in->calibration_falling_edge_valid_;
645  out->last_calibration_rising_edge = in->last_calibration_rising_edge_;
646  out->last_calibration_falling_edge = in->last_calibration_falling_edge_;
647  out->is_enabled = in->is_enabled_;
648  out->halted = in->halted_;
649  out->last_commanded_current = in->last_commanded_current_;
650  out->last_executed_current = in->last_executed_current_;
651  out->last_measured_current = in->last_measured_current_;
652  out->last_commanded_effort = in->last_commanded_effort_;
653  out->last_executed_effort = in->last_executed_effort_;
654  out->last_measured_effort = in->last_measured_effort_;
655  out->motor_voltage = in->motor_voltage_;
656  out->num_encoder_errors = in->num_encoder_errors_;
657  }
658 
659  // controller state
660  std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
661  for (unsigned int i = 0; i < controllers.size(); ++i)
662  {
663  pr2_mechanism_msgs::ControllerStatistics *out = &pub_mech_stats_.msg_.controller_statistics[i];
664  out->timestamp = now;
665  out->running = controllers[i].c->isRunning();
666  out->max_time = ros::Duration(max(controllers[i].stats->acc));
667  out->mean_time = ros::Duration(mean(controllers[i].stats->acc));
668  out->variance_time = ros::Duration(sqrt(variance(controllers[i].stats->acc)));
669  out->num_control_loop_overruns = controllers[i].stats->num_control_loop_overruns;
670  out->time_last_control_loop_overrun = controllers[i].stats->time_last_control_loop_overrun;
671  }
672 
673  pub_mech_stats_.msg_.header.stamp = ros::Time::now();
674 
676  }
677  }
678 }
679 
680 
681 
683  pr2_mechanism_msgs::ReloadControllerLibraries::Request &req,
684  pr2_mechanism_msgs::ReloadControllerLibraries::Response &resp)
685 {
686  // lock services
687  ROS_DEBUG("reload libraries service called");
688  boost::mutex::scoped_lock guard(services_lock_);
689  ROS_DEBUG("reload libraries service locked");
690 
691  // only reload libraries if no controllers are running
692  std::vector<std::string> controllers;
693  getControllerNames(controllers);
694  if (!controllers.empty() && !req.force_kill){
695  ROS_ERROR("Controller manager: Cannot reload controller libraries because there are still %i controllers running", (int)controllers.size());
696  resp.ok = false;
697  return true;
698  }
699 
700  // kill running controllers if requested
701  if (!controllers.empty()){
702  ROS_INFO("Controller manager: Killing all running controllers");
703  std::vector<std::string> empty;
704  if (!switchController(empty,controllers, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT)){
705  ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to stop running controllers");
706  resp.ok = false;
707  return true;
708  }
709  for (unsigned int i=0; i<controllers.size(); i++){
710  if (!unloadController(controllers[i])){
711  ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to unload controller %s",
712  controllers[i].c_str());
713  resp.ok = false;
714  return true;
715  }
716  }
717  getControllerNames(controllers);
718  }
719  assert(controllers.empty());
720 
721  // create new controller loader
723  "pr2_controller_interface::Controller"));
724  ROS_INFO("Controller manager: reloaded controller libraries");
725  resp.ok = true;
726 
727  ROS_DEBUG("reload libraries service finished");
728  return true;
729 }
730 
731 
733  pr2_mechanism_msgs::ListControllerTypes::Request &req,
734  pr2_mechanism_msgs::ListControllerTypes::Response &resp)
735 {
736  // pretend to use the request
737  (void) req;
738 
739  // lock services
740  ROS_DEBUG("list types service called");
741  boost::mutex::scoped_lock guard(services_lock_);
742  ROS_DEBUG("list types service locked");
743 
744  resp.types = controller_loader_->getDeclaredClasses();
745 
746  ROS_DEBUG("list types service finished");
747  return true;
748 }
749 
750 
752  pr2_mechanism_msgs::ListControllers::Request &req,
753  pr2_mechanism_msgs::ListControllers::Response &resp)
754 {
755  // pretend to use the request
756  (void) req;
757 
758  // lock services
759  ROS_DEBUG("list controller service called");
760  boost::mutex::scoped_lock guard(services_lock_);
761  ROS_DEBUG("list controller service locked");
762 
763  std::vector<std::string> controllers;
764  std::vector<size_t> schedule;
765 
766  getControllerNames(controllers);
767  getControllerSchedule(schedule);
768  assert(controllers.size() == schedule.size());
769  resp.controllers.resize(controllers.size());
770  resp.state.resize(controllers.size());
771 
772  for (size_t i=0; i<schedule.size(); i++){
773  // add controller state
774  Controller* c = getControllerByName(controllers[schedule[i]]);
775  assert(c);
776  resp.controllers[i] = controllers[schedule[i]];
777  if (c->isRunning())
778  resp.state[i] = "running";
779  else
780  resp.state[i] = "stopped";
781  }
782 
783  ROS_DEBUG("list controller service finished");
784  return true;
785 }
786 
787 
789  pr2_mechanism_msgs::LoadController::Request &req,
790  pr2_mechanism_msgs::LoadController::Response &resp)
791 {
792  // lock services
793  ROS_DEBUG("loading service called for controller %s ",req.name.c_str());
794  boost::mutex::scoped_lock guard(services_lock_);
795  ROS_DEBUG("loading service locked");
796 
797  resp.ok = loadController(req.name);
798 
799  ROS_DEBUG("loading service finished for controller %s ",req.name.c_str());
800  return true;
801 }
802 
803 
805  pr2_mechanism_msgs::UnloadController::Request &req,
806  pr2_mechanism_msgs::UnloadController::Response &resp)
807 {
808  // lock services
809  ROS_DEBUG("unloading service called for controller %s ",req.name.c_str());
810  boost::mutex::scoped_lock guard(services_lock_);
811  ROS_DEBUG("unloading service locked");
812 
813  resp.ok = unloadController(req.name);
814 
815  ROS_DEBUG("unloading service finished for controller %s ",req.name.c_str());
816  return true;
817 }
818 
819 
821  pr2_mechanism_msgs::SwitchController::Request &req,
822  pr2_mechanism_msgs::SwitchController::Response &resp)
823 {
824  // lock services
825  ROS_DEBUG("switching service called");
826  boost::mutex::scoped_lock guard(services_lock_);
827  ROS_DEBUG("switching service locked");
828 
829  resp.ok = switchController(req.start_controllers, req.stop_controllers, req.strictness);
830 
831  ROS_DEBUG("switching service finished");
832  return true;
833 }
834 
std::vector< pr2_controller_interface::Controller * > stop_request_
#define ROS_FATAL(...)
bool listControllerTypesSrv(pr2_mechanism_msgs::ListControllerTypes::Request &req, pr2_mechanism_msgs::ListControllerTypes::Response &resp)
ROSCPP_DECL void start()
bool unloadControllerSrv(pr2_mechanism_msgs::UnloadController::Request &req, pr2_mechanism_msgs::UnloadController::Response &resp)
bool switchController(const std::vector< std::string > &start_controllers, const std::vector< std::string > &stop_controllers, const int strictness)
pr2_hardware_interface::HardwareInterface * hw_
std::vector< size_t > controllers_scheduling_[2]
#define ROS_ERROR(...)
#define ROS_WARN(...)
bool switchControllerSrv(pr2_mechanism_msgs::SwitchController::Request &req, pr2_mechanism_msgs::SwitchController::Response &resp)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void getControllerSchedule(std::vector< size_t > &schedule)
#define ROS_DEBUG(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
boost::shared_ptr< pluginlib::ClassLoader< pr2_controller_interface::Controller > > controller_loader_
bool initXml(TiXmlElement *root)
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
bool reloadControllerLibrariesSrv(pr2_mechanism_msgs::ReloadControllerLibraries::Request &req, pr2_mechanism_msgs::ReloadControllerLibraries::Response &resp)
TimeStatistics acc
ROSCPP_DECL bool ok()
std::vector< pr2_controller_interface::Controller * > start_request_
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
bool loadControllerSrv(pr2_mechanism_msgs::LoadController::Request &req, pr2_mechanism_msgs::LoadController::Response &resp)
realtime_tools::RealtimePublisher< pr2_mechanism_msgs::MechanismStatistics > pub_mech_stats_
bool unloadController(const std::string &name)
virtual pr2_controller_interface::Controller * getControllerByName(const std::string &name)
std::vector< JointState > joint_states_
bool scheduleControllers(const std::vector< ControllerSpec > &c, std::vector< size_t > &schedule)
bool listControllersSrv(pr2_mechanism_msgs::ListControllers::Request &req, pr2_mechanism_msgs::ListControllers::Response &resp)
static Time now()
std::vector< ControllerSpec > controllers_lists_[2]
bool loadController(const std::string &name)
pr2_mechanism_model::RobotState * state_
realtime_tools::RealtimePublisher< sensor_msgs::JointState > pub_joint_state_
double max(double a, double b)
int i
void getControllerNames(std::vector< std::string > &v)


pr2_controller_manager
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Tue Mar 7 2023 03:54:59