bool ControllerManager::reloadControllerLibrariesSrv(
controller_manager_msgs::ReloadControllerLibraries::Request &req,
controller_manager_msgs::ReloadControllerLibraries::Response &resp)
{
// lock services
ROS_DEBUG("reload libraries service called");
boost::mutex::scoped_lock guard(services_lock_);
ROS_DEBUG("reload libraries service locked");
// only reload libraries if no controllers are running
std::vector<std::string> controllers;
getControllerNames(controllers);
if (!controllers.empty() && !req.force_kill){
ROS_ERROR("Controller manager: Cannot reload controller libraries because there are still %i controllers running", (int)controllers.size());
resp.ok = false;
return true;
}
// kill running controllers if requested
if (!controllers.empty()){
ROS_INFO("Controller manager: Killing all running controllers");
std::vector<std::string> empty;
if (!switchController(empty,controllers, controller_manager_msgs::SwitchController::Request::BEST_EFFORT)){
ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to stop running controllers");
resp.ok = false;
return true;
}
for (unsigned int i=0; i<controllers.size(); i++){
if (!unloadController(controllers[i])){
ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to unload controller %s",
controllers[i].c_str());
resp.ok = false;
return true;
}
}
getControllerNames(controllers);
}
assert(controllers.empty());
// Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders)
for(std::list<LoaderPtr>::iterator it = controller_loaders_.begin(); it != controller_loaders_.end(); ++it)
{
(*it)->reload();
ROS_INFO("Controller manager: reloaded controller libraries for %s", (*it)->getName().c_str());
}
resp.ok = true;
ROS_DEBUG("reload libraries service finished");
return true;
}
void JointControllerManager::stopHook()
{
// Stop running controllers
stopController(getControllerNames(getRunningControllers()), BEST_EFFORT);
// Stop joint interface
if (!joint_iface_->stop())
{
PAL_ERROR("Failed to successfully stop component: Could not stop low level joint access.");
}
}
