controller_manager.cpp
Go to the documentation of this file.
00001 /*
00002     Copyright (c) 2011, Antons Rebguns <email>
00003     All rights reserved.
00004 
00005     Redistribution and use in source and binary forms, with or without
00006     modification, are permitted provided that the following conditions are met:
00007         * Redistributions of source code must retain the above copyright
00008         notice, this list of conditions and the following disclaimer.
00009         * Redistributions in binary form must reproduce the above copyright
00010         notice, this list of conditions and the following disclaimer in the
00011         documentation and/or other materials provided with the distribution.
00012         * Neither the name of the <organization> nor the
00013         names of its contributors may be used to endorse or promote products
00014         derived from this software without specific prior written permission.
00015 
00016     THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00017     EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018     WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019     DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00020     DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023     ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024     (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025     SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <string>
00029 #include <map>
00030 #include <XmlRpcValue.h>
00031 #include <boost/foreach.hpp>
00032 
00033 #include <dynamixel_hardware_interface/controller_manager.h>
00034 #include <dynamixel_hardware_interface/serial_proxy.h>
00035 #include <dynamixel_hardware_interface/single_joint_controller.h>
00036 #include <dynamixel_hardware_interface/multi_joint_controller.h>
00037 #include <dynamixel_hardware_interface/JointState.h>
00038 
00039 #include <ros/ros.h>
00040 #include <pluginlib/class_loader.h>
00041 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00042 #include <diagnostic_msgs/DiagnosticArray.h>
00043 
00044 #include <dynamixel_hardware_interface/StartController.h>
00045 #include <dynamixel_hardware_interface/StopController.h>
00046 #include <dynamixel_hardware_interface/RestartController.h>
00047 
00048 namespace dynamixel_controller_manager
00049 {
00050 
00051 ControllerManager::ControllerManager() : nh_(ros::NodeHandle()), private_nh_(ros::NodeHandle("~"))
00052 {
00053     private_nh_.param<double>("diagnostics_rate", diagnostics_rate_, 1.0);
00054     
00055     if (!private_nh_.getParam("namespace", manager_namespace_))
00056     {
00057         ROS_ERROR("dynamixel_controller_manager requires namespace paramater to be set");
00058     }
00059     
00060     XmlRpc::XmlRpcValue serial_ports;
00061     
00062     if (!private_nh_.getParam("serial_ports", serial_ports))
00063     {
00064         ROS_ERROR("dynamixel_controller_manager requires serial_ports parameter to be set");
00065     }
00066     
00067     if (serial_ports.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00068     {
00069         ROS_ERROR("dynamixel_controller_manager serial_ports has to be a map, passed type is %d", serial_ports.getType());
00070     }
00071     
00072     std::string port_namespace;
00073     XmlRpc::XmlRpcValue::iterator it;
00074     
00075     for (it = serial_ports.begin(); it != serial_ports.end(); ++it)
00076     {
00077         port_namespace = (*it).first;
00078         std::string prefix = "serial_ports/" + port_namespace + "/";
00079         
00080         std::string port_name;
00081         private_nh_.param<std::string>(prefix + "port_name", port_name, "/dev/ttyUSB0");
00082 
00083         int baud_rate_int;
00084         private_nh_.param<int>(prefix + "baud_rate", baud_rate_int, 1000000);
00085         std::string baud_rate = boost::lexical_cast<std::string>(baud_rate_int);
00086         
00087         int min_motor_id;
00088         private_nh_.param<int>(prefix + "min_motor_id", min_motor_id, 1);
00089 
00090         int max_motor_id;
00091         private_nh_.param<int>(prefix + "max_motor_id", max_motor_id, 25);
00092         
00093         int update_rate;
00094         private_nh_.param<int>(prefix + "update_rate", update_rate, 10);
00095         
00096         prefix += "diagnostics/";
00097         
00098         int error_level_temp;
00099         private_nh_.param<int>(prefix + "error_level_temp", error_level_temp, 65);
00100         
00101         int warn_level_temp;
00102         private_nh_.param<int>(prefix + "warn_level_temp", warn_level_temp, 60);
00103         
00104         dynamixel_hardware_interface::SerialProxy* serial_proxy =
00105             new dynamixel_hardware_interface::SerialProxy(port_name,
00106                                                           port_namespace,
00107                                                           baud_rate,
00108                                                           min_motor_id,
00109                                                           max_motor_id,
00110                                                           update_rate,
00111                                                           diagnostics_rate_,
00112                                                           error_level_temp,
00113                                                           warn_level_temp);
00114         if (!serial_proxy->connect())
00115         {
00116             delete serial_proxy;
00117             continue;
00118         }
00119         
00120         serial_proxies_[port_namespace] = serial_proxy;
00121     }
00122     
00123     if (serial_proxies_.empty())
00124     {
00125         ROS_FATAL("No motors found on any configured serial port, aborting");
00126         exit(1);
00127     }
00128     
00129     sjc_loader_.reset(new pluginlib::ClassLoader<controller::SingleJointController>("dynamixel_hardware_interface", "controller::SingleJointController"));
00130     mjc_loader_.reset(new pluginlib::ClassLoader<controller::MultiJointController>("dynamixel_hardware_interface", "controller::MultiJointController"));
00131     
00132     diagnostics_pub_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1000);
00133     
00134     start_controller_server_ = nh_.advertiseService(manager_namespace_ + "/start_controller",
00135                                                     &ControllerManager::startControllerSrv, this);
00136     
00137     stop_controller_server_ = nh_.advertiseService(manager_namespace_ + "/stop_controller",
00138                                                    &ControllerManager::stopControllerSrv, this);
00139     
00140     restart_controller_server_ = nh_.advertiseService(manager_namespace_ + "/restart_controller",
00141                                                       &ControllerManager::restartControllerSrv, this);
00142     
00143     if (diagnostics_rate_ > 0)
00144     {
00145         terminate_diagnostics_ = false;
00146         diagnostics_thread_ = new boost::thread(boost::bind(&ControllerManager::publishDiagnosticInformation, this));
00147     }
00148     else
00149     {
00150         diagnostics_thread_ = NULL;
00151     }
00152 }
00153 
00154 ControllerManager::~ControllerManager()
00155 {
00156     if (diagnostics_thread_)
00157     {
00158         {
00159             boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00160             terminate_diagnostics_ = true;
00161         }
00162         diagnostics_thread_->join();
00163         delete diagnostics_thread_;
00164     }
00165 
00166     std::map<std::string, dynamixel_hardware_interface::SerialProxy*>::iterator sp_it;
00167     
00168     for (sp_it = serial_proxies_.begin(); sp_it != serial_proxies_.end(); ++sp_it)
00169     {
00170         delete sp_it->second;
00171     }
00172 
00173     std::map<std::string, controller::SingleJointController*>::iterator c_it;
00174     
00175     for (c_it = sj_controllers_.begin(); c_it != sj_controllers_.end(); ++c_it)
00176     {
00177         delete c_it->second;
00178     }
00179 }
00180 
00181 bool ControllerManager::startController(std::string name, std::string port)
00182 {
00183     boost::mutex::scoped_lock c_guard(controllers_lock_);
00184     ROS_DEBUG("Will start controller '%s'", name.c_str());
00185     
00186     std::string type;
00187     if (!nh_.getParam(name + "/type", type))
00188     {
00189         ROS_ERROR("Type not specified for %s controller", name.c_str());
00190         return false;
00191     }
00192     
00193     // assume we are loading a single joint controller, then look for its
00194     // name in declared multi-joint controllers, if found we are loading
00195     // a multi-joint controller
00196     bool single_joint_controller = true;
00197     std::vector<std::string> mj_classes = mjc_loader_->getDeclaredClasses();
00198     for (size_t i = 0; i < mj_classes.size(); ++i)
00199     {
00200         if (mj_classes[i] == type)
00201         {
00202             single_joint_controller = false;
00203             break;
00204         }
00205     }
00206     
00207     if (single_joint_controller)
00208     {
00209         ROS_DEBUG("Loading single-joint controller");
00210         
00211         if (port.empty())
00212         {
00213             ROS_ERROR("Port name is not specified for controller %s", name.c_str());
00214             return false;
00215         }
00216         
00217         if (serial_proxies_.find(port) == serial_proxies_.end())
00218         {
00219             ROS_ERROR("Serial port %s is not managed by %s controller manager", port.c_str(), manager_namespace_.c_str());
00220             return false;
00221         }
00222         
00223         if (sj_controllers_.find(name) != sj_controllers_.end())
00224         {
00225             ROS_ERROR("Controller %s is already started", name.c_str());
00226             return false;
00227         }
00228         
00229         controller::SingleJointController* c = NULL;
00230         ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
00231         
00232         try
00233         {
00234             c = sjc_loader_->createClassInstance(type, true);
00235         }
00236         catch (const std::runtime_error &ex)
00237         {
00238             ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
00239         }
00240         
00241         // checks if controller was constructed
00242         if (c == NULL)
00243         {
00244             if (type == "")
00245             {
00246                 ROS_ERROR("Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server?", name.c_str());
00247             }
00248             else
00249             {
00250                 ROS_ERROR("Could not load controller '%s' because controller type '%s' does not exist", name.c_str(), type.c_str());
00251             }
00252             
00253             return false;
00254         }
00255         
00256         // Initializes the controller
00257         ROS_DEBUG("Initializing controller '%s'", name.c_str());
00258         bool initialized = false;
00259         
00260         try
00261         {
00262             initialized = c->initialize(name, port, serial_proxies_[port]->getSerialPort());
00263         }
00264         catch(std::exception &e)
00265         {
00266             ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
00267             initialized = false;
00268         }
00269         catch(...)
00270         {
00271             ROS_ERROR("Exception thrown while initializing controller %s", name.c_str());
00272             initialized = false;
00273         }
00274         
00275         if (!initialized)
00276         {
00277             delete c;
00278             ROS_ERROR("Initializing controller '%s' failed", name.c_str());
00279             return false;
00280         }
00281         
00282         c->start();
00283         
00284         sj_controllers_[name] = c;
00285         ROS_DEBUG("Initialized controller '%s' succesful", name.c_str());
00286     }
00287     else
00288     {
00289         ROS_DEBUG("Loading multi-joint controller");
00290         
00291         std::vector<std::string> dependencies;
00292         XmlRpc::XmlRpcValue raw;
00293         if (!nh_.getParam(name + "/dependencies", raw))
00294         {
00295             ROS_ERROR("Dependencies are not specified for multi-joint controller %s", name.c_str());
00296             return false;
00297         }
00298         
00299         if (raw.getType() != XmlRpc::XmlRpcValue::TypeArray)
00300         {
00301             ROS_ERROR("Dependencies parameter of controller %s is not a list", name.c_str());
00302             return false;
00303         }
00304         
00305         for (int i = 0; i < raw.size(); ++i)
00306         {
00307             if (raw[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00308             {
00309                 ROS_ERROR("All dependencies of controller %s should be strings", name.c_str());
00310                 return false;
00311             }
00312             
00313             dependencies.push_back(static_cast<std::string>(raw[i]));
00314         }
00315         
00316         if (mj_controllers_.find(name) != mj_controllers_.end() ||
00317             mj_waiting_controllers_.find(name) != mj_waiting_controllers_.end())
00318         {
00319             ROS_ERROR("Multi-joint controller %s is already started", name.c_str());
00320             return false;
00321         }
00322         
00323         std::pair<std::string, std::vector<std::string> > mjc_spec(name, dependencies);
00324         mj_waiting_controllers_.insert(name);
00325         waiting_mjcs_.insert(mjc_spec);
00326     }
00327     
00328     checkDeps();
00329     return true;
00330 }
00331 
00332 bool ControllerManager::stopController(std::string name)
00333 {
00334     boost::mutex::scoped_lock c_guard(controllers_lock_);
00335     ROS_DEBUG("Will stop controller '%s'", name.c_str());
00336     
00337     std::map<std::string, controller::SingleJointController*>::iterator sit;
00338     std::map<std::string, controller::MultiJointController*>::iterator mit;
00339     
00340     sit = sj_controllers_.find(name);
00341     mit = mj_controllers_.find(name);
00342     
00343     if (sit == sj_controllers_.end())
00344     {
00345         ROS_DEBUG("%s not found in single_joint_controller map", name.c_str());
00346         
00347         if (mit == mj_controllers_.end())
00348         {
00349             ROS_ERROR("Unable to stop %s, controller is not running", name.c_str());
00350             return false;
00351         }
00352         else
00353         {
00354             controller::MultiJointController* c = mit->second;
00355             ROS_DEBUG("stopping multi-joint controller %s", name.c_str());
00356             c->stop();
00357             mj_controllers_.erase(mit);
00358             delete c;
00359         }
00360     }
00361     // trying to stop a single-joint controller
00362     else
00363     {
00364         // first check if any of the loaded multi-joint controllers require this
00365         // single-joint controller (i.e. have it in their dependencies list)
00366         std::map<std::string, controller::MultiJointController*>::iterator it;
00367         
00368         for (it = mj_controllers_.begin(); it != mj_controllers_.end(); ++it)
00369         {
00370             // get dependencies
00371             std::vector<controller::SingleJointController*> deps = it->second->getDependencies();
00372             
00373             // go through the list and compare each dep name to passed in controller name
00374             for (size_t i = 0; i < deps.size(); ++i)
00375             {
00376                 std::string dep_name = deps[i]->getName();
00377                 
00378                 // found a match means this multi-joint controller depends on the single-joint
00379                 // controller we are trying to stop, report failure
00380                 if (dep_name == name)
00381                 {
00382                     ROS_ERROR("Can not stop single-joint controller %s, multi-joint controller %s still depends on it", name.c_str(), it->first.c_str());
00383                     return false;
00384                 }
00385             }
00386         }
00387         
00388         controller::SingleJointController* c = sit->second;
00389         ROS_DEBUG("stopping single-joint controller %s", name.c_str());
00390         c->stop();
00391         sj_controllers_.erase(sit);
00392         delete c;
00393     }
00394     
00395     return true;
00396 }
00397 
00398 bool ControllerManager::restartController(std::string name)
00399 {
00400     ROS_DEBUG("Will restart controller '%s'", name.c_str());
00401     
00402     std::string port;
00403     
00404     {
00405         boost::mutex::scoped_lock c_guard(controllers_lock_);
00406         std::map<std::string, controller::SingleJointController*>::const_iterator sit;
00407         sit = sj_controllers_.find(name);
00408         
00409         // if not found in both single and multi-joint maps, it is not loaded and running
00410         if (sit == sj_controllers_.end())
00411         {
00412             if (mj_controllers_.find(name) == mj_controllers_.end())
00413             {
00414                 ROS_ERROR("Controller %s is not running", name.c_str());
00415                 return false;
00416             }
00417         }
00418         else
00419         {
00420             port = sit->second->getPortNamespace();
00421         }
00422     }
00423     
00424     if (stopController(name) && startController(name, port)) { return true; }
00425     return false;
00426 }
00427 
00428 
00429 void ControllerManager::publishDiagnosticInformation()
00430 {
00431     diagnostic_msgs::DiagnosticArray diag_msg;
00432     diagnostic_updater::DiagnosticStatusWrapper joint_status;
00433     ros::Rate rate(diagnostics_rate_);
00434 
00435     while (nh_.ok())
00436     {
00437         {
00438             boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00439             if (terminate_diagnostics_) { break; }
00440         }
00441 
00442         diag_msg.status.clear();
00443         diag_msg.header.stamp = ros::Time::now();
00444 
00445         {
00446             boost::mutex::scoped_lock c_guard(controllers_lock_);
00447             
00448             std::map<std::string, controller::SingleJointController*>::iterator it;
00449             
00450             for (it = sj_controllers_.begin(); it != sj_controllers_.end(); ++it)
00451             {
00452                 dynamixel_hardware_interface::JointState state = it->second->getJointState();
00453                 if (state.header.stamp.isZero()) { continue; }
00454                 
00455                 joint_status.clear();
00456                 joint_status.name = "Joint Controller " + it->second->getName();
00457                 joint_status.add("Moving", state.moving ? "True" : "False");
00458                 joint_status.add("Target Position", state.target_position);
00459                 joint_status.add("Target Velocity", state.target_velocity);
00460                 joint_status.add("Position", state.position);
00461                 joint_status.add("Velocity", state.velocity);
00462                 joint_status.add("Position Error", state.position - state.target_position);
00463                 joint_status.add("Velocity Error", state.velocity != 0 ? state.velocity - state.target_velocity : 0);
00464                 joint_status.add("Load", state.load);
00465                 joint_status.summary(joint_status.OK, "OK");
00466                 
00467                 diag_msg.status.push_back(joint_status);
00468             }
00469             
00470         }
00471         
00472         diagnostics_pub_.publish(diag_msg);
00473         rate.sleep();
00474     }
00475 }
00476 
00477 void ControllerManager::checkDeps()
00478 {
00479     std::set<std::string> loaded_controllers;
00480     std::map<std::string, controller::SingleJointController*>::iterator c_it;
00481     
00482     for (c_it = sj_controllers_.begin(); c_it != sj_controllers_.end(); ++c_it)
00483     {
00484         loaded_controllers.insert(c_it->first);
00485     }
00486     
00487     std::set<std::pair<std::string, std::vector<std::string> > >::iterator it;
00488     
00489     for (it = waiting_mjcs_.begin(); it != waiting_mjcs_.end(); )
00490     {
00491         std::string name = it->first;
00492         std::set<std::string> deps(it->second.begin(), it->second.end());
00493         
00494         if (std::includes(loaded_controllers.begin(), loaded_controllers.end(), deps.begin(), deps.end()))
00495         {
00496             ROS_DEBUG("All dependencies are loaded for multi-joint controller %s", name.c_str());
00497             
00498             std::vector<controller::SingleJointController*> dependencies;
00499             std::set<std::string>::iterator d_it;
00500             
00501             for (d_it = deps.begin(); d_it != deps.end(); ++d_it)
00502             {
00503                 dependencies.push_back(sj_controllers_[*d_it]);
00504             }
00505             
00506             controller::MultiJointController* c = NULL;
00507             std::string type;
00508             
00509             if (nh_.getParam(name + "/type", type))
00510             {
00511                 ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
00512                 
00513                 try
00514                 {
00515                     c = mjc_loader_->createClassInstance(type, true);
00516                 }
00517                 catch (const std::runtime_error &ex)
00518                 {
00519                     ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
00520                 }
00521             }
00522             
00523             // checks if controller was constructed
00524             if (c == NULL)
00525             {
00526                 if (type == "")
00527                 {
00528                     ROS_ERROR("Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server?", name.c_str());
00529                 }
00530                 else
00531                 {
00532                     ROS_ERROR("Could not load controller '%s' because controller type '%s' does not exist", name.c_str(), type.c_str());
00533                 }
00534                 
00535                 ++it;
00536                 continue;
00537             }
00538             
00539             // Initializes the controller
00540             ROS_DEBUG("Initializing controller '%s'", name.c_str());
00541             bool initialized = false;
00542             
00543             try
00544             {
00545                 initialized = c->initialize(name, dependencies);
00546             }
00547             catch(std::exception &e)
00548             {
00549                 ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
00550                 initialized = false;
00551             }
00552             catch(...)
00553             {
00554                 ROS_ERROR("Exception thrown while initializing controller %s", name.c_str());
00555                 initialized = false;
00556             }
00557             
00558             if (!initialized)
00559             {
00560                 delete c;
00561                 ROS_ERROR("Initializing controller '%s' failed", name.c_str());
00562                 ++it;
00563                 continue;
00564             }
00565             
00566             c->start();
00567             
00568             mj_controllers_[name] = c;
00569             mj_waiting_controllers_.erase(name);
00570             waiting_mjcs_.erase(it++);
00571             
00572             ROS_DEBUG("Initialized controller '%s' succesful", name.c_str());
00573         }
00574         else
00575         {
00576             std::vector<std::string> diff(deps.size());
00577             std::set_difference(deps.begin(), deps.end(), loaded_controllers.begin(), loaded_controllers.end(), diff.begin());
00578             
00579             std::string still_waiting = "[";
00580             
00581             for (size_t i = 0; i < diff.size(); ++i)
00582             {
00583                 if (!diff[i].empty()) { still_waiting += diff[i] + ", "; }
00584             }
00585             
00586             if (diff.empty()) { still_waiting = ""; }
00587             else { still_waiting = still_waiting.erase(still_waiting.size() - 2) + "]"; }
00588             
00589             ROS_INFO("Multi-joint controller %s is still waiting for %s single-joint controllers", name.c_str(), still_waiting.c_str());
00590             
00591             ++it;
00592         }
00593     }
00594 }
00595 
00596 bool ControllerManager::startControllerSrv(dynamixel_hardware_interface::StartController::Request& req,
00597                                            dynamixel_hardware_interface::StartController::Response& res)
00598 {
00599     std::string name = req.name;
00600     
00601     if (name.empty())
00602     {
00603         ROS_ERROR("Controller name is not specified");
00604         res.success = false;
00605         return false;
00606     }
00607     
00608     ROS_DEBUG("Start service called for controller %s ", name.c_str());
00609     boost::mutex::scoped_lock s_guard(services_lock_);
00610     ROS_DEBUG("Start service locked");
00611     
00612     if (!startController(name, req.port))
00613     {
00614         res.success = false;
00615         return false;
00616     }
00617     
00618     ROS_DEBUG("Controller %s successfully started", name.c_str());
00619     res.success = true;
00620     return true;
00621 }
00622 
00623 bool ControllerManager::stopControllerSrv(dynamixel_hardware_interface::StopController::Request& req,
00624                                           dynamixel_hardware_interface::StopController::Response& res)
00625 {
00626     std::string name = req.name;
00627     
00628     if (name.empty())
00629     {
00630         ROS_ERROR("Controller name is not specified");
00631         res.success = false;
00632         return false;
00633     }
00634     
00635     ROS_DEBUG("Stop service called for controller %s ", name.c_str());
00636     boost::mutex::scoped_lock s_guard(services_lock_);
00637     ROS_DEBUG("Stop service locked");
00638     
00639     if (!stopController(name))
00640     {
00641         res.success = false;
00642         return false;
00643     }
00644     
00645     ROS_DEBUG("Controller %s successfully stopped", name.c_str());
00646     res.success = true;
00647     return true;
00648 }
00649 
00650 bool ControllerManager::restartControllerSrv(dynamixel_hardware_interface::RestartController::Request& req,
00651                                              dynamixel_hardware_interface::RestartController::Response& res)
00652 {
00653     std::string name = req.name;
00654     
00655     if (name.empty())
00656     {
00657         ROS_ERROR("Controller name is not specified");
00658         res.success = false;
00659         return false;
00660     }
00661     
00662     ROS_DEBUG("Restart service called for controller %s ", name.c_str());
00663     boost::mutex::scoped_lock s_guard(services_lock_);
00664     ROS_DEBUG("Restart service locked");
00665 
00666     if (!restartController(name))
00667     {
00668         res.success = false;
00669         return false;
00670     }
00671     
00672     res.success = true;
00673     return true;
00674 }
00675 
00676 }
00677 
00678 int main(int argc, char** argv)
00679 {
00680     ros::init(argc, argv, "dynamixel_controller_manager");
00681     dynamixel_controller_manager::ControllerManager cm;
00682     ros::spin();
00683     return 1;
00684 }


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10