34 #include <boost/thread/thread.hpp> 35 #include <boost/thread/condition.hpp> 51 cm_node_(nh,
"pr2_controller_manager"),
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())
73 ROS_ERROR(
"Failed to initialize pr2 mechanism model");
86 if (type != urdf::Joint::REVOLUTE &&
87 type != urdf::Joint::CONTINUOUS &&
88 type != urdf::Joint::PRISMATIC){
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);
109 "pr2_controller_interface::Controller"));
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();
150 for (
size_t i=0; i<controllers.size(); i++){
152 controllers[scheduling[i]].c->updateRequest();
154 controllers[scheduling[i]].stats->acc((end - start).toSec());
156 controllers[scheduling[i]].stats->num_control_loop_overruns++;
157 controllers[scheduling[i]].stats->time_last_control_loop_overrun = end;
178 ROS_FATAL(
"Failed to stop controller in realtime loop. This should never happen.");
183 ROS_FATAL(
"Failed to start controller in realtime loop. This should never happen.");
194 for (
size_t i = 0; i < controllers.size(); ++i)
196 if (controllers[i].name == name)
197 return controllers[i].c.get();
206 for (
size_t i = 0; i < controllers.size(); ++i)
208 names.push_back(controllers[i].name);
221 ROS_DEBUG(
"Will load controller '%s'", name.c_str());
233 std::vector<ControllerSpec>
239 for (
size_t i = 0; i < from.size(); ++i)
240 to.push_back(from[i]);
243 for (
size_t j = 0; j < to.size(); ++j)
245 if (to[j].name == name)
248 ROS_ERROR(
"A controller named '%s' was already loaded inside the controller manager", name.c_str());
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());
263 ROS_ERROR(
"Exception thrown while constructing nodehandle for controller with name '%s'", name.c_str());
270 ROS_DEBUG(
"Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
276 for(
unsigned int i = 0; i < classes.size(); ++i)
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());
291 catch (
const std::runtime_error &ex)
293 ROS_ERROR(
"Could not load class %s: %s", type.c_str(), ex.what());
298 if (controller == NULL)
302 ROS_ERROR(
"Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server?",
305 ROS_ERROR(
"Could not load controller '%s' because controller type '%s' does not exist",
306 name.c_str(), type.c_str());
311 ROS_DEBUG(
"Initializing controller '%s'", name.c_str());
314 initialized = controller->initRequest(
this,
state_, c_node);
316 catch(std::exception &e){
317 ROS_ERROR(
"Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
321 ROS_ERROR(
"Exception thrown while initializing controller %s", name.c_str());
327 ROS_ERROR(
"Initializing controller '%s' failed", name.c_str());
330 ROS_DEBUG(
"Initialized controller '%s' succesful", name.c_str());
333 to.resize(to.size() + 1);
334 to[to.size()-1].name = name;
335 to[to.size()-1].c = controller;
340 ROS_ERROR(
"Scheduling controller '%s' failed", name.c_str());
347 for (
size_t i=0; i<to.size(); i++)
361 ROS_DEBUG(
"Successfully load controller '%s'", name.c_str());
370 ROS_DEBUG(
"Will unload controller '%s'", name.c_str());
382 std::vector<ControllerSpec>
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());
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());
406 bool removed =
false;
407 for (
size_t i = 0; i < from.size(); ++i)
409 if (from[i].name == name){
410 if (from[i].c->isRunning()){
412 ROS_ERROR(
"Could not unload controller with name %s because it is still running",
419 to.push_back(from[i]);
426 ROS_ERROR(
"Could not unload controller with name %s because no controller with this name exists",
432 ROS_DEBUG(
"Rescheduling controller execution order");
435 ROS_ERROR(
"Scheduling controllers failed when removing controller '%s' failed", name.c_str());
440 ROS_DEBUG(
"Resizing controller state vector");
443 for (
size_t i=0; i<to.size(); i++)
447 ROS_DEBUG(
"Realtime switches over to new controller list");
457 ROS_DEBUG(
"Destruct controller finished");
460 ROS_DEBUG(
"Successfully unloaded controller '%s'", name.c_str());
471 ROS_FATAL(
"The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen.");
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;
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());
489 for (
unsigned int i=0; i<stop_controllers.size(); i++)
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());
500 ROS_DEBUG(
"Could not stop controller with name %s because no controller with this name exists",
501 stop_controllers[i].c_str());
505 ROS_DEBUG(
"Found controller %s that needs to be stopped in list of controllers",
506 stop_controllers[i].c_str());
513 for (
unsigned int i=0; i<start_controllers.size(); i++)
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());
525 ROS_DEBUG(
"Could not start controller with name %s because no controller with this name exists",
526 start_controllers[i].c_str());
530 ROS_DEBUG(
"Found controller %s that needs to be started in list of controllers",
531 start_controllers[i].c_str());
542 ROS_DEBUG(
"Request atomic controller switch from realtime loop");
548 ROS_DEBUG(
"Successfully switched controllers");
567 if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
604 if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
609 out->timestamp = now;
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();
631 pr2_mechanism_msgs::ActuatorStatistics *out = &
pub_mech_stats_.
msg_.actuator_statistics[i];
633 out->timestamp = now;
634 out->name = it->first;
661 for (
unsigned int i = 0; i < controllers.size(); ++i)
663 pr2_mechanism_msgs::ControllerStatistics *out = &
pub_mech_stats_.
msg_.controller_statistics[i];
664 out->timestamp = now;
665 out->running = controllers[i].c->isRunning();
667 out->mean_time =
ros::Duration(mean(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;
683 pr2_mechanism_msgs::ReloadControllerLibraries::Request &req,
684 pr2_mechanism_msgs::ReloadControllerLibraries::Response &resp)
687 ROS_DEBUG(
"reload libraries service called");
689 ROS_DEBUG(
"reload libraries service locked");
692 std::vector<std::string> 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());
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");
709 for (
unsigned int i=0; i<controllers.size(); i++){
711 ROS_ERROR(
"Controller manager: Cannot reload controller libraries because failed to unload controller %s",
712 controllers[i].c_str());
719 assert(controllers.empty());
723 "pr2_controller_interface::Controller"));
724 ROS_INFO(
"Controller manager: reloaded controller libraries");
727 ROS_DEBUG(
"reload libraries service finished");
733 pr2_mechanism_msgs::ListControllerTypes::Request &req,
734 pr2_mechanism_msgs::ListControllerTypes::Response &resp)
746 ROS_DEBUG(
"list types service finished");
752 pr2_mechanism_msgs::ListControllers::Request &req,
753 pr2_mechanism_msgs::ListControllers::Response &resp)
759 ROS_DEBUG(
"list controller service called");
761 ROS_DEBUG(
"list controller service locked");
763 std::vector<std::string> controllers;
764 std::vector<size_t> schedule;
768 assert(controllers.size() == schedule.size());
769 resp.controllers.resize(controllers.size());
770 resp.state.resize(controllers.size());
772 for (
size_t i=0; i<schedule.size(); i++){
776 resp.controllers[i] = controllers[schedule[i]];
778 resp.state[i] =
"running";
780 resp.state[i] =
"stopped";
783 ROS_DEBUG(
"list controller service finished");
789 pr2_mechanism_msgs::LoadController::Request &req,
790 pr2_mechanism_msgs::LoadController::Response &resp)
793 ROS_DEBUG(
"loading service called for controller %s ",req.name.c_str());
799 ROS_DEBUG(
"loading service finished for controller %s ",req.name.c_str());
805 pr2_mechanism_msgs::UnloadController::Request &req,
806 pr2_mechanism_msgs::UnloadController::Response &resp)
809 ROS_DEBUG(
"unloading service called for controller %s ",req.name.c_str());
815 ROS_DEBUG(
"unloading service finished for controller %s ",req.name.c_str());
821 pr2_mechanism_msgs::SwitchController::Request &req,
822 pr2_mechanism_msgs::SwitchController::Response &resp)
829 resp.ok =
switchController(req.start_controllers, req.stop_controllers, req.strictness);
std::vector< pr2_controller_interface::Controller * > stop_request_
bool motors_previously_halted_
double last_measured_effort_
double last_calibration_rising_edge_
ros::ServiceServer srv_reload_libraries_
boost::mutex controllers_lock_
Statistics post_update_stats_
bool listControllerTypesSrv(pr2_mechanism_msgs::ListControllerTypes::Request &req, pr2_mechanism_msgs::ListControllerTypes::Response &resp)
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)
def stop_controllers(names)
ros::NodeHandle controller_node_
double last_executed_effort_
void propagateActuatorPositionToJointPosition()
boost::mutex services_lock_
pr2_hardware_interface::HardwareInterface * hw_
std::vector< size_t > controllers_scheduling_[2]
ros::Time last_published_joint_state_
double last_commanded_effort_
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 publishMechanismStatistics()
void getControllerSchedule(std::vector< size_t > &schedule)
ros::ServiceServer srv_load_controller_
double last_executed_current_
boost::shared_ptr< pluginlib::ClassLoader< pr2_controller_interface::Controller > > controller_loader_
Statistics pre_update_stats_
bool initXml(TiXmlElement *root)
bool reloadControllerLibrariesSrv(pr2_mechanism_msgs::ReloadControllerLibraries::Request &req, pr2_mechanism_msgs::ReloadControllerLibraries::Response &resp)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
bool calibration_rising_edge_valid_
ros::Duration publish_period_mechanism_stats_
std::vector< pr2_controller_interface::Controller * > start_request_
virtual ~ControllerManager()
ros::Time last_published_mechanism_stats_
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)
pr2_mechanism_model::Robot model_
double last_commanded_current_
realtime_tools::RealtimePublisher< pr2_mechanism_msgs::MechanismStatistics > pub_mech_stats_
bool initXml(TiXmlElement *config)
bool calibration_falling_edge_valid_
bool unloadController(const std::string &name)
double last_measured_current_
virtual pr2_controller_interface::Controller * getControllerByName(const std::string &name)
ros::Duration publish_period_joint_state_
ros::ServiceServer srv_switch_controller_
double last_calibration_falling_edge_
std::vector< JointState > joint_states_
bool scheduleControllers(const std::vector< ControllerSpec > &c, std::vector< size_t > &schedule)
int current_controllers_list_
bool getParam(const std::string &key, std::string &s) const
bool listControllersSrv(pr2_mechanism_msgs::ListControllers::Request &req, pr2_mechanism_msgs::ListControllers::Response &resp)
ros::ServiceServer srv_unload_controller_
std::vector< ControllerSpec > controllers_lists_[2]
bool loadController(const std::string &name)
pr2_mechanism_model::RobotState * state_
ros::ServiceServer srv_list_controllers_
realtime_tools::RealtimePublisher< sensor_msgs::JointState > pub_joint_state_
double max(double a, double b)
ros::ServiceServer srv_list_controller_types_
bool calibration_reading_
void propagateJointEffortToActuatorEffort()
def start_controllers(names)
void getControllerNames(std::vector< std::string > &v)