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 ¤t_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
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;
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 ¶ms, 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>()){
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 }