$search
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