dummynode.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 #ifndef ASEBA_ASSERT
00022 #define ASEBA_ASSERT
00023 #endif
00024 
00025 #include "../../vm/vm.h"
00026 #include "../../vm/natives.h"
00027 #include "../../common/productids.h"
00028 #include "../../common/consts.h"
00029 #include "../../transport/buffer/vm-buffer.h"
00030 #include <dashel/dashel.h>
00031 #include <iostream>
00032 #include <sstream>
00033 #include <valarray>
00034 #include <cassert>
00035 #include <cstring>
00036 
00037 extern AsebaVMDescription nodeDescription;
00038 
00039 class AsebaNode: public Dashel::Hub
00040 {
00041 private:
00042         AsebaVMState vm;
00043         std::valarray<unsigned short> bytecode;
00044         std::valarray<signed short> stack;
00045         struct Variables
00046         {
00047                 sint16 id;
00048                 sint16 source;
00049                 sint16 args[32];
00050                 sint16 productId;
00051                 sint16 user[1024];
00052         } variables;
00053         char mutableName[12];
00054         
00055 public:
00056         // public because accessed from a glue function
00057         uint16 lastMessageSource;
00058         std::valarray<uint8> lastMessageData;
00059         // this must be public because of bindings to C functions
00060         Dashel::Stream* stream;
00061         
00062 public:
00063         
00064         AsebaNode()
00065         {
00066                 // setup variables
00067                 vm.nodeId = 1;
00068                 
00069                 bytecode.resize(512);
00070                 vm.bytecode = &bytecode[0];
00071                 vm.bytecodeSize = bytecode.size();
00072                 
00073                 stack.resize(64);
00074                 vm.stack = &stack[0];
00075                 vm.stackSize = stack.size();
00076                 
00077                 vm.variables = reinterpret_cast<sint16 *>(&variables);
00078                 vm.variablesSize = sizeof(variables) / sizeof(sint16);
00079         }
00080         
00081         void listen(int basePort, int deltaPort)
00082         {
00083                 const int port(basePort + deltaPort);
00084                 vm.nodeId = 1 + deltaPort;
00085                 strncpy(mutableName, "dummynode-0", 12);
00086                 mutableName[10] = '0' + deltaPort;
00087                 nodeDescription.name = mutableName;
00088                 
00089                 // connect network
00090                 try
00091                 {
00092                         std::ostringstream oss;
00093                         oss << "tcpin:port=" << port;
00094                         Dashel::Hub::connect(oss.str());
00095                 }
00096                 catch (Dashel::DashelException e)
00097                 {
00098                         std::cerr << "Cannot create listening port " << port << ": " << e.what() << std::endl;
00099                         abort();
00100                 }
00101                 
00102                 // init VM
00103                 AsebaVMInit(&vm);
00104         }
00105         
00106         virtual void connectionCreated(Dashel::Stream *stream)
00107         {
00108                 std::string targetName = stream->getTargetName();
00109                 if (targetName.substr(0, targetName.find_first_of(':')) == "tcp")
00110                 {
00111                         std::cerr << this << " : New client connected." << std::endl;
00112                         if (this->stream)
00113                         {
00114                                 closeStream(this->stream);
00115                                 std::cerr << this << " : Disconnected old client." << std::endl;
00116                         }
00117                         this->stream = stream;
00118                 }
00119         }
00120         
00121         virtual void connectionClosed(Dashel::Stream *stream, bool abnormal)
00122         {
00123                 this->stream = 0;
00124                 // clear breakpoints
00125                 vm.breakpointsCount = 0;
00126                 
00127                 if (abnormal)
00128                         std::cerr << this << " : Client has disconnected unexpectedly." << std::endl;
00129                 else
00130                         std::cerr << this << " : Client has disconnected properly." << std::endl;
00131         }
00132         
00133         virtual void incomingData(Dashel::Stream *stream)
00134         {
00135                 uint16 temp;
00136                 uint16 len;
00137                 
00138                 stream->read(&temp, 2);
00139                 len = bswap16(temp);
00140                 stream->read(&temp, 2);
00141                 lastMessageSource = bswap16(temp);
00142                 lastMessageData.resize(len+2);
00143                 stream->read(&lastMessageData[0], lastMessageData.size());
00144                 
00145                 AsebaProcessIncomingEvents(&vm);
00146         }
00147         
00148         virtual void applicationStep()
00149         {
00150                 // run VM
00151                 AsebaVMRun(&vm, 65535);
00152                 
00153                 // reschedule a periodic event if we are not in step by step
00154                 if (AsebaMaskIsClear(vm.flags, ASEBA_VM_STEP_BY_STEP_MASK) || AsebaMaskIsClear(vm.flags, ASEBA_VM_EVENT_ACTIVE_MASK))
00155                         AsebaVMSetupEvent(&vm, ASEBA_EVENT_LOCAL_EVENTS_START-0);
00156         }
00157 } node;
00158 
00159 // Implementation of aseba glue code
00160 
00161 extern "C" void AsebaPutVmToSleep(AsebaVMState *vm) 
00162 {
00163         std::cerr << "Received request to go into sleep" << std::endl;
00164 }
00165 
00166 extern "C" void AsebaSendBuffer(AsebaVMState *vm, const uint8* data, uint16 length)
00167 {
00168         Dashel::Stream* stream = node.stream;
00169         if (stream)
00170         {
00171                 try
00172                 {
00173                         uint16 temp;
00174                         temp = bswap16(length - 2);
00175                         stream->write(&temp, 2);
00176                         temp = bswap16(vm->nodeId);
00177                         stream->write(&temp, 2);
00178                         stream->write(data, length);
00179                         stream->flush();
00180                 }
00181                 catch (Dashel::DashelException e)
00182                 {
00183                         std::cerr << "Cannot write to socket: " << stream->getFailReason() << std::endl;
00184                 }
00185         }
00186 }
00187 
00188 extern "C" uint16 AsebaGetBuffer(AsebaVMState *vm, uint8* data, uint16 maxLength, uint16* source)
00189 {
00190         if (node.lastMessageData.size())
00191         {
00192                 *source = node.lastMessageSource;
00193                 memcpy(data, &node.lastMessageData[0], node.lastMessageData.size());
00194         }
00195         return node.lastMessageData.size();
00196 }
00197 
00198 extern AsebaVMDescription nodeDescription;
00199 
00200 extern "C" const AsebaVMDescription* AsebaGetVMDescription(AsebaVMState *vm)
00201 {
00202         return &nodeDescription;
00203 }
00204 
00205 static AsebaNativeFunctionPointer nativeFunctions[] =
00206 {
00207         ASEBA_NATIVES_STD_FUNCTIONS,
00208 };
00209 
00210 static const AsebaNativeFunctionDescription* nativeFunctionsDescriptions[] =
00211 {
00212         ASEBA_NATIVES_STD_DESCRIPTIONS,
00213         0
00214 };
00215 
00216 extern "C" const AsebaNativeFunctionDescription * const * AsebaGetNativeFunctionsDescriptions(AsebaVMState *vm)
00217 {
00218         return nativeFunctionsDescriptions;
00219 }
00220 
00221 extern "C" void AsebaNativeFunction(AsebaVMState *vm, uint16 id)
00222 {
00223         nativeFunctions[id](vm);
00224 }
00225 
00226 
00227 static const AsebaLocalEventDescription localEvents[] = {
00228         { "timer", "periodic timer at 50 Hz" },
00229         { NULL, NULL }
00230 };
00231 
00232 extern "C" const AsebaLocalEventDescription * AsebaGetLocalEventsDescriptions(AsebaVMState *vm)
00233 {
00234         return localEvents;
00235 }
00236 
00237 extern "C" void AsebaWriteBytecode(AsebaVMState *vm)
00238 {
00239         std::cerr << "Received request to write bytecode into flash" << std::endl;
00240 }
00241 
00242 extern "C" void AsebaResetIntoBootloader(AsebaVMState *vm)
00243 {
00244         std::cerr << "Received request to reset into bootloader" << std::endl;
00245 }
00246 
00247 extern "C" void AsebaAssert(AsebaVMState *vm, AsebaAssertReason reason)
00248 {
00249         std::cerr << "\nFatal error; exception: ";
00250         switch (reason)
00251         {
00252                 case ASEBA_ASSERT_UNKNOWN: std::cerr << "undefined"; break;
00253                 case ASEBA_ASSERT_UNKNOWN_BINARY_OPERATOR: std::cerr << "unknown binary operator"; break;
00254                 case ASEBA_ASSERT_UNKNOWN_BYTECODE: std::cerr << "unknown bytecode"; break;
00255                 case ASEBA_ASSERT_STACK_OVERFLOW: std::cerr << "stack overflow"; break;
00256                 case ASEBA_ASSERT_STACK_UNDERFLOW: std::cerr << "stack underflow"; break;
00257                 case ASEBA_ASSERT_OUT_OF_VARIABLES_BOUNDS: std::cerr << "out of variables bounds"; break;
00258                 case ASEBA_ASSERT_OUT_OF_BYTECODE_BOUNDS: std::cerr << "out of bytecode bounds"; break;
00259                 case ASEBA_ASSERT_STEP_OUT_OF_RUN: std::cerr << "step out of run"; break;
00260                 case ASEBA_ASSERT_BREAKPOINT_OUT_OF_BYTECODE_BOUNDS: std::cerr << "breakpoint out of bytecode bounds"; break;
00261                 default: std::cerr << "unknown exception"; break;
00262         }
00263         std::cerr << ".\npc = " << vm->pc << ", sp = " << vm->sp;
00264         abort();
00265         std::cerr << "\nResetting VM" << std::endl;
00266         AsebaVMInit(vm);
00267 }
00268 
00269 
00270 int main(int argc, char* argv[])
00271 {
00272         const int basePort = ASEBA_DEFAULT_PORT;
00273         int deltaPort = 0;
00274         if (argc > 1)
00275         {
00276                 deltaPort = atoi(argv[1]);
00277                 if (deltaPort < 0 || deltaPort >= 9)
00278                 {
00279                         std::cerr << "Usage: " << argv[0] << " [delta port, from 0 to 9]" << std::endl;
00280                         return 1;
00281                 }
00282         }
00283         node.listen(basePort, deltaPort);
00284         while (node.step(10))
00285         {
00286                 node.applicationStep();
00287         }
00288 }


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