switch.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 <cstdlib>
00022 #include <cstring>
00023 #include <iostream>
00024 #include <sstream>
00025 #include <set>
00026 #include <valarray>
00027 #include <vector>
00028 #include <iterator>
00029 #include "switch.h"
00030 #include "../transport/dashel_plugins/dashel-plugins.h"
00031 #include "../common/consts.h"
00032 #include "../utils/utils.h"
00033 #include "../msg/msg.h"
00034 #include "../msg/endian.h"
00035 
00036 namespace Aseba 
00037 {
00038         using namespace std;
00039         using namespace Dashel;
00040         
00043 
00045         Switch::Switch(unsigned port, bool verbose, bool dump, bool forward, bool rawTime) :
00046                 #ifdef DASHEL_VERSION_INT
00047                 Dashel::Hub(verbose || dump),
00048                 #endif // DASHEL_VERSION_INT
00049                 verbose(verbose),
00050                 dump(dump),
00051                 forward(forward),
00052                 rawTime(rawTime)
00053         {
00054                 ostringstream oss;
00055                 oss << "tcpin:port=" << port;
00056                 connect(oss.str());
00057         }
00058         
00059         void Switch::connectionCreated(Stream *stream)
00060         {
00061                 if (verbose)
00062                 {
00063                         dumpTime(cout, rawTime);
00064                         cout << "Incoming connection from " << stream->getTargetName() << endl;
00065                 }
00066         }
00067         
00068         void Switch::incomingData(Stream *stream)
00069         {
00070                 Message* message(Message::receive(stream));
00071                 
00072                 // remap source
00073                 {
00074                         const IdRemapTable::const_iterator remapIt(idRemapTable.find(stream));
00075                         if (remapIt != idRemapTable.end() &&
00076                                 (message->source == remapIt->second.second)
00077                         )
00078                                 message->source = remapIt->second.first;
00079                 }
00080                 
00081                 // if requested, dump
00082                 if (dump)
00083                 {
00084                         message->dump(std::wcout);
00085                         std::wcout << std::endl;
00086                 }
00087                 
00088                 // write on all connected streams
00089                 CmdMessage* cmdMessage(dynamic_cast<CmdMessage*>(message));
00090                 for (StreamsSet::iterator it = dataStreams.begin(); it != dataStreams.end();++it)
00091                 {
00092                         Stream* destStream = *it;
00093                         
00094                         if ((forward) && (destStream == stream))
00095                                 continue;
00096                         
00097                         try
00098                         {
00099                                 const IdRemapTable::const_iterator remapIt(idRemapTable.find(destStream));
00100                                 if (cmdMessage && 
00101                                         remapIt != idRemapTable.end())
00102                                 {
00103                                         if (cmdMessage->dest == remapIt->second.first)
00104                                         {
00105                                                 const uint16 oldDest(cmdMessage->dest);
00106                                                 cmdMessage->dest = remapIt->second.second;
00107                                                 message->serialize(destStream);
00108                                                 cmdMessage->dest = oldDest;
00109                                         }
00110                                 }
00111                                 else
00112                                 {
00113                                         message->serialize(destStream);
00114                                 }
00115                                 destStream->flush();
00116                         }
00117                         catch (DashelException e)
00118                         {
00119                                 // if this stream has a problem, ignore it for now, and let Hub call connectionClosed later.
00120                                 std::cerr << "error while writing" << std::endl;
00121                         }
00122                 }
00123                 
00124                 delete message;
00125         }
00126         
00127         void Switch::connectionClosed(Stream *stream, bool abnormal)
00128         {
00129                 if (verbose)
00130                 {
00131                         dumpTime(cout);
00132                         if (abnormal)
00133                                 cout << "Abnormal connection closed to " << stream->getTargetName() << " : " << stream->getFailReason() << endl;
00134                         else
00135                                 cout << "Normal connection closed to " << stream->getTargetName() << endl;
00136                 }
00137         }
00138         
00139         void Switch::broadcastDummyUserMessage()
00140         {
00141                 Aseba::UserMessage uMsg;
00142                 uMsg.type = 0;
00143                 //for (int i = 0; i < 80; i++)
00144                 for (StreamsSet::iterator it = dataStreams.begin(); it != dataStreams.end();++it)
00145                 {
00146                         uMsg.serialize(*it);
00147                         (*it)->flush();
00148                 }
00149         }
00150         
00151         void Switch::remapId(Dashel::Stream* stream, const uint16 localId, const uint16 targetId)
00152         {
00153                 idRemapTable[stream] = IdPair(localId, targetId);
00154         }
00155         
00157 };
00158 
00160 void dumpHelp(std::ostream &stream, const char *programName)
00161 {
00162         stream << "Aseba switch, connects aseba components together, usage:\n";
00163         stream << programName << " [options] [additional targets]*\n";
00164         stream << "Options:\n";
00165         stream << "-v, --verbose   : makes the switch verbose\n";
00166         stream << "-d, --dump      : makes the switch dump all data\n";
00167         stream << "-l, --loop      : makes the switch transmit messages back to the send, not only forward them.\n";
00168         stream << "-p port         : listens to incoming connection on this port\n";
00169         stream << "--rawtime       : shows time in the form of sec:usec since 1970\n";
00170         stream << "-h, --help      : shows this help\n";
00171         stream << "-V, --version   : shows the version number\n";
00172         stream << "Additional targets are any valid Dashel targets." << std::endl;
00173         stream << "Report bugs to: aseba-dev@gna.org" << std::endl;
00174 }
00175 
00177 void dumpVersion(std::ostream &stream)
00178 {
00179         stream << "Aseba switch " << ASEBA_VERSION << std::endl;
00180         stream << "Aseba protocol " << ASEBA_PROTOCOL_VERSION << std::endl;
00181         stream << "Licence LGPLv3: GNU LGPL version 3 <http://www.gnu.org/licenses/lgpl.html>\n";
00182 }
00183  
00184 int main(int argc, char *argv[])
00185 {
00186         Dashel::initPlugins();
00187         unsigned port = ASEBA_DEFAULT_PORT;
00188         bool verbose = false;
00189         bool dump = false;
00190         bool forward = true;
00191         bool rawTime = false;
00192         std::vector<std::string> additionalTargets;
00193         
00194         int argCounter = 1;
00195         
00196         while (argCounter < argc)
00197         {
00198                 const char *arg = argv[argCounter];
00199                 
00200                 if ((strcmp(arg, "-v") == 0) || (strcmp(arg, "--verbose") == 0))
00201                 {
00202                         verbose = true;
00203                 }
00204                 else if ((strcmp(arg, "-d") == 0) || (strcmp(arg, "--dump") == 0))
00205                 {
00206                         dump = true;
00207                 }
00208                 else if ((strcmp(arg, "-l") == 0) || (strcmp(arg, "--loop") == 0))
00209                 {
00210                         forward = false;
00211                 }
00212                 else if (strcmp(arg, "-p") == 0)
00213                 {
00214                         if (argCounter + 1 >= argc)
00215                         {
00216                                 std::cerr << "port value needed" << std::endl;
00217                                 return 1;
00218                         }
00219                         arg = argv[++argCounter];
00220                         port = atoi(arg);
00221                 }
00222                 else if (strcmp(arg, "--rawtime") == 0)
00223                 {
00224                         rawTime = true;
00225                 }
00226                 else if ((strcmp(arg, "-h") == 0) || (strcmp(arg, "--help") == 0))
00227                 {
00228                         dumpHelp(std::cout, argv[0]);
00229                         return 0;
00230                 }
00231                 else if ((strcmp(arg, "-V") == 0) || (strcmp(arg, "--version") == 0))
00232                 {
00233                         dumpVersion(std::cout);
00234                         return 0;
00235                 }
00236                 else
00237                 {
00238                         additionalTargets.push_back(argv[argCounter]);
00239                 }
00240                 argCounter++;
00241         }
00242         
00243         try
00244         {
00245                 Aseba::Switch aswitch(port, verbose, dump, forward, rawTime);
00246                 for (size_t i = 0; i < additionalTargets.size(); i++)
00247                 {
00248                         const std::string& target(additionalTargets[i]);
00249                         Dashel::Stream* stream = aswitch.connect(target);
00250                         
00251                         // see whether we have to remap the id of this stream
00252                         Dashel::ParameterSet remapIdDecoder;
00253                         remapIdDecoder.add("dummy:remapLocal=-1;remapTarget=1");
00254                         remapIdDecoder.add(target.c_str());
00255                         const int remappedLocalId(remapIdDecoder.get<int>("remapLocal"));
00256                         const int remappedTargetId(remapIdDecoder.get<int>("remapTarget"));
00257                         if (target.find("remapLocal=") != std::string::npos)
00258                         {
00259                                 aswitch.remapId(stream, uint16(remappedLocalId), uint16(remappedTargetId));
00260                                 if (verbose)
00261                                         std::cout << "Remapping local " << remappedLocalId << " with remote " << remappedTargetId << std::endl;
00262                         }
00263                 }
00264                 /*
00265                 Uncomment this and comment aswitch.run() to flood all pears with dummy user messages
00266                 while (1)
00267                 {
00268                         aswitch.step(10);
00269                         aswitch.broadcastDummyUserMessage();
00270                 }*/
00271                 aswitch.run();
00272         }
00273         catch(Dashel::DashelException e)
00274         {
00275                 std::cerr << e.what() << std::endl;
00276         }
00277         
00278         return 0;
00279 }
00280 
00281 


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