medulla.cpp
Go to the documentation of this file.
00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2007--2012:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006                 and other contributors, see authors.txt for details
00007         
00008         This program is free software: you can redistribute it and/or modify
00009         it under the terms of the GNU Lesser General Public License as published
00010         by the Free Software Foundation, version 3 of the License.
00011         
00012         This program is distributed in the hope that it will be useful,
00013         but WITHOUT ANY WARRANTY; without even the implied warranty of
00014         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015         GNU Lesser General Public License for more details.
00016         
00017         You should have received a copy of the GNU Lesser General Public License
00018         along with this program. If not, see <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include <QCoreApplication>
00022 #include <cstdlib>
00023 #include <cstring>
00024 #include <iostream>
00025 #include <sstream>
00026 #include <set>
00027 #include <valarray>
00028 #include <vector>
00029 #include <iterator>
00030 #include "medulla.h"
00031 #include "medulla.moc"
00032 #include "../common/consts.h"
00033 #include "../common/types.h"
00034 #include "../utils/utils.h"
00035 #include "../transport/dashel_plugins/dashel-plugins.h"
00036 #include <QDBusMessage>
00037 #include <QDBusMetaType>
00038 #include <QtXml>
00039 #include <QtDebug>
00040 
00041 #if DASHEL_VERSION_INT < 10003
00042 #       error "You need at least Dashel version 1.0.3 to compile Medulla"
00043 #endif // DAHSEL_VERSION_INT
00044 
00045 namespace Aseba 
00046 {
00047         using namespace std;
00048         using namespace Dashel;
00049         
00052         
00053         std::vector<sint16> toAsebaVector(const Values& values)
00054         {
00055                 std::vector<sint16> data;
00056                 data.reserve(values.size());
00057                 for (int i = 0; i < values.size(); ++i)
00058                         data.push_back(values[i]);
00059                 return data;
00060         }
00061         
00062         Values fromAsebaVector(const std::vector<sint16>& values)
00063         {
00064                 Values data;
00065                 for (size_t i = 0; i < values.size(); ++i)
00066                         data.push_back(values[i]);
00067                 return data;
00068         }
00069         
00070         void EventFilterInterface::emitEvent(const quint16 id, const QString& name, const Values& data)
00071         {
00072                 emit Event(id, name, data);
00073         }
00074         
00075         void EventFilterInterface::ListenEvent(const quint16 event)
00076         {
00077                 network->listenEvent(this, event);
00078         }
00079         
00080         void EventFilterInterface::ListenEventName(const QString& name, const QDBusMessage &message)
00081         {
00082                 size_t event;
00083                 if (network->commonDefinitions.events.contains(name.toStdWString(), &event))
00084                         network->listenEvent(this, event);
00085                 else
00086                         network->DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("no event named %0").arg(name)));
00087         }
00088         
00089         void EventFilterInterface::IgnoreEvent(const quint16 event)
00090         {
00091                 network->ignoreEvent(this, event);
00092         }
00093         
00094         void EventFilterInterface::IgnoreEventName(const QString& name, const QDBusMessage &message)
00095         {
00096                 size_t event;
00097                 if (network->commonDefinitions.events.contains(name.toStdWString(), &event))
00098                         network->ignoreEvent(this, event);
00099                 else
00100                         network->DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("no event named %0").arg(name)));
00101         }
00102         
00103         void EventFilterInterface::Free()
00104         {
00105                 network->filterDestroyed(this);
00106                 deleteLater();
00107         }
00108         
00109         AsebaNetworkInterface::AsebaNetworkInterface(Hub* hub, bool systemBus) :
00110                 QDBusAbstractAdaptor(hub),
00111                 hub(hub),
00112                 systemBus(systemBus),
00113                 eventsFiltersCounter(0)
00114         {
00115                 qDBusRegisterMetaType<Values>();
00116                 
00117                 //FIXME: here no error handling is done, with system bus these calls can fail   
00118                 DBusConnectionBus().registerObject("/", hub);
00119                 DBusConnectionBus().registerService("ch.epfl.mobots.Aseba");
00120         }
00121         
00122         void AsebaNetworkInterface::processMessage(Message *message, Dashel::Stream* sourceStream)
00123         {
00124                 // send messages to Dashel peers
00125                 hub->sendMessage(message, sourceStream);
00126                 
00127                 // scan this message for nodes descriptions
00128                 DescriptionsManager::processMessage(message);
00129                 
00130                 // if user message, send to D-Bus as well
00131                 UserMessage *userMessage = dynamic_cast<UserMessage *>(message);
00132                 if (userMessage)
00133                 {
00134                         sendEventOnDBus(userMessage->type, fromAsebaVector(userMessage->data));
00135                 }
00136                 
00137                 // if variables, check for pending answers
00138                 Variables *variables = dynamic_cast<Variables *>(message);
00139                 if (variables)
00140                 {
00141                         const unsigned nodeId(variables->source);
00142                         const unsigned pos(variables->start);
00143                         for (RequestsList::iterator it = pendingReads.begin(); it != pendingReads.end(); ++it)
00144                         {
00145                                 RequestData* request(*it);
00146                                 if (request->nodeId == nodeId && request->pos == pos)
00147                                 {
00148                                         QDBusMessage &reply(request->reply);
00149                                         Values values(fromAsebaVector(variables->variables));
00150                                         reply << QVariant::fromValue(values);
00151                                         DBusConnectionBus().send(reply);
00152                                         delete request;
00153                                         pendingReads.erase(it);
00154                                         break;
00155                                 }
00156                         }
00157                 }
00158                 
00159                 delete message;
00160         }
00161         
00162         void AsebaNetworkInterface::sendEventOnDBus(const quint16 event, const Values& data)
00163         {
00164                 QList<EventFilterInterface*> filters = eventsFilters.values(event);
00165                 QString name;
00166                 if (event < commonDefinitions.events.size())
00167                         name = QString::fromStdWString(commonDefinitions.events[event].name);
00168                 else
00169                         name = "?";
00170                 for (int i = 0; i < filters.size(); ++i)
00171                         filters.at(i)->emitEvent(event, name, data);
00172         }
00173         
00174         void AsebaNetworkInterface::listenEvent(EventFilterInterface* filter, quint16 event)
00175         {
00176                 eventsFilters.insert(event, filter);
00177         }
00178         
00179         void AsebaNetworkInterface::ignoreEvent(EventFilterInterface* filter, quint16 event)
00180         {
00181                 eventsFilters.remove(event, filter);
00182         }
00183         
00184         void AsebaNetworkInterface::filterDestroyed(EventFilterInterface* filter)
00185         {
00186                 QList<quint16> events = eventsFilters.keys(filter);
00187                 for (int i = 0; i < events.size(); ++i)
00188                         eventsFilters.remove(events.at(i), filter);
00189         }
00190         
00191         void AsebaNetworkInterface::LoadScripts(const QString& fileName, const QDBusMessage &message)
00192         {
00193                 QFile file(fileName);
00194                 if (!file.open(QFile::ReadOnly))
00195                 {
00196                         DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("file %0 does not exists").arg(fileName)));
00197                         return;
00198                 }
00199                 
00200                 QDomDocument document("aesl-source");
00201                 QString errorMsg;
00202                 int errorLine;
00203                 int errorColumn;
00204                 if (!document.setContent(&file, false, &errorMsg, &errorLine, &errorColumn))
00205                 {
00206                         DBusConnectionBus().send(message.createErrorReply(QDBusError::Other, QString("Error in XML source file: %0 at line %1, column %2").arg(errorMsg).arg(errorLine).arg(errorColumn)));
00207                         return;
00208                 }
00209                 
00210                 commonDefinitions.events.clear();
00211                 commonDefinitions.constants.clear();
00212                 userDefinedVariablesMap.clear();
00213                 
00214                 int noNodeCount = 0;
00215                 QDomNode domNode = document.documentElement().firstChild();
00216                 
00217                 // FIXME: this code depends on event and contants being before any code
00218                 bool wasError = false;
00219                 while (!domNode.isNull())
00220                 {
00221                         if (domNode.isElement())
00222                         {
00223                                 QDomElement element = domNode.toElement();
00224                                 if (element.tagName() == "node")
00225                                 {
00226                                         bool ok;
00227                                         const unsigned nodeId(getNodeId(element.attribute("name").toStdWString(), element.attribute("nodeId", 0).toUInt(), &ok));
00228                                         if (ok)
00229                                         {
00230                                                 std::wistringstream is(element.firstChild().toText().data().toStdWString());
00231                                                 Error error;
00232                                                 BytecodeVector bytecode;
00233                                                 unsigned allocatedVariablesCount;
00234                                                 
00235                                                 Compiler compiler;
00236                                                 compiler.setTargetDescription(getDescription(nodeId));
00237                                                 compiler.setCommonDefinitions(&commonDefinitions);
00238                                                 bool result = compiler.compile(is, bytecode, allocatedVariablesCount, error);
00239                                                 
00240                                                 if (result)
00241                                                 {
00242                                                         typedef std::vector<Message*> MessageVector;
00243                                                         MessageVector messages;
00244                                                         sendBytecode(messages, nodeId, std::vector<uint16>(bytecode.begin(), bytecode.end()));
00245                                                         for (MessageVector::const_iterator it = messages.begin(); it != messages.end(); ++it)
00246                                                         {
00247                                                                 hub->sendMessage(*it);
00248                                                                 delete *it;
00249                                                         }
00250                                                         Run msg(nodeId);
00251                                                         hub->sendMessage(msg);
00252                                                 }
00253                                                 else
00254                                                 {
00255                                                         DBusConnectionBus().send(message.createErrorReply(QDBusError::Failed, QString::fromStdWString(error.toWString())));
00256                                                         wasError = true;
00257                                                         break;
00258                                                 }
00259                                                 // retrieve user-defined variables for use in get/set
00260                                                 userDefinedVariablesMap[element.attribute("name")] = *compiler.getVariablesMap();
00261                                         }
00262                                         else
00263                                                 noNodeCount++;
00264                                 }
00265                                 else if (element.tagName() == "event")
00266                                 {
00267                                         const QString eventName(element.attribute("name"));
00268                                         const unsigned eventSize(element.attribute("size").toUInt());
00269                                         if (eventSize > ASEBA_MAX_EVENT_ARG_SIZE)
00270                                         {
00271                                                 DBusConnectionBus().send(message.createErrorReply(QDBusError::Failed, QString("Event %1 has a length %2 larger than maximum %3").arg(eventName).arg(eventSize).arg(ASEBA_MAX_EVENT_ARG_SIZE)));
00272                                                 wasError = true;
00273                                                 break;
00274                                         }
00275                                         else
00276                                         {
00277                                                 commonDefinitions.events.push_back(NamedValue(eventName.toStdWString(), eventSize));
00278                                         }
00279                                 }
00280                                 else if (element.tagName() == "constant")
00281                                 {
00282                                         commonDefinitions.constants.push_back(NamedValue(element.attribute("name").toStdWString(), element.attribute("value").toUInt()));
00283                                 }
00284                         }
00285                         domNode = domNode.nextSibling();
00286                 }
00287                 
00288                 // check if there was an error
00289                 if (wasError)
00290                 {
00291                         std::wcerr << QString("There was an error while loading script %1").arg(fileName).toStdWString() << std::endl;
00292                         commonDefinitions.events.clear();
00293                         commonDefinitions.constants.clear();
00294                         userDefinedVariablesMap.clear();
00295                 }
00296                 
00297                 // check if there was some matching problem
00298                 if (noNodeCount)
00299                 {
00300                         std::wcerr << QString("%0 scripts have no corresponding nodes in the current network and have not been loaded.").arg(noNodeCount).toStdWString() << std::endl;
00301                 }
00302         }
00303         
00304         QStringList AsebaNetworkInterface::GetNodesList() const
00305         {
00306                 QStringList list;
00307                 for (NodesNamesMap::const_iterator it = nodesNames.begin(); it != nodesNames.end(); ++it)
00308                 {
00309                         list.push_back(it.key());
00310                 }
00311                 return list;
00312         }
00313 
00314         qint16 AsebaNetworkInterface::GetNodeId(const QString& node, const QDBusMessage &message) const
00315         {
00316                 NodesNamesMap::const_iterator nodeIt(nodesNames.find(node));
00317                 if (nodeIt == nodesNames.end())
00318                 {
00319                         DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("node %0 does not exists").arg(node)));
00320                         return 0;
00321                 }
00322                 return nodeIt.value();  
00323         }
00324 
00325         QStringList AsebaNetworkInterface::GetVariablesList(const QString& node) const
00326         {
00327                 NodesNamesMap::const_iterator it(nodesNames.find(node));
00328                 if (it != nodesNames.end())
00329                 {
00330                         // node defined variables
00331                         const unsigned nodeId(it.value());
00332                         const NodesDescriptionsMap::const_iterator descIt(nodesDescriptions.find(nodeId));
00333                         const NodeDescription& description(descIt->second);
00334                         QStringList list;
00335                         for (size_t i = 0; i < description.namedVariables.size(); ++i)
00336                         {
00337                                 list.push_back(QString::fromStdWString(description.namedVariables[i].name));
00338                         }
00339                         
00340                         // user defined variables
00341                         const UserDefinedVariablesMap::const_iterator userVarMapIt(userDefinedVariablesMap.find(node));
00342                         if (userVarMapIt != userDefinedVariablesMap.end())
00343                         {
00344                                 const Compiler::VariablesMap& variablesMap(*userVarMapIt);
00345                                 for (Compiler::VariablesMap::const_iterator jt = variablesMap.begin(); jt != variablesMap.end(); ++jt)
00346                                 {
00347                                         list.push_back(QString::fromStdWString(jt->first));
00348                                 }
00349                         }
00350                         
00351                         return list;
00352                 }
00353                 else
00354                 {
00355                         return QStringList();
00356                 }
00357         }
00358         
00359         void AsebaNetworkInterface::SetVariable(const QString& node, const QString& variable, const Values& data, const QDBusMessage &message) const
00360         {
00361                 // make sure the node exists
00362                 NodesNamesMap::const_iterator nodeIt(nodesNames.find(node));
00363                 if (nodeIt == nodesNames.end())
00364                 {
00365                         DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("node %0 does not exists").arg(node)));
00366                         return;
00367                 }
00368                 const unsigned nodeId(nodeIt.value());
00369                 
00370                 unsigned pos(unsigned(-1));
00371                 
00372                 // check whether variable is user-defined
00373                 const UserDefinedVariablesMap::const_iterator userVarMapIt(userDefinedVariablesMap.find(node));
00374                 if (userVarMapIt != userDefinedVariablesMap.end())
00375                 {
00376                         const Compiler::VariablesMap& userVarMap(userVarMapIt.value());
00377                         const Compiler::VariablesMap::const_iterator userVarIt(userVarMap.find(variable.toStdWString()));
00378                         if (userVarIt != userVarMap.end())
00379                         {
00380                                 pos = userVarIt->second.first;
00381                         }
00382                 }
00383                 
00384                 // if variable is not user-defined, check whether it is provided by this node
00385                 if (pos == unsigned(-1))
00386                 {
00387                         bool ok;
00388                         pos = getVariablePos(nodeId, variable.toStdWString(), &ok);
00389                         if (!ok)
00390                         {
00391                                 DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("variable %0 does not exists in node %1").arg(variable).arg(node)));
00392                                 return;
00393                         }
00394                 }
00395                 
00396                 SetVariables msg(nodeId, pos, toAsebaVector(data));
00397                 hub->sendMessage(msg);
00398         }
00399         
00400         Values AsebaNetworkInterface::GetVariable(const QString& node, const QString& variable, const QDBusMessage &message)
00401         {
00402                 // make sure the node exists
00403                 NodesNamesMap::const_iterator nodeIt(nodesNames.find(node));
00404                 if (nodeIt == nodesNames.end())
00405                 {
00406                         DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("node %0 does not exists").arg(node)));
00407                         return Values();
00408                 }
00409                 const unsigned nodeId(nodeIt.value());
00410                 
00411                 unsigned pos(unsigned(-1));
00412                 unsigned length(unsigned(-1));
00413                 
00414                 // check whether variable is user-defined
00415                 const UserDefinedVariablesMap::const_iterator userVarMapIt(userDefinedVariablesMap.find(node));
00416                 if (userVarMapIt != userDefinedVariablesMap.end())
00417                 {
00418                         const Compiler::VariablesMap& userVarMap(userVarMapIt.value());
00419                         const Compiler::VariablesMap::const_iterator userVarIt(userVarMap.find(variable.toStdWString()));
00420                         if (userVarIt != userVarMap.end())
00421                         {
00422                                 pos = userVarIt->second.first;
00423                                 length = userVarIt->second.second;
00424                         }
00425                 }
00426                 
00427                 // if variable is not user-defined, check whether it is provided by this node
00428                 if (pos == unsigned(-1))
00429                 {
00430                         bool ok1, ok2;
00431                         pos = getVariablePos(nodeId, variable.toStdWString(), &ok1);
00432                         length = getVariableSize(nodeId, variable.toStdWString(), &ok2);
00433                         if (!(ok1 && ok2))
00434                         {
00435                                 DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("variable %0 does not exists in node %1").arg(variable).arg(node)));
00436                                 return Values();
00437                         }
00438                 }
00439                 
00440                 // send request to aseba network
00441                 {
00442                         GetVariables msg(nodeId, pos, length);
00443                         hub->sendMessage(msg);
00444                 }
00445                 
00446                 // build bookkeeping for async reply
00447                 RequestData *request = new RequestData;
00448                 request->nodeId = nodeId;
00449                 request->pos = pos;
00450                 message.setDelayedReply(true);
00451                 request->reply = message.createReply();
00452                 
00453                 pendingReads.push_back(request);
00454                 return Values();
00455         }
00456         
00457         void AsebaNetworkInterface::SendEvent(const quint16 event, const Values& data)
00458         {
00459                 // send event to DBus listeners
00460                 sendEventOnDBus(event, data);
00461                 
00462                 // send on TCP
00463                 UserMessage msg(event, toAsebaVector(data));
00464                 hub->sendMessage(msg);
00465         }
00466         
00467         void AsebaNetworkInterface::SendEventName(const QString& name, const Values& data, const QDBusMessage &message)
00468         {
00469                 size_t event;
00470                 if (commonDefinitions.events.contains(name.toStdWString(), &event))
00471                         SendEvent(event, data);
00472                 else
00473                         DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("no event named %0").arg(name)));
00474         }
00475         
00476         QDBusObjectPath AsebaNetworkInterface::CreateEventFilter()
00477         {
00478                 QDBusObjectPath path(QString("/events_filters/%0").arg(eventsFiltersCounter++));
00479                 DBusConnectionBus().registerObject(path.path(), new EventFilterInterface(this), QDBusConnection::ExportScriptableContents);
00480                 return path;
00481         }
00482         
00483         void AsebaNetworkInterface::nodeDescriptionReceived(unsigned nodeId)
00484         {
00485                 nodesNames[QString::fromStdWString(nodesDescriptions[nodeId].name)] = nodeId;
00486         }
00487 
00488         inline QDBusConnection AsebaNetworkInterface::DBusConnectionBus() const
00489         {
00490                 if (systemBus)
00491                         return QDBusConnection::systemBus();
00492                 else
00493                         return QDBusConnection::sessionBus();
00494         }
00495         
00496         // the following methods run in the main thread (event loop)
00497         
00498         Hub::Hub(unsigned port, bool verbose, bool dump, bool forward, bool rawTime, bool systemBus) :
00499                 #ifdef DASHEL_VERSION_INT
00500                 Dashel::Hub(verbose || dump),
00501                 #endif // DASHEL_VERSION_INT
00502                 verbose(verbose),
00503                 dump(dump),
00504                 forward(forward),
00505                 rawTime(rawTime)
00506         {
00507                 // TODO: work in progress to remove ugly delay
00508                 AsebaNetworkInterface* network(new AsebaNetworkInterface(this, systemBus));
00509                 QObject::connect(this, SIGNAL(messageAvailable(Message*, Dashel::Stream*)), network, SLOT(processMessage(Message*, Dashel::Stream*)));
00510                 QObject::connect(this, SIGNAL(firstConnectionCreated()), SLOT(firstConnectionAvailable()));
00511                 ostringstream oss;
00512                 oss << "tcpin:port=" << port;
00513                 Dashel::Hub::connect(oss.str());
00514         }
00515         
00516         void Hub::sendMessage(Message *message, Stream* sourceStream)
00517         {
00518                 // dump if requested
00519                 if (dump)
00520                 {
00521                         dumpTime(cout, rawTime);
00522                         message->dump(wcout);
00523                         cout << std::endl;
00524                 }
00525                 
00526                 // Called from the dbus thread, not the Hub thread, need to lock        
00527                 lock();
00528 
00529                 // write on all connected streams
00530                 for (StreamsSet::iterator it = dataStreams.begin(); it != dataStreams.end();++it)
00531                 {
00532                         Stream* destStream(*it);
00533                         
00534                         if ((forward) && (destStream == sourceStream))
00535                                 continue;
00536                         
00537                         try
00538                         {
00539                                 message->serialize(destStream);
00540                                 destStream->flush();
00541                         }
00542                         catch (DashelException e)
00543                         {
00544                                 // if this stream has a problem, ignore it for now, and let Hub call connectionClosed later.
00545                                 std::cerr << "error while writing message" << std::endl;
00546                         }
00547                 }
00548 
00549                 unlock();
00550         }
00551         
00552         void Hub::sendMessage(Message& message, Dashel::Stream* sourceStream)
00553         {
00554                 sendMessage(&message, sourceStream);
00555         }
00556         
00557         void Hub::firstConnectionAvailable()
00558         {
00559                 QTimer::singleShot(200, this, SLOT(requestDescription()));
00560         }
00561         
00562         void Hub::requestDescription()
00563         {
00564                 emit messageAvailable(new GetDescription(), 0);
00565         }
00566         
00567         // the following methods run in the blocking reception thread
00568         
00569         // In QThread main function, we just make our Dashel hub switch listen for incoming data
00570         void Hub::run()
00571         {
00572                 Dashel::Hub::run();
00573         }
00574         
00575         // the following method run in the blocking reception thread
00576         
00577         void Hub::incomingData(Stream *stream)
00578         {
00579                 // receive message
00580                 Message *message(0);
00581                 try
00582                 {
00583                         message = Message::receive(stream);
00584                 }
00585                 catch (DashelException e)
00586                 {
00587                         // if this stream has a problem, ignore it for now, and let Hub call connectionClosed later.
00588                         std::cerr << "error while reading message" << std::endl;
00589                 }
00590                 
00591                 // send on DBus, the receiver can delete it
00592                 emit messageAvailable(message, stream);
00593         }
00594         
00595         void Hub::connectionCreated(Stream *stream)
00596         {
00597                 if (verbose)
00598                 {
00599                         dumpTime(cout, rawTime);
00600                         cout << "Incoming connection from " << stream->getTargetName() << endl;
00601                 }
00602                 
00603                 if (dataStreams.size() == 1)
00604                 {
00605                         emit firstConnectionCreated();
00606                 }
00607         }
00608         
00609         void Hub::connectionClosed(Stream* stream, bool abnormal)
00610         {
00611                 if (verbose)
00612                 {
00613                         dumpTime(cout);
00614                         if (abnormal)
00615                                 cout << "Abnormal connection closed to " << stream->getTargetName() << " : " << stream->getFailReason() << endl;
00616                         else
00617                                 cout << "Normal connection closed to " << stream->getTargetName() << endl;
00618                 }
00619         }
00620         
00622 };
00623 
00625 void dumpHelp(std::ostream &stream, const char *programName)
00626 {
00627         stream << "Aseba medulla, connects aseba components together and with D-Bus, usage:\n";
00628         stream << programName << " [options] [additional targets]*\n";
00629         stream << "Options:\n";
00630         stream << "-v, --verbose   : makes the switch verbose\n";
00631         stream << "-d, --dump      : makes the switch dump the content of messages\n";
00632         stream << "-l, --loop      : makes the switch transmit messages back to the send, not only forward them.\n";
00633         stream << "-p port         : listens to incoming connection on this port\n";
00634         stream << "--rawtime       : shows time in the form of sec:usec since 1970\n";
00635         stream << "--system        : connects medulla to the system d-bus bus\n";       
00636         stream << "-h, --help      : shows this help\n";
00637         stream << "-V, --version   : shows the version number\n";
00638         stream << "Additional targets are any valid Dashel targets." << std::endl;
00639         stream << "Report bugs to: aseba-dev@gna.org" << std::endl;
00640 }
00641 
00643 void dumpVersion(std::ostream &stream)
00644 {
00645         stream << "Aseba medulla " << ASEBA_VERSION << std::endl;
00646         stream << "Aseba protocol " << ASEBA_PROTOCOL_VERSION << std::endl;
00647         stream << "Licence LGPLv3: GNU LGPL version 3 <http://www.gnu.org/licenses/lgpl.html>\n";
00648 }
00649 
00650 int main(int argc, char *argv[])
00651 {
00652         QCoreApplication app(argc, argv);
00653         Dashel::initPlugins();
00654         
00655         unsigned port = ASEBA_DEFAULT_PORT;
00656         bool verbose = false;
00657         bool dump = false;
00658         bool forward = true;
00659         bool rawTime = false;
00660         bool systemBus = false;
00661         std::vector<std::string> additionalTargets;
00662         
00663         int argCounter = 1;
00664         
00665         while (argCounter < argc)
00666         {
00667                 const char *arg = argv[argCounter];
00668                 
00669                 if ((strcmp(arg, "-v") == 0) || (strcmp(arg, "--verbose") == 0))
00670                 {
00671                         verbose = true;
00672                 }
00673                 else if ((strcmp(arg, "-d") == 0) || (strcmp(arg, "--dump") == 0))
00674                 {
00675                         dump = true;
00676                 }
00677                 else if ((strcmp(arg, "-l") == 0) || (strcmp(arg, "--loop") == 0))
00678                 {
00679                         forward = false;
00680                 }
00681                 else if (strcmp(arg, "-p") == 0)
00682                 {
00683                         arg = argv[++argCounter];
00684                         port = atoi(arg);
00685                 }
00686                 else if (strcmp(arg, "--rawtime") == 0)
00687                 {
00688                         rawTime = true;
00689                 }
00690                 else if (strcmp(arg, "--system") == 0)
00691                 {
00692                         systemBus = true;
00693                 }
00694                 else if ((strcmp(arg, "-h") == 0) || (strcmp(arg, "--help") == 0))
00695                 {
00696                         dumpHelp(std::cout, argv[0]);
00697                         return 0;
00698                 }
00699                 else if ((strcmp(arg, "-V") == 0) || (strcmp(arg, "--version") == 0))
00700                 {
00701                         dumpVersion(std::cout);
00702                         return 0;
00703                 }
00704                 else
00705                 {
00706                         additionalTargets.push_back(argv[argCounter]);
00707                 }
00708                 argCounter++;
00709         }
00710         
00711         Aseba::Hub hub(port, verbose, dump, forward, rawTime, systemBus);
00712         
00713         try
00714         {
00715                 for (size_t i = 0; i < additionalTargets.size(); i++)
00716                         ((Dashel::Hub&)hub).connect(additionalTargets[i]);
00717         }
00718         catch(Dashel::DashelException e)
00719         {
00720                 std::cerr << e.what() << std::endl;
00721         }
00722         
00723         hub.start();
00724         
00725         return app.exec();
00726 }
00727 
00728 


aseba
Author(s): Stéphane Magnenat
autogenerated on Sun Oct 5 2014 23:46:38