asebaros.cpp
Go to the documentation of this file.
00001 #include "asebaros.h"
00002 
00003 #include <aseba/compiler/compiler.h>
00004 
00005 #include <ros/console.h>
00006 
00007 #include <vector>
00008 #include <sstream>
00009 #include <boost/format.hpp>
00010 
00011 #include <libxml/parser.h>
00012 #include <libxml/tree.h>
00013 
00014 
00015 using namespace asebaros;
00016 using namespace std;
00017 using namespace boost;
00018 using namespace Dashel;
00019 using namespace Aseba;
00020 
00021 // UTF8 to wstring
00022 std::wstring widen(const char *src)
00023 {
00024         const size_t destSize(mbstowcs(0, src, 0)+1);
00025         std::vector<wchar_t> buffer(destSize, 0);
00026         mbstowcs(&buffer[0], src, destSize);
00027         return std::wstring(buffer.begin(), buffer.end() - 1);
00028 }
00029 std::wstring widen(const std::string& src)
00030 {
00031         return widen(src.c_str());
00032 }
00033 
00034 // wstring to UTF8
00035 std::string narrow(const wchar_t* src)
00036 {
00037         const size_t destSize(wcstombs(0, src, 0)+1);
00038         std::vector<char> buffer(destSize, 0);
00039         wcstombs(&buffer[0], src, destSize);
00040         return std::string(buffer.begin(), buffer.end() - 1);
00041 }
00042 std::string narrow(const std::wstring& src)
00043 {
00044         return narrow(src.c_str());
00045 }
00046 
00047 
00048 // AsebaDashelHub
00049 
00050 AsebaDashelHub::AsebaDashelHub(AsebaROS* asebaROS, unsigned port, bool forward):
00051         Dashel::Hub(),
00052         asebaROS(asebaROS),
00053         forward(forward)
00054 {
00055         ostringstream oss;
00056         oss << "tcpin:port=" << port;
00057         Dashel::Hub::connect(oss.str());
00058 }
00059 
00060 static wstring asebaMsgToString(Message *message)
00061 {
00062         wostringstream oss;
00063         message->dump(oss);
00064         return oss.str();
00065 }
00066 
00067 void AsebaDashelHub::sendMessage(Message *message, bool doLock, Stream* sourceStream)
00068 {
00069         // dump if requested
00070         // TODO: put a request for unicode version of ROS debug
00071         ROS_DEBUG_STREAM("sending aseba message: " << narrow(asebaMsgToString(message)));
00072         
00073         // Might be called from the ROS thread, not the Hub thread, need to lock
00074         if (doLock)
00075                 lock();
00076 
00077         // write on all connected streams
00078         for (StreamsSet::iterator it = dataStreams.begin(); it != dataStreams.end();++it)
00079         {
00080                 Stream* destStream(*it);
00081                 
00082                 if ((forward) && (destStream == sourceStream))
00083                         continue;
00084                 
00085                 try
00086                 {
00087                         message->serialize(destStream);
00088                         destStream->flush();
00089                 }
00090                 catch (DashelException e)
00091                 {
00092                         // if this stream has a problem, ignore it for now, and let Hub call connectionClosed later.
00093                         ROS_ERROR("error while writing message");
00094                 }
00095         }
00096 
00097         if (doLock)
00098                 unlock();
00099 }
00100 
00101 void AsebaDashelHub::operator()()
00102 {
00103         Hub::run();
00104         //cerr << "hub returned" << endl;
00105         ros::shutdown();
00106 }
00107 
00108 void AsebaDashelHub::startThread()
00109 {
00110         thread = new boost::thread(ref(*this));
00111 }
00112 
00113 void AsebaDashelHub::stopThread()
00114 {
00115         Hub::stop();
00116         thread->join();
00117         delete thread;
00118         thread = 0;
00119 }
00120 
00121 // the following method run in the blocking reception thread
00122         
00123 void AsebaDashelHub::incomingData(Stream *stream)
00124 {
00125         // receive message
00126         Message *message = 0;
00127         try
00128         {
00129                 message = Message::receive(stream);
00130         }
00131         catch (DashelException e)
00132         {
00133                 // if this stream has a problem, ignore it for now, and let Hub call connectionClosed later.
00134                 ROS_ERROR("error while writing message");
00135         }
00136         
00137         // send message to Dashel peers
00138         sendMessage(message, false, stream);
00139         
00140         // process message for ROS peers, the receiver will delete it
00141         asebaROS->processAsebaMessage(message);
00142         
00143         // free the message
00144         delete message;
00145 }
00146 
00147 void AsebaDashelHub::connectionCreated(Stream *stream)
00148 {
00149         ROS_INFO_STREAM("Incoming connection from " << stream->getTargetName());
00150         
00151         if (dataStreams.size() == 1)
00152         {
00153                 // Note: on some robot such as the marXbot, because of hardware
00154                 // constraints this might not work. In this case, an external
00155                 // hack is required
00156                 GetDescription getDescription;
00157                 sendMessage(&getDescription, false);
00158         }
00159 }
00160 
00161 void AsebaDashelHub::connectionClosed(Stream* stream, bool abnormal)
00162 {
00163         if (abnormal)
00164                 ROS_INFO_STREAM("Abnormal connection closed to " << stream->getTargetName() << " : " << stream->getFailReason());
00165         else
00166                 ROS_INFO_STREAM("Normal connection closed to " << stream->getTargetName());
00167 }
00168 
00169 
00170 // AsebaROS
00171 
00172 bool AsebaROS::loadScript(LoadScripts::Request& req, LoadScripts::Response& res)
00173 {
00174         // locking: in this method, we lock access to the object's members
00175         
00176         // open document
00177         const string& fileName(req.fileName);
00178         xmlDoc *doc = xmlReadFile(fileName.c_str(), NULL, 0);
00179         if (!doc)
00180         {
00181                 ROS_ERROR_STREAM("Cannot read XML from file " << fileName);
00182                 return false;
00183         }
00184     xmlNode *domRoot = xmlDocGetRootElement(doc);
00185         
00186         // clear existing data
00187         mutex.lock();
00188         commonDefinitions.events.clear();
00189         commonDefinitions.constants.clear();
00190         userDefinedVariablesMap.clear();
00191         pubs.clear();
00192         subs.clear();
00193         mutex.unlock();
00194         
00195         // load new data
00196         int noNodeCount = 0;
00197         bool wasError = false;
00198         if (!xmlStrEqual(domRoot->name, BAD_CAST("network")))
00199         {
00200                 ROS_ERROR("root node is not \"network\", XML considered as invalid");
00201                 wasError = true;
00202         }
00203         else for (xmlNode *domNode = xmlFirstElementChild(domRoot); domNode; domNode = domNode->next)
00204         {
00205                 //cerr << "node " << domNode->name << endl;
00206                 if (domNode->type == XML_ELEMENT_NODE)
00207                 {
00208                         if (xmlStrEqual(domNode->name, BAD_CAST("node")))
00209                         {
00210                                 // get attributes, child and content
00211                                 xmlChar *name = xmlGetProp(domNode, BAD_CAST("name"));
00212                                 if (!name)
00213                                         ROS_WARN("missing \"name\" attribute in \"node\" entry");
00214                                 else
00215                                 {
00216                                         const string _name((const char *)name);
00217                                         xmlChar * text = xmlNodeGetContent(domNode);
00218                                         if (!text)
00219                                                 ROS_WARN("missing text in \"node\" entry");
00220                                         else
00221                                         {
00222                                                 //cerr << text << endl;
00223                                                 unsigned preferedId(0);
00224                                                 xmlChar *storedId = xmlGetProp(domNode, BAD_CAST("nodeId"));
00225                                                 if (storedId)
00226                                                         preferedId = unsigned(atoi((char*)storedId));
00227                                                 mutex.lock();
00228                                                 bool ok;
00229                                                 unsigned nodeId(DescriptionsManager::getNodeId(widen(_name), preferedId, &ok));
00230                                                 mutex.unlock();
00231                                                 if (ok)
00232                                                 {
00233                                                         mutex.lock();
00234                                                         // compile code
00235                                                         std::wistringstream is(widen((const char *)text));
00236                                                         Error error;
00237                                                         BytecodeVector bytecode;
00238                                                         unsigned allocatedVariablesCount;
00239                                                         
00240                                                         Compiler compiler;
00241                                                         compiler.setTargetDescription(getDescription(nodeId));
00242                                                         compiler.setCommonDefinitions(&commonDefinitions);
00243                                                         bool result = compiler.compile(is, bytecode, allocatedVariablesCount, error);
00244                                                         mutex.unlock();
00245                                                         
00246                                                         if (result)
00247                                                         {
00248                                                                 typedef std::vector<Message*> MessageVector;
00249                                                                 MessageVector messages;
00250                                                                 sendBytecode(messages, nodeId, std::vector<uint16>(bytecode.begin(), bytecode.end()));
00251                                                                 for (MessageVector::const_iterator it = messages.begin(); it != messages.end(); ++it)
00252                                                                 {
00253                                                                         hub.sendMessage(*it, true);
00254                                                                         delete *it;
00255                                                                 }
00256                                                                 Run msg(nodeId);
00257                                                                 hub.sendMessage(&msg, true);
00258                                                                 // retrieve user-defined variables for use in get/set
00259                                                                 mutex.lock();
00260                                                                 userDefinedVariablesMap[_name] = *compiler.getVariablesMap();
00261                                                                 mutex.unlock();
00262                                                         }
00263                                                         else
00264                                                         {
00265                                                                 ROS_ERROR_STREAM("compilation of " << fileName << ", node " << _name << " failed: " << narrow(error.toWString()));
00266                                                                 wasError = true;
00267                                                         }
00268                                                 }
00269                                                 else
00270                                                         noNodeCount++;
00271                                                 
00272                                                 // free attribute and content
00273                                                 xmlFree(text);
00274                                         }
00275                                         xmlFree(name);
00276                                 }
00277                         }
00278                         else if (xmlStrEqual(domNode->name, BAD_CAST("event")))
00279                         {
00280                                 // get attributes
00281                                 xmlChar *name = xmlGetProp(domNode, BAD_CAST("name"));
00282                                 if (!name)
00283                                         ROS_WARN("missing \"name\" attribute in \"event\" entry");
00284                                 xmlChar *size = xmlGetProp(domNode, BAD_CAST("size")); 
00285                                 if (!size)
00286                                         ROS_WARN("missing \"size\" attribute in \"event\" entry");
00287                                 // add event
00288                                 if (name && size)
00289                                 {
00290                                         int eventSize(atoi((const char *)size));
00291                                         if (eventSize > ASEBA_MAX_EVENT_ARG_SIZE)
00292                                         {
00293                                                 ROS_ERROR("Event %s has a length %d larger than maximum %d", name, eventSize, ASEBA_MAX_EVENT_ARG_SIZE);
00294                                                 wasError = true;
00295                                                 break;
00296                                         }
00297                                         else
00298                                         {
00299                                                 lock_guard<boost::mutex> lock(mutex);
00300                                                 commonDefinitions.events.push_back(NamedValue(widen((const char *)name), eventSize));
00301                                         }
00302                                 }
00303                                 // free attributes
00304                                 if (name)
00305                                         xmlFree(name);
00306                                 if (size)
00307                                         xmlFree(size);
00308                         }
00309                         else if (xmlStrEqual(domNode->name, BAD_CAST("constant")))
00310                         {
00311                                 // get attributes
00312                                 xmlChar *name = xmlGetProp(domNode, BAD_CAST("name"));
00313                                 if (!name)
00314                                         ROS_WARN("missing \"name\" attribute in \"constant\" entry");
00315                                 xmlChar *value = xmlGetProp(domNode, BAD_CAST("value")); 
00316                                 if (!value)
00317                                         ROS_WARN("missing \"value\" attribute in \"constant\" entry");
00318                                 // add constant if attributes are valid
00319                                 if (name && value)
00320                                 {
00321                                         lock_guard<boost::mutex> lock(mutex);
00322                                         commonDefinitions.constants.push_back(NamedValue(widen((const char *)name), atoi((const char *)value)));
00323                                 }
00324                                 // free attributes
00325                                 if (name)
00326                                         xmlFree(name);
00327                                 if (value)
00328                                         xmlFree(value);
00329                         }
00330                         else
00331                                 ROS_WARN_STREAM("Unknown XML node seen in .aesl file: " << domNode->name);
00332                 }
00333         }
00334 
00335         // release memory
00336         xmlFreeDoc(doc);
00337         
00338         // check if there was an error
00339         if (wasError)
00340         {
00341                 ROS_ERROR_STREAM("There was an error while loading script " << fileName);
00342                 mutex.lock();
00343                 commonDefinitions.events.clear();
00344                 commonDefinitions.constants.clear();
00345                 userDefinedVariablesMap.clear();
00346                 mutex.unlock();
00347         }
00348         
00349         // check if there was some matching problem
00350         if (noNodeCount)
00351         {
00352                 ROS_WARN_STREAM(noNodeCount << " scripts have no corresponding nodes in the current network and have not been loaded.");
00353         }
00354         
00355         // recreate publishers and subscribers
00356         mutex.lock();
00357         typedef EventsDescriptionsVector::const_iterator EventsDescriptionsConstIt;
00358         for (size_t i = 0; i < commonDefinitions.events.size(); ++i)
00359         {
00360                 const wstring& name(commonDefinitions.events[i].name);
00361                 pubs.push_back(n.advertise<AsebaEvent>(narrow(L"events/"+name), 100));
00362                 subs.push_back(n.subscribe<AsebaEvent>(narrow(L"events/"+name), 100, bind(&AsebaROS::knownEventReceived, this, i, _1)));
00363         }
00364         mutex.unlock();
00365         
00366         return true;
00367 }
00368 
00369 bool AsebaROS::getNodeList(GetNodeList::Request& req, GetNodeList::Response& res)
00370 {
00371         lock_guard<boost::mutex> lock(mutex);
00372         
00373         transform(nodesNames.begin(), nodesNames.end(), back_inserter(res.nodeList), bind(&NodesNamesMap::value_type::first,_1));
00374         return true;
00375 }
00376 
00377 bool AsebaROS::getNodeId(GetNodeId::Request& req, GetNodeId::Response& res)
00378 {
00379         lock_guard<boost::mutex> lock(mutex);
00380         
00381         NodesNamesMap::const_iterator nodeIt(nodesNames.find(req.nodeName));
00382         if (nodeIt != nodesNames.end())
00383         {
00384                 res.nodeId = nodeIt->second;
00385                 return true;
00386         }
00387         else
00388         {
00389                 ROS_ERROR_STREAM("node " << req.nodeName << " does not exists");
00390                 return false;
00391         }
00392 }
00393 
00394 bool AsebaROS::getNodeName(GetNodeName::Request& req, GetNodeName::Response& res)
00395 {
00396         lock_guard<boost::mutex> lock(mutex);
00397         
00398         NodesDescriptionsMap::const_iterator nodeIt(nodesDescriptions.find(req.nodeId));
00399         if (nodeIt != nodesDescriptions.end())
00400         {
00401                 res.nodeName = narrow(nodeIt->second.name);
00402                 return true;
00403         }
00404         else
00405         {
00406                 ROS_ERROR_STREAM("node " << req.nodeId << " does not exists");
00407                 return false;
00408         }
00409 }
00410 
00411 struct ExtractNameVar
00412 {
00413         string operator()(const std::pair<std::wstring, std::pair<unsigned, unsigned> > p) const { return narrow(p.first); }
00414 };
00415 
00416 struct ExtractNameDesc
00417 {
00418         string operator()(const TargetDescription::NamedVariable& nv) const { return narrow(nv.name); }
00419 };
00420 
00421 bool AsebaROS::getVariableList(GetVariableList::Request& req, GetVariableList::Response& res)
00422 {
00423         lock_guard<boost::mutex> lock(mutex);
00424         
00425         NodesNamesMap::const_iterator nodeIt(nodesNames.find(req.nodeName));
00426         if (nodeIt != nodesNames.end())
00427         {
00428                 // search if we have a user-defined variable map?
00429                 const UserDefinedVariablesMap::const_iterator userVarMapIt(userDefinedVariablesMap.find(req.nodeName));
00430                 if (userVarMapIt != userDefinedVariablesMap.end())
00431                 {
00432                         // yes, us it 
00433                         const Compiler::VariablesMap& variablesMap(userVarMapIt->second);
00434                         transform(variablesMap.begin(), variablesMap.end(),
00435                                           back_inserter(res.variableList), ExtractNameVar());
00436                 }
00437                 else
00438                 {
00439                         // no, then only show node-defined variables
00440                         const unsigned nodeId(nodeIt->second);
00441                         const NodesDescriptionsMap::const_iterator descIt(nodesDescriptions.find(nodeId));
00442                         const NodeDescription& description(descIt->second);
00443                         transform(description.namedVariables.begin(), description.namedVariables.end(),
00444                                           back_inserter(res.variableList), ExtractNameDesc());
00445                 }
00446                 return true;
00447         }
00448         else
00449         {
00450                 ROS_ERROR_STREAM("node " << req.nodeName << " does not exists");
00451                 return false;
00452         }
00453 }
00454 
00455 
00456 
00457 bool AsebaROS::setVariable(SetVariable::Request& req, SetVariable::Response& res)
00458 {
00459         // lock the access to the member methods
00460         unsigned nodeId, pos;
00461         
00462         mutex.lock();
00463         bool exists = getNodePosFromNames(req.nodeName, req.variableName, nodeId, pos);
00464         mutex.unlock();
00465         
00466         if (!exists)
00467                 return false;
00468         
00469         SetVariables msg(nodeId, pos, req.data);
00470         hub.sendMessage(&msg, true);
00471         return true;
00472 }
00473 
00474 bool AsebaROS::getVariable(GetVariable::Request& req, GetVariable::Response& res)
00475 {
00476         unsigned nodeId, pos;
00477         
00478         // lock the access to the member methods, wait will unlock the underlying mutex
00479         unique_lock<boost::mutex> lock(mutex);
00480         
00481         // get information about variable
00482         bool exists = getNodePosFromNames(req.nodeName, req.variableName, nodeId, pos);
00483         if (!exists)
00484                 return false;
00485         bool ok;
00486         unsigned length = getVariableSize(nodeId, widen(req.variableName), &ok);
00487         if (!ok)
00488                 return false;
00489         
00490         // create query
00491         const GetVariableQueryKey key(nodeId, pos);
00492         GetVariableQueryValue query;
00493         getVariableQueries[key] = &query;
00494         lock.unlock();
00495         
00496         // send message, outside lock to avoid deadlocks
00497         GetVariables msg(nodeId, pos, length);
00498         hub.sendMessage(&msg, true);
00499         system_time const timeout(get_system_time()+posix_time::milliseconds(100));
00500         
00501         // wait 100 ms, considering the possibility of spurious wakes
00502         bool result;
00503         lock.lock();
00504         while (query.data.empty())
00505         {
00506                 result = query.cond.timed_wait(lock, timeout);
00507                 if (!result)
00508                         break;
00509         }
00510         
00511         // remove key and return answer
00512         getVariableQueries.erase(key);
00513         if (result)
00514         {
00515                 res.data = query.data;
00516                 return true;
00517         }
00518         else
00519         {
00520                 ROS_ERROR_STREAM("read of node " << req.nodeName << ", variable " << req.variableName << " did not return a valid answer within 100ms");
00521                 return false;
00522         }
00523 }
00524 
00525 bool AsebaROS::getEventId(GetEventId::Request& req, GetEventId::Response& res)
00526 {
00527         // needs locking, called by ROS's service thread
00528         lock_guard<boost::mutex> lock(mutex);
00529         size_t id;
00530         if (commonDefinitions.events.contains(widen(req.name), &id))
00531         {
00532                 res.id = id;
00533                 return true;
00534         }
00535         return false;
00536 }
00537 
00538 bool AsebaROS::getEventName(GetEventName::Request& req, GetEventName::Response& res)
00539 {
00540         // needs locking, called by ROS's service thread
00541         lock_guard<boost::mutex> lock(mutex);
00542         if (req.id < commonDefinitions.events.size())
00543         {
00544                 res.name = narrow(commonDefinitions.events[req.id].name);
00545                 return true;
00546         }
00547         return false;
00548 }
00549 
00550 bool AsebaROS::getNodePosFromNames(const string& nodeName, const string& variableName, unsigned& nodeId, unsigned& pos) const
00551 {
00552         // does not need locking, called by other member function already within the lock
00553         
00554         // make sure the node exists
00555         NodesNamesMap::const_iterator nodeIt(nodesNames.find(nodeName));
00556         if (nodeIt == nodesNames.end())
00557         {
00558                 ROS_ERROR_STREAM("node " << nodeName << " does not exists");
00559                 return false;
00560         }
00561         nodeId = nodeIt->second;
00562         pos = unsigned(-1);
00563         
00564         // check whether variable is user-defined
00565         const UserDefinedVariablesMap::const_iterator userVarMapIt(userDefinedVariablesMap.find(nodeName));
00566         if (userVarMapIt != userDefinedVariablesMap.end())
00567         {
00568                 const Compiler::VariablesMap& userVarMap(userVarMapIt->second);
00569                 const Compiler::VariablesMap::const_iterator userVarIt(userVarMap.find(widen(variableName)));
00570                 if (userVarIt != userVarMap.end())
00571                 {
00572                         pos = userVarIt->second.first;
00573                 }
00574         }
00575         
00576         // if variable is not user-defined, check whether it is provided by this node
00577         if (pos == unsigned(-1))
00578         {
00579                 bool ok;
00580                 pos = getVariablePos(nodeId, widen(variableName), &ok);
00581                 if (!ok)
00582                 {
00583                         ROS_ERROR_STREAM("variable " << variableName << " does not exists in node " << nodeName);
00584                         return false;
00585                 }
00586         }
00587         return true;
00588 }
00589 
00590 void AsebaROS::sendEventOnROS(const UserMessage* asebaMessage)
00591 {
00592         // does not need locking, called by other member function already within lock
00593         if ((pubs.size() == commonDefinitions.events.size()) && // if different, we are currently loading a new script, publish on anonymous channel
00594                 (asebaMessage->type < commonDefinitions.events.size()))
00595         {
00596                 // known, send on a named channel
00597                 shared_ptr<AsebaEvent> event(new AsebaEvent);
00598                 event->stamp = ros::Time::now();
00599                 event->source = asebaMessage->source;
00600                 event->data = asebaMessage->data;
00601                 pubs[asebaMessage->type].publish(event);
00602         }
00603         else
00604         {
00605                 // unknown, send on the anonymous channel
00606                 shared_ptr<AsebaAnonymousEvent> event(new AsebaAnonymousEvent);
00607                 event->stamp = ros::Time::now();
00608                 event->source = asebaMessage->source;
00609                 event->type = asebaMessage->type;
00610                 event->data = asebaMessage->data;
00611                 anonPub.publish(event);
00612         }
00613 }
00614 
00615 void AsebaROS::nodeDescriptionReceived(unsigned nodeId)
00616 {
00617         // does not need locking, called by parent object
00618         nodesNames[narrow(nodesDescriptions.at(nodeId).name)] = nodeId;
00619 }
00620 
00621 void AsebaROS::eventReceived(const AsebaAnonymousEventConstPtr& event)
00622 {
00623         // does not need locking, does not touch object's members
00624         if (event->source == 0)
00625         {
00626                 // forward only messages with source 0, which means, originating from this computer
00627                 UserMessage userMessage(event->type, event->data);
00628                 hub.sendMessage(&userMessage, true);
00629         }
00630 }
00631 
00632 void AsebaROS::knownEventReceived(const uint16 id, const AsebaEventConstPtr& event)
00633 {
00634         // does not need locking, does not touch object's members
00635         if (event->source == 0)
00636         {
00637                 // forward only messages with source 0, which means, originating from this computer
00638                 UserMessage userMessage(id, event->data);
00639                 hub.sendMessage(&userMessage, true);
00640         }
00641 }
00642 
00643 AsebaROS::AsebaROS(unsigned port, bool forward):
00644         n("aseba"),
00645         anonPub(n.advertise<AsebaAnonymousEvent>("anonymous_events", 100)),
00646         anonSub(n.subscribe("anonymous_events", 100, &AsebaROS::eventReceived, this)),
00647         hub(this, port, forward) // hub for dashel 
00648 {
00649         // does not need locking, called by main
00650         
00651         // script
00652         s.push_back(n.advertiseService("load_script", &AsebaROS::loadScript, this));
00653         
00654         // nodes
00655         s.push_back(n.advertiseService("get_node_list", &AsebaROS::getNodeList, this));
00656         s.push_back(n.advertiseService("get_node_id", &AsebaROS::getNodeId, this));
00657         s.push_back(n.advertiseService("get_node_name", &AsebaROS::getNodeName, this));
00658         
00659         // variables
00660         s.push_back(n.advertiseService("get_variable_list", &AsebaROS::getVariableList, this));
00661         s.push_back(n.advertiseService("set_variable", &AsebaROS::setVariable, this));
00662         s.push_back(n.advertiseService("get_variable", &AsebaROS::getVariable, this));
00663         
00664         // events
00665         s.push_back(n.advertiseService("get_event_id", &AsebaROS::getEventId, this));
00666         s.push_back(n.advertiseService("get_event_name", &AsebaROS::getEventName, this));
00667 }
00668 
00669 AsebaROS::~AsebaROS()
00670 {
00671         // does not need locking, called by main
00672         xmlCleanupParser();
00673 }
00674 
00675 void AsebaROS::run()
00676 {
00677         // does not need locking, called by main
00678         hub.startThread();
00679         ros::spin();
00680         //cerr << "ros returned" << endl;
00681         hub.stopThread();
00682 }
00683 
00684 void AsebaROS::processAsebaMessage(Message *message)
00685 {
00686         // needs locking, called by Dashel hub
00687         lock_guard<boost::mutex> lock(mutex);
00688         
00689         // scan this message for nodes descriptions
00690         DescriptionsManager::processMessage(message);
00691         
00692         // if user message, send to D-Bus as well
00693         UserMessage *userMessage = dynamic_cast<UserMessage *>(message);
00694         if (userMessage)
00695                 sendEventOnROS(userMessage);
00696         
00697         // if variables, check for pending answers
00698         Variables *variables = dynamic_cast<Variables *>(message);
00699         if (variables)
00700         {
00701                 const GetVariableQueryKey queryKey(variables->source, variables->start);
00702                 GetVariableQueryMap::const_iterator queryIt(getVariableQueries.find(queryKey));
00703                 if (queryIt != getVariableQueries.end())
00704                 {
00705                         queryIt->second->data = variables->variables;
00706                         queryIt->second->cond.notify_one();
00707                 }
00708                 else
00709                         ROS_WARN_STREAM("received Variables from node " << variables->source << ", pos " << variables->start << ", but no corresponding query was found");
00710         }
00711 }
00712 
00714 void dumpHelp(std::ostream &stream, const char *programName)
00715 {
00716         stream << "AsebaROS, connects aseba components together and with ROS, usage:\n";
00717         stream << programName << " [options] [additional targets]*\n";
00718         stream << "Options:\n";
00719         stream << "-l, --loop      : makes the switch transmit messages back to the send, not only forward them.\n";
00720         stream << "-p port         : listens to incoming connection on this port\n";
00721         stream << "-h, --help      : shows this help\n";
00722         stream << "ROS_OPTIONS     : see ROS documentation\n";
00723         stream << "Additional targets are any valid Dashel targets." << std::endl;
00724 }
00725 
00726 int main(int argc, char *argv[])
00727 {
00728         ros::init(argc, argv, "aseba");
00729         
00730         unsigned port = ASEBA_DEFAULT_PORT;
00731         bool forward = true;
00732         std::vector<std::string> additionalTargets;
00733         
00734         int argCounter = 1;
00735         
00736         while (argCounter < argc)
00737         {
00738                 const char *arg = argv[argCounter];
00739                 
00740                 if ((strcmp(arg, "-l") == 0) || (strcmp(arg, "--loop") == 0))
00741                 {
00742                         forward = false;
00743                 }
00744                 else if (strcmp(arg, "-p") == 0)
00745                 {
00746                         arg = argv[++argCounter];
00747                         port = atoi(arg);
00748                 }
00749                 else if ((strcmp(arg, "-h") == 0) || (strcmp(arg, "--help") == 0))
00750                 {
00751                         dumpHelp(std::cout, argv[0]);
00752                         return 0;
00753                 }
00754                 else
00755                 {
00756                         additionalTargets.push_back(argv[argCounter]);
00757                 }
00758                 argCounter++;
00759         }
00760         
00761         AsebaROS asebaROS(port, forward);
00762         
00763         try
00764         {
00765                 for (size_t i = 0; i < additionalTargets.size(); i++)
00766                         asebaROS.connectTarget(additionalTargets[i]);
00767         }
00768         catch(Dashel::DashelException e)
00769         {
00770                 std::cerr << e.what() << std::endl;
00771         }
00772         
00773         asebaROS.run();
00774 
00775         return 0;
00776 }
00777 
00778 
00779 


asebaros
Author(s): Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:17:35