ros_chain.cpp
Go to the documentation of this file.
00001 #include <ros/package.h>
00002 
00003 #include <canopen_chain_node/ros_chain.h>
00004 
00005 #include <std_msgs/Int8.h>
00006 #include <std_msgs/Int16.h>
00007 #include <std_msgs/Int32.h>
00008 #include <std_msgs/Int64.h>
00009 #include <std_msgs/UInt8.h>
00010 #include <std_msgs/UInt16.h>
00011 #include <std_msgs/UInt32.h>
00012 #include <std_msgs/UInt64.h>
00013 #include <std_msgs/Float32.h>
00014 #include <std_msgs/Float64.h>
00015 #include <std_msgs/String.h>
00016 
00017 namespace canopen {
00018 
00019 PublishFunc::func_type PublishFunc::create(ros::NodeHandle &nh,  const std::string &name, boost::shared_ptr<canopen::Node> node, const std::string &key, bool force){
00020     boost::shared_ptr<ObjectStorage> s = node->getStorage();
00021 
00022     switch(ObjectDict::DataTypes(s->dict_->get(key)->data_type)){
00023         case ObjectDict::DEFTYPE_INTEGER8:       return create< std_msgs::Int8    >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER8>::type>(key), force);
00024         case ObjectDict::DEFTYPE_INTEGER16:      return create< std_msgs::Int16   >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER16>::type>(key), force);
00025         case ObjectDict::DEFTYPE_INTEGER32:      return create< std_msgs::Int32   >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER32>::type>(key), force);
00026         case ObjectDict::DEFTYPE_INTEGER64:      return create< std_msgs::Int64   >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER64>::type>(key), force);
00027 
00028         case ObjectDict::DEFTYPE_UNSIGNED8:      return create< std_msgs::UInt8   >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED8>::type>(key), force);
00029         case ObjectDict::DEFTYPE_UNSIGNED16:     return create< std_msgs::UInt16  >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16>::type>(key), force);
00030         case ObjectDict::DEFTYPE_UNSIGNED32:     return create< std_msgs::UInt32  >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED32>::type>(key), force);
00031         case ObjectDict::DEFTYPE_UNSIGNED64:     return create< std_msgs::UInt64  >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED64>::type>(key), force);
00032 
00033         case ObjectDict::DEFTYPE_REAL32:         return create< std_msgs::Float32 >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL32>::type>(key), force);
00034         case ObjectDict::DEFTYPE_REAL64:         return create< std_msgs::Float64 >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL64>::type>(key), force);
00035 
00036         case ObjectDict::DEFTYPE_VISIBLE_STRING: return create< std_msgs::String  >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_VISIBLE_STRING>::type>(key), force);
00037         case ObjectDict::DEFTYPE_OCTET_STRING:   return create< std_msgs::String  >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN>::type>(key), force);
00038         case ObjectDict::DEFTYPE_UNICODE_STRING: return create< std_msgs::String  >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNICODE_STRING>::type>(key), force);
00039         case ObjectDict::DEFTYPE_DOMAIN:         return create< std_msgs::String  >(nh, name, s->entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN>::type>(key), force);
00040 
00041         default: return 0;
00042     }
00043 }
00044 
00045 void RosChain::logState(const can::State &s){
00046     boost::shared_ptr<can::DriverInterface> interface = interface_;
00047     std::string msg;
00048     if(interface && !interface->translateError(s.internal_error, msg)) msg  =  "Undefined"; ;
00049     ROS_INFO_STREAM("Current state: " << s.driver_state << " device error: " << s.error_code << " internal_error: " << s.internal_error << " (" << msg << ")");
00050 }
00051 
00052 void RosChain::run(){
00053     running_ = true;
00054     time_point abs_time = boost::chrono::high_resolution_clock::now();
00055     while(running_){
00056         LayerStatus s;
00057         try{
00058             read(s);
00059             write(s);
00060             if(!s.bounded<LayerStatus::Warn>()) ROS_ERROR_STREAM_THROTTLE(10, s.reason());
00061             else if(!s.bounded<LayerStatus::Ok>()) ROS_WARN_STREAM_THROTTLE(10, s.reason());
00062         }
00063         catch(const canopen::Exception& e){
00064             ROS_ERROR_STREAM_THROTTLE(1, boost::diagnostic_information(e));
00065         }
00066         if(!sync_){
00067             abs_time += update_duration_;
00068             boost::this_thread::sleep_until(abs_time);
00069         }
00070     }
00071 }
00072 
00073 bool RosChain::handle_init(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
00074     ROS_INFO("Initializing XXX");
00075     boost::mutex::scoped_lock lock(mutex_);
00076     if(getLayerState() > Off){
00077         res.success = true;
00078         res.message = "already initialized";
00079         return true;
00080     }
00081     thread_.reset(new boost::thread(&RosChain::run, this));
00082     LayerReport status;
00083     try{
00084         init(status);
00085         res.success = status.bounded<LayerStatus::Ok>();
00086         res.message = status.reason();
00087         if(!status.bounded<LayerStatus::Warn>()){
00088             diag(status);
00089             res.message = status.reason();
00090         }else{
00091             heartbeat_timer_.restart();
00092             return true;
00093         }
00094     }
00095     catch( const std::exception &e){
00096         std::string info = boost::diagnostic_information(e);
00097         ROS_ERROR_STREAM(info);
00098         res.message = info;
00099         status.error(res.message);
00100     }
00101     catch(...){
00102         res.message = "Unknown exception";
00103         status.error(res.message);
00104     }
00105 
00106     res.success = false;
00107     shutdown(status);
00108 
00109     return true;
00110 }
00111 bool RosChain::handle_recover(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
00112     ROS_INFO("Recovering XXX");
00113     boost::mutex::scoped_lock lock(mutex_);
00114     res.success = false;
00115 
00116     if(getLayerState() > Init){
00117         LayerReport status;
00118         try{
00119             if(!reset_errors_before_recover_ || emcy_handlers_->callFunc<LayerStatus::Warn>(&EMCYHandler::resetErrors, status)){
00120                 recover(status);
00121             }
00122             if(!status.bounded<LayerStatus::Warn>()){
00123                 diag(status);
00124             }
00125             res.success = status.bounded<LayerStatus::Warn>();
00126             res.message = status.reason();
00127         }
00128         catch( const std::exception &e){
00129             std::string info = boost::diagnostic_information(e);
00130             ROS_ERROR_STREAM(info);
00131             res.message = info;
00132         }
00133         catch(...){
00134             res.message = "Unknown exception";
00135         }
00136     }else{
00137         res.message = "not running";
00138     }
00139     return true;
00140 }
00141 
00142 void RosChain::handleWrite(LayerStatus &status, const LayerState &current_state) {
00143     LayerStack::handleWrite(status, current_state);
00144     if(current_state > Shutdown){
00145         for(std::vector<boost::function<void() > >::iterator it = publishers_.begin(); it != publishers_.end(); ++it) (*it)();
00146     }
00147 }
00148 
00149 void RosChain::handleShutdown(LayerStatus &status){
00150     boost::mutex::scoped_lock lock(diag_mutex_);
00151     heartbeat_timer_.stop();
00152     LayerStack::handleShutdown(status);
00153     if(running_){
00154         running_ = false;
00155         thread_->interrupt();
00156         thread_->join();
00157         thread_.reset();
00158     }
00159 }
00160 
00161 bool RosChain::handle_shutdown(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
00162     ROS_INFO("Shuting down XXX");
00163     boost::mutex::scoped_lock lock(mutex_);
00164     res.success = true;
00165     if(getLayerState() > Init){
00166         LayerStatus s;
00167         halt(s);
00168         shutdown(s);
00169     }else{
00170         res.message = "not running";
00171     }
00172     return true;
00173 }
00174 
00175 bool RosChain::handle_halt(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
00176     ROS_INFO("Halting down XXX");
00177     boost::mutex::scoped_lock lock(mutex_);
00178      res.success = true;
00179      if(getLayerState() > Init){
00180         LayerStatus s;
00181         halt(s);
00182     }else{
00183         res.message = "not running";
00184     }
00185     return true;
00186 }
00187 
00188 bool RosChain::handle_get_object(canopen_chain_node::GetObject::Request  &req, canopen_chain_node::GetObject::Response &res){
00189     std::map<std::string, boost::shared_ptr<canopen::Node> >::iterator it = nodes_lookup_.find(req.node);
00190     if(it == nodes_lookup_.end()){
00191         res.message = "node not found";
00192     }else{
00193         try {
00194             res.value = it->second->getStorage()->getStringReader(canopen::ObjectDict::Key(req.object), req.cached)();
00195             res.success = true;
00196         } catch(std::exception& e) {
00197             res.message = boost::diagnostic_information(e);
00198         }
00199     }
00200     return true;
00201 }
00202 
00203 bool RosChain::handle_set_object(canopen_chain_node::SetObject::Request  &req, canopen_chain_node::SetObject::Response &res){
00204     std::map<std::string, boost::shared_ptr<canopen::Node> >::iterator it = nodes_lookup_.find(req.node);
00205     if(it == nodes_lookup_.end()){
00206         res.message = "node not found";
00207     }else{
00208         try {
00209             it->second->getStorage()->getStringWriter(canopen::ObjectDict::Key(req.object), req.cached)(req.value);
00210             res.success = true;
00211         } catch(std::exception& e) {
00212             res.message = boost::diagnostic_information(e);
00213         }
00214     }
00215     return true;
00216 }
00217 
00218 bool RosChain::setup_bus(){
00219     ros::NodeHandle bus_nh(nh_priv_,"bus");
00220     std::string can_device;
00221     std::string driver_plugin;
00222     std::string master_alloc;
00223     bool loopback;
00224 
00225     if(!bus_nh.getParam("device",can_device)){
00226         ROS_ERROR("Device not set");
00227         return false;
00228     }
00229 
00230     bus_nh.param("loopback",loopback, false);
00231 
00232     bus_nh.param("driver_plugin",driver_plugin, std::string("can::SocketCANInterface"));
00233 
00234     try{
00235         interface_ =  driver_loader_.createInstance(driver_plugin);
00236     }
00237 
00238     catch(pluginlib::PluginlibException& ex){
00239         ROS_ERROR_STREAM(ex.what());
00240         return false;
00241     }
00242 
00243     state_listener_ = interface_->createStateListener(can::StateInterface::StateDelegate(this, &RosChain::logState));
00244 
00245     if(bus_nh.getParam("master_type",master_alloc)){
00246         ROS_ERROR("please migrate to master allocators");
00247         return false;
00248     }
00249 
00250     bus_nh.param("master_allocator",master_alloc, std::string("canopen::SimpleMaster::Allocator"));
00251 
00252     if(master_alloc == "canopen::LocalMaster::Allocator" || master_alloc == "canopen::SharedMaster::Allocator" || master_alloc == "canopen::UnrestrictedMaster::Allocator"){
00253         ROS_WARN_STREAM(master_alloc << " is going be removed, please consider using canopen::ExternalMaster::Allocator in combination with canopen_chain_sync");
00254     }
00255     try{
00256         master_= master_allocator_.allocateInstance(master_alloc, can_device, interface_);
00257     }
00258     catch( const std::exception &e){
00259         std::string info = boost::diagnostic_information(e);
00260         ROS_ERROR_STREAM(info);
00261         return false;
00262     }
00263 
00264     if(!master_){
00265         ROS_ERROR_STREAM("Could not allocate master.");
00266         return false;
00267     }
00268 
00269     add(boost::make_shared<CANLayer>(interface_, can_device, loopback));
00270 
00271     return true;
00272 }
00273 
00274 bool RosChain::setup_sync(){
00275     ros::NodeHandle sync_nh(nh_priv_,"sync");
00276 
00277     int sync_ms = 0;
00278     int sync_overflow = 0;
00279 
00280     if(!sync_nh.getParam("interval_ms", sync_ms)){
00281         ROS_WARN("Sync interval was not specified, so sync is disabled per default");
00282     }
00283 
00284     if(sync_ms < 0){
00285         ROS_ERROR_STREAM("Sync interval  "<< sync_ms << " is invalid");
00286         return false;
00287     }
00288 
00289     int update_ms = sync_ms;
00290     if(sync_ms == 0) nh_priv_.getParam("update_ms", update_ms);
00291     if(update_ms == 0){
00292         ROS_ERROR_STREAM("Update interval  "<< sync_ms << " is invalid");
00293         return false;
00294     }else{
00295         update_duration_ = boost::chrono::milliseconds(update_ms);
00296     }
00297 
00298     if(sync_ms){
00299         if(!sync_nh.getParam("overflow", sync_overflow)){
00300             ROS_WARN("Sync overflow was not specified, so overflow is disabled per default");
00301         }
00302         if(sync_overflow == 1 || sync_overflow > 240){
00303             ROS_ERROR_STREAM("Sync overflow  "<< sync_overflow << " is invalid");
00304             return false;
00305         }
00306         if(sync_nh.param("silence_us", 0) != 0){
00307             ROS_WARN("silence_us is not supported anymore");
00308         }
00309 
00310         // TODO: parse header
00311         sync_ = master_->getSync(SyncProperties(can::MsgHeader(0x80), sync_ms, sync_overflow));
00312 
00313         if(!sync_ && sync_ms){
00314             ROS_ERROR_STREAM("Initializing sync master failed");
00315             return false;
00316         }
00317         add(sync_);
00318     }
00319     return true;
00320 }
00321 
00322 bool RosChain::setup_heartbeat(){
00323         ros::NodeHandle hb_nh(nh_priv_,"heartbeat");
00324         std::string msg;
00325         double rate = 0;
00326 
00327         bool got_any = hb_nh.getParam("msg", msg);
00328         got_any = hb_nh.getParam("rate", rate) || got_any;
00329 
00330         if( !got_any) return true; // nothing todo
00331 
00332         if(rate <=0 ){
00333             ROS_ERROR_STREAM("Rate '"<< rate << "' is invalid");
00334             return false;
00335         }
00336 
00337         hb_sender_.frame = can::toframe(msg);
00338 
00339 
00340         if(!hb_sender_.frame.isValid()){
00341             ROS_ERROR_STREAM("Message '"<< msg << "' is invalid");
00342             return false;
00343         }
00344 
00345         hb_sender_.interface = interface_;
00346 
00347         heartbeat_timer_.start(Timer::TimerDelegate(&hb_sender_, &HeartbeatSender::send) , boost::chrono::duration<double>(1.0/rate), false);
00348 
00349         return true;
00350 
00351 
00352 }
00353 std::pair<std::string, bool> parseObjectName(std::string obj_name){
00354     size_t pos = obj_name.find('!');
00355     bool force = pos != std::string::npos;
00356     if(force) obj_name.erase(pos);
00357     return std::make_pair(obj_name, force);
00358 }
00359 
00360 bool addLoggerEntries(XmlRpc::XmlRpcValue merged, const std::string param, uint8_t level, Logger &logger){
00361     if(merged.hasMember(param)){
00362         try{
00363             XmlRpc::XmlRpcValue objs = merged[param];
00364             for(int i = 0; i < objs.size(); ++i){
00365                 std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);
00366 
00367                 if(!logger.add(level, obj_name.first, obj_name.second)){
00368                     ROS_ERROR_STREAM("Could not create logger for '" << obj_name.first << "'");
00369                     return false;
00370                 }
00371             }
00372         }
00373         catch(...){
00374             ROS_ERROR_STREAM("Could not parse " << param << " parameter");
00375             return false;
00376         }
00377     }
00378     return true;
00379 }
00380 
00381 bool RosChain::setup_nodes(){
00382     nodes_.reset(new canopen::LayerGroupNoDiag<canopen::Node>("301 layer"));
00383     add(nodes_);
00384 
00385     emcy_handlers_.reset(new canopen::LayerGroupNoDiag<canopen::EMCYHandler>("EMCY layer"));
00386 
00387     XmlRpc::XmlRpcValue nodes;
00388     if(!nh_priv_.getParam("nodes", nodes)){
00389         ROS_WARN("falling back to 'modules', please switch to 'nodes'");
00390         nh_priv_.getParam("modules", nodes);
00391     }
00392     MergedXmlRpcStruct defaults;
00393     nh_priv_.getParam("defaults", defaults);
00394 
00395     if(nodes.getType() ==  XmlRpc::XmlRpcValue::TypeArray){
00396         XmlRpc::XmlRpcValue new_stuct;
00397         for(size_t i = 0; i < nodes.size(); ++i){
00398             if(nodes[i].hasMember("name")){
00399                 std::string &name = nodes[i]["name"];
00400                 new_stuct[name] = nodes[i];
00401             }else{
00402                 ROS_ERROR_STREAM("Node at list index " << i << " has no name");
00403                 return false;
00404             }
00405         }
00406         nodes = new_stuct;
00407     }
00408 
00409     for(XmlRpc::XmlRpcValue::iterator it = nodes.begin(); it != nodes.end(); ++it){
00410         int node_id;
00411         try{
00412             node_id = it->second["id"];
00413         }
00414         catch(...){
00415             ROS_ERROR_STREAM("Node '" << it->first  << "' has no id");
00416             return false;
00417         }
00418         MergedXmlRpcStruct merged(it->second, defaults);
00419 
00420         if(!it->second.hasMember("name")){
00421             merged["name"]=it->first;
00422         }
00423 
00424         ObjectDict::Overlay overlay;
00425         if(merged.hasMember("dcf_overlay")){
00426             XmlRpc::XmlRpcValue dcf_overlay = merged["dcf_overlay"];
00427             if(dcf_overlay.getType() != XmlRpc::XmlRpcValue::TypeStruct){
00428                 ROS_ERROR_STREAM("dcf_overlay is no struct");
00429                 return false;
00430             }
00431             for(XmlRpc::XmlRpcValue::iterator ito = dcf_overlay.begin(); ito!= dcf_overlay.end(); ++ito){
00432                 if(ito->second.getType() != XmlRpc::XmlRpcValue::TypeString){
00433                     ROS_ERROR_STREAM("dcf_overlay '" << ito->first << "' must be string");
00434                     return false;
00435                 }
00436                 overlay.push_back(ObjectDict::Overlay::value_type(ito->first, ito->second));
00437             }
00438 
00439         }
00440 
00441         std::string eds;
00442 
00443         try{
00444             eds = (std::string) merged["eds_file"];
00445         }
00446         catch(...){
00447             ROS_ERROR_STREAM("EDS path '" << eds << "' invalid");
00448             return false;
00449         }
00450 
00451         try{
00452             if(merged.hasMember("eds_pkg")){
00453                 std::string pkg = merged["eds_pkg"];
00454                 std::string p = ros::package::getPath(pkg);
00455                 if(p.empty()){
00456                         ROS_WARN_STREAM("Package '" << pkg << "' was not found");
00457                 }else{
00458                     eds = (boost::filesystem::path(p)/eds).make_preferred().native();;
00459                 }
00460             }
00461         }
00462         catch(...){
00463         }
00464 
00465         boost::shared_ptr<ObjectDict>  dict = ObjectDict::fromFile(eds, overlay);
00466         if(!dict){
00467             ROS_ERROR_STREAM("EDS '" << eds << "' could not be parsed");
00468             return false;
00469         }
00470         boost::shared_ptr<canopen::Node> node = boost::make_shared<canopen::Node>(interface_, dict, node_id, sync_);
00471 
00472         boost::shared_ptr<Logger> logger = boost::make_shared<Logger>(node);
00473 
00474         if(!nodeAdded(merged, node, logger)) return false;
00475 
00476         if(!addLoggerEntries(merged, "log", diagnostic_updater::DiagnosticStatusWrapper::OK, *logger)) return false;
00477         if(!addLoggerEntries(merged, "log_warn", diagnostic_updater::DiagnosticStatusWrapper::WARN, *logger)) return false;
00478         if(!addLoggerEntries(merged, "log_error", diagnostic_updater::DiagnosticStatusWrapper::ERROR, *logger)) return false;
00479 
00480         loggers_.push_back(logger);
00481         diag_updater_.add(it->first, boost::bind(&Logger::log, logger, _1));
00482 
00483         std::string node_name = std::string(merged["name"]);
00484 
00485         if(merged.hasMember("publish")){
00486             try{
00487                 XmlRpc::XmlRpcValue objs = merged["publish"];
00488                 for(int i = 0; i < objs.size(); ++i){
00489                     std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);
00490 
00491                     boost::function<void()> pub = PublishFunc::create(nh_, node_name +"_"+obj_name.first, node, obj_name.first, obj_name.second);
00492                     if(!pub){
00493                         ROS_ERROR_STREAM("Could not create publisher for '" << obj_name.first << "'");
00494                         return false;
00495                     }
00496                     publishers_.push_back(pub);
00497                 }
00498             }
00499             catch(...){
00500                 ROS_ERROR("Could not parse publish parameter");
00501                 return false;
00502             }
00503         }
00504         nodes_->add(node);
00505         nodes_lookup_.insert(std::make_pair(node_name, node));
00506 
00507         boost::shared_ptr<canopen::EMCYHandler> emcy = boost::make_shared<canopen::EMCYHandler>(interface_, node->getStorage());
00508         emcy_handlers_->add(emcy);
00509         logger->add(emcy);
00510 
00511     }
00512     return true;
00513 }
00514 bool RosChain::nodeAdded(XmlRpc::XmlRpcValue &params, const boost::shared_ptr<canopen::Node> &node, const boost::shared_ptr<Logger> &logger){
00515     return true;
00516 }
00517 
00518 void RosChain::report_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat){
00519     boost::mutex::scoped_lock lock(diag_mutex_);
00520     LayerReport r;
00521     if(getLayerState() == Off){
00522         stat.summary(stat.WARN,"Not initailized");
00523     }else if(!running_){
00524         stat.summary(stat.ERROR,"Thread is not running");
00525     }else{
00526         diag(r);
00527         if(r.bounded<LayerStatus::Unbounded>()){ // valid
00528             stat.summary(r.get(), r.reason());
00529             for(std::vector<std::pair<std::string, std::string> >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){
00530                 stat.add(it->first, it->second);
00531             }
00532         }
00533     }
00534 }
00535 
00536 RosChain::RosChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv)
00537 : LayerStack("ROS stack"),driver_loader_("socketcan_interface", "can::DriverInterface"),
00538   master_allocator_("canopen_master", "canopen::Master::Allocator"),
00539   nh_(nh), nh_priv_(nh_priv),
00540   diag_updater_(nh_,nh_priv_),
00541   running_(false),
00542   reset_errors_before_recover_(false){}
00543 
00544 bool RosChain::setup(){
00545     boost::mutex::scoped_lock lock(mutex_);
00546     bool okay = setup_chain();
00547     if(okay) add(emcy_handlers_);
00548     return okay;
00549 }
00550 
00551 bool RosChain::setup_chain(){
00552     std::string hw_id;
00553     nh_priv_.param("hardware_id", hw_id, std::string("none"));
00554     nh_priv_.param("reset_errors_before_recover", reset_errors_before_recover_, false);
00555 
00556     diag_updater_.setHardwareID(hw_id);
00557     diag_updater_.add("chain", this, &RosChain::report_diagnostics);
00558 
00559     diag_timer_ = nh_.createTimer(ros::Duration(diag_updater_.getPeriod()/2.0),boost::bind(&diagnostic_updater::Updater::update, &diag_updater_));
00560 
00561     ros::NodeHandle nh_driver(nh_, "driver");
00562 
00563     srv_init_ = nh_driver.advertiseService("init",&RosChain::handle_init, this);
00564     srv_recover_ = nh_driver.advertiseService("recover",&RosChain::handle_recover, this);
00565     srv_halt_ = nh_driver.advertiseService("halt",&RosChain::handle_halt, this);
00566     srv_shutdown_ = nh_driver.advertiseService("shutdown",&RosChain::handle_shutdown, this);
00567 
00568     srv_get_object_ = nh_driver.advertiseService("get_object",&RosChain::handle_get_object, this);
00569     srv_set_object_ = nh_driver.advertiseService("set_object",&RosChain::handle_set_object, this);
00570 
00571     return setup_bus() && setup_sync() && setup_heartbeat() && setup_nodes();
00572 }
00573 
00574 RosChain::~RosChain(){
00575     try{
00576         LayerStatus s;
00577         halt(s);
00578         shutdown(s);
00579     }catch(...){ LOG("CATCH"); }
00580 }
00581 
00582 }


canopen_chain_node
Author(s): Mathias Lüdtke
autogenerated on Sun Sep 3 2017 03:10:52