playground.cpp
Go to the documentation of this file.
00001 /*
00002         Playground - An active arena to learn multi-robots programming
00003         Copyright (C) 1999--2008:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006         3D models
00007         Copyright (C) 2008:
00008                 Basilio Noris
00009         Aseba - an event-based framework for distributed robot control
00010         Copyright (C) 2007--2012:
00011                 Stephane Magnenat <stephane at magnenat dot net>
00012                 (http://stephane.magnenat.net)
00013                 and other contributors, see authors.txt for details
00014         
00015         This program is free software: you can redistribute it and/or modify
00016         it under the terms of the GNU Lesser General Public License as published
00017         by the Free Software Foundation, version 3 of the License.
00018         
00019         This program is distributed in the hope that it will be useful,
00020         but WITHOUT ANY WARRANTY; without even the implied warranty of
00021         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022         GNU Lesser General Public License for more details.
00023         
00024         You should have received a copy of the GNU Lesser General Public License
00025         along with this program. If not, see <http://www.gnu.org/licenses/>.
00026 */
00027 
00028 #ifndef ASEBA_ASSERT
00029 #define ASEBA_ASSERT
00030 #endif
00031 
00032 #include "../../vm/vm.h"
00033 #include "../../vm/natives.h"
00034 #include "../../common/consts.h"
00035 #include "../../transport/buffer/vm-buffer.h"
00036 #include "../../common/productids.h"
00037 #include <enki/PhysicalEngine.h>
00038 #include <enki/robots/e-puck/EPuck.h>
00039 #include <iostream>
00040 #include <QtGui>
00041 #include <QtDebug>
00042 #include <QtXml>
00043 #include "playground.h"
00044 //#include <playground.moc>
00045 #include <string.h>
00046 
00047 #ifdef USE_SDL
00048 #include <SDL/SDL.h>
00049 #endif
00050 
00052 template<typename Derived, typename Base>
00053 inline Derived polymorphic_downcast(Base base)
00054 {
00055         Derived derived = dynamic_cast<Derived>(base);
00056         assert(derived);
00057         return derived;
00058 }
00059 
00060 // Definition of the aseba glue
00061 
00062 namespace Enki
00063 {
00064         class AsebaFeedableEPuck;
00065 }
00066 
00067 extern "C" void PlaygroundNative_energysend(AsebaVMState *vm);
00068 extern "C" void PlaygroundNative_energyreceive(AsebaVMState *vm);
00069 extern "C" void PlaygroundNative_energyamount(AsebaVMState *vm);
00070 
00071 extern "C" AsebaNativeFunctionDescription PlaygroundNativeDescription_energysend;
00072 extern "C" AsebaNativeFunctionDescription PlaygroundNativeDescription_energyreceive;
00073 extern "C" AsebaNativeFunctionDescription PlaygroundNativeDescription_energyamount;
00074 
00075 
00076 static AsebaNativeFunctionPointer nativeFunctions[] =
00077 {
00078         ASEBA_NATIVES_STD_FUNCTIONS,
00079         PlaygroundNative_energysend,
00080         PlaygroundNative_energyreceive,
00081         PlaygroundNative_energyamount
00082 };
00083 
00084 static const AsebaNativeFunctionDescription* nativeFunctionsDescriptions[] =
00085 {
00086         ASEBA_NATIVES_STD_DESCRIPTIONS,
00087         &PlaygroundNativeDescription_energysend,
00088         &PlaygroundNativeDescription_energyreceive,
00089         &PlaygroundNativeDescription_energyamount,
00090         0
00091 };
00092 
00093 extern "C" AsebaVMDescription vmDescription;
00094 
00095 // this uglyness is necessary to glue with C code
00096 Enki::PlaygroundViewer *playgroundViewer = 0;
00097 
00098 namespace Enki
00099 {
00100         #define LEGO_RED                                                Enki::Color(0.77, 0.2, 0.15)
00101         #define LEGO_GREEN                                              Enki::Color(0, 0.5, 0.17)
00102         #define LEGO_BLUE                                               Enki::Color(0, 0.38 ,0.61)
00103         #define LEGO_WHITE                                              Enki::Color(0.9, 0.9, 0.9)
00104         
00105         #define EPUCK_FEEDER_INITIAL_ENERGY             10
00106         #define EPUCK_FEEDER_THRESHOLD_HIDE             2
00107         #define EPUCK_FEEDER_THRESHOLD_SHOW             4
00108         #define EPUCK_FEEDER_RADIUS                             5
00109         #define EPUCK_FEEDER_RANGE                              10
00110         
00111         #define EPUCK_FEEDER_COLOR_ACTIVE               LEGO_GREEN
00112         #define EPUCK_FEEDER_COLOR_INACTIVE             LEGO_WHITE
00113         
00114         #define EPUCK_FEEDER_D_ENERGY                   4
00115         #define EPUCK_FEEDER_RECHARGE_RATE              0.5
00116         #define EPUCK_FEEDER_MAX_ENERGY                 100
00117         
00118         #define EPUCK_FEEDER_HEIGHT                             5
00119         
00120         #define EPUCK_INITIAL_ENERGY                    10
00121         #define EPUCK_ENERGY_CONSUMPTION_RATE   1
00122         
00123         #define SCORE_MODIFIER_COEFFICIENT              0.2
00124         
00125         #define INITIAL_POOL_ENERGY                             10
00126         
00127         #define ACTIVATION_OBJECT_COLOR                 LEGO_RED
00128         #define ACTIVATION_OBJECT_HEIGHT                6
00129         
00130         #define DEATH_ANIMATION_STEPS                   30
00131         
00132         extern GLint GenFeederBase();
00133         extern GLint GenFeederCharge0();
00134         extern GLint GenFeederCharge1();
00135         extern GLint GenFeederCharge2();
00136         extern GLint GenFeederCharge3();
00137         extern GLint GenFeederRing();
00138         
00139         class ScoreModifier: public GlobalInteraction
00140         {
00141         public:
00142                 ScoreModifier(Robot* owner) : GlobalInteraction(owner) {}
00143                 
00144                 virtual void step(double dt, World *w);
00145         };
00146         
00147         class FeedableEPuck: public EPuck
00148         {
00149         public:
00150                 double energy;
00151                 double score;
00152                 QString name;
00153                 int diedAnimation;
00154                 ScoreModifier scoreModifier;
00155         
00156         public:
00157                 FeedableEPuck() :
00158                         EPuck(CAPABILITY_BASIC_SENSORS | CAPABILITY_CAMERA), 
00159                         energy(EPUCK_INITIAL_ENERGY),
00160                         score(0),
00161                         diedAnimation(-1),
00162                         scoreModifier(this)
00163                 {
00164                         addGlobalInteraction(&scoreModifier);
00165                 }
00166                 
00167                 void controlStep(double dt)
00168                 {
00169                         EPuck::controlStep(dt);
00170                         
00171                         energy -= dt * EPUCK_ENERGY_CONSUMPTION_RATE;
00172                         score += dt;
00173                         if (energy < 0)
00174                         {
00175                                 score /= 2;
00176                                 energy = EPUCK_INITIAL_ENERGY;
00177                                 diedAnimation = DEATH_ANIMATION_STEPS;
00178                         }
00179                         else if (diedAnimation >= 0)
00180                                 diedAnimation--;
00181                 }
00182         };
00183         
00184         void ScoreModifier::step(double dt, World *w)
00185         {
00186                 double x = owner->pos.x;
00187                 double y = owner->pos.y;
00188                 if ((x > 32) && (x < 110.4-32) && (y > 67.2) && (y < 110.4-32))
00189                         polymorphic_downcast<FeedableEPuck*>(owner)->score += dt * SCORE_MODIFIER_COEFFICIENT;
00190         }
00191         
00192         class AsebaFeedableEPuck : public FeedableEPuck
00193         {
00194         public:
00195                 int joystick;
00196                 AsebaVMState vm;
00197                 std::valarray<unsigned short> bytecode;
00198                 std::valarray<signed short> stack;
00199                 struct Variables
00200                 {
00201                         sint16 id;
00202                         sint16 source;
00203                         sint16 args[32];
00204                         sint16 productId; 
00205                         sint16 speedL; // left motor speed
00206                         sint16 speedR; // right motor speed
00207                         sint16 colorR; // body red [0..100] %
00208                         sint16 colorG; // body green [0..100] %
00209                         sint16 colorB; // body blue [0..100] %
00210                         sint16 prox[8]; // 
00211                         sint16 camR[60]; // camera red (left, middle, right) [0..100] %
00212                         sint16 camG[60]; // camera green (left, middle, right) [0..100] %
00213                         sint16 camB[60]; // camera blue (left, middle, right) [0..100] %
00214                         sint16 energy;
00215                         sint16 user[256];
00216                 } variables;
00217                 
00218         public:
00219                 AsebaFeedableEPuck(int id) :
00220                         joystick(-1)
00221                 {
00222                         vm.nodeId = id;
00223                         
00224                         bytecode.resize(512);
00225                         vm.bytecode = &bytecode[0];
00226                         vm.bytecodeSize = bytecode.size();
00227                         
00228                         stack.resize(64);
00229                         vm.stack = &stack[0];
00230                         vm.stackSize = stack.size();
00231                         
00232                         vm.variables = reinterpret_cast<sint16 *>(&variables);
00233                         vm.variablesSize = sizeof(variables) / sizeof(sint16);
00234                         
00235                         AsebaVMInit(&vm);
00236                         
00237                         variables.id = id;
00238                         variables.productId = ASEBA_PID_PLAYGROUND;
00239                 }
00240                 
00241                 virtual ~AsebaFeedableEPuck()
00242                 {
00243                         
00244                 }
00245                 
00246         public:
00247                 double toDoubleClamp(sint16 val, double mul, double min, double max)
00248                 {
00249                         double v = static_cast<double>(val) * mul;
00250                         if (v > max)
00251                                 v = max;
00252                         else if (v < min)
00253                                 v = min;
00254                         return v;
00255                 }
00256                 
00257                 void controlStep(double dt)
00258                 {
00259                         // set physical variables
00260                         leftSpeed = (double)(variables.speedL * 12.8) / 1000.;
00261                         rightSpeed = (double)(variables.speedR * 12.8) / 1000.;
00262                         Color c;
00263                         c.setR(toDoubleClamp(variables.colorR, 0.01, 0, 1));
00264                         c.setG(toDoubleClamp(variables.colorG, 0.01, 0, 1));
00265                         c.setB(toDoubleClamp(variables.colorB, 0.01, 0, 1));
00266                         setColor(c);
00267                         
00268                         // if this robot is controlled by a joystick, override the wheel speed commands
00269                         #ifdef USE_SDL
00270                         SDL_JoystickUpdate();
00271                         if (joystick >= 0 && joystick < playgroundViewer->joysticks.size())
00272                         {
00273                                 const double SPEED_MAX = 13.;
00274                                 double x = SDL_JoystickGetAxis(playgroundViewer->joysticks[joystick], 0) / (32767. / SPEED_MAX);
00275                                 double y = -SDL_JoystickGetAxis(playgroundViewer->joysticks[joystick], 1) / (32767. / SPEED_MAX);
00276                                 leftSpeed = y + x;
00277                                 rightSpeed = y - x;
00278                         }
00279                         #endif // USE_SDL
00280                         
00281                         // do motion
00282                         FeedableEPuck::controlStep(dt);
00283                         
00284                         // get physical variables
00285                         variables.prox[0] = static_cast<sint16>(infraredSensor0.finalValue);
00286                         variables.prox[1] = static_cast<sint16>(infraredSensor1.finalValue);
00287                         variables.prox[2] = static_cast<sint16>(infraredSensor2.finalValue);
00288                         variables.prox[3] = static_cast<sint16>(infraredSensor3.finalValue);
00289                         variables.prox[4] = static_cast<sint16>(infraredSensor4.finalValue);
00290                         variables.prox[5] = static_cast<sint16>(infraredSensor5.finalValue);
00291                         variables.prox[6] = static_cast<sint16>(infraredSensor6.finalValue);
00292                         variables.prox[7] = static_cast<sint16>(infraredSensor7.finalValue);
00293                         for (size_t i = 0; i < 60; i++)
00294                         {
00295                                 variables.camR[i] = static_cast<sint16>(camera.image[i].r() * 100.);
00296                                 variables.camG[i] = static_cast<sint16>(camera.image[i].g() * 100.);
00297                                 variables.camB[i] = static_cast<sint16>(camera.image[i].b() * 100.);
00298                         }
00299                         
00300                         variables.energy = static_cast<sint16>(energy);
00301                         
00302                         // run VM
00303                         AsebaVMRun(&vm, 1000);
00304                         
00305                         // reschedule a IR sensors and camera events if we are not in step by step
00306                         if (AsebaMaskIsClear(vm.flags, ASEBA_VM_STEP_BY_STEP_MASK) || AsebaMaskIsClear(vm.flags, ASEBA_VM_EVENT_ACTIVE_MASK))
00307                         {
00308                                 AsebaVMSetupEvent(&vm, ASEBA_EVENT_LOCAL_EVENTS_START);
00309                                 AsebaVMRun(&vm, 1000);
00310                                 AsebaVMSetupEvent(&vm, ASEBA_EVENT_LOCAL_EVENTS_START-1);
00311                                 AsebaVMRun(&vm, 1000);
00312                         }
00313                 }
00314         };
00315         
00316         class EPuckFeeding : public LocalInteraction
00317         {
00318         public:
00319                 double energy;
00320 
00321         public :
00322                 EPuckFeeding(Robot *owner) : energy(EPUCK_FEEDER_INITIAL_ENERGY)
00323                 {
00324                         r = EPUCK_FEEDER_RANGE;
00325                         this->owner = owner;
00326                 }
00327                 
00328                 void objectStep(double dt, World *w, PhysicalObject *po)
00329                 {
00330                         FeedableEPuck *epuck = dynamic_cast<FeedableEPuck *>(po);
00331                         if (epuck && energy > 0)
00332                         {
00333                                 double dEnergy = dt * EPUCK_FEEDER_D_ENERGY;
00334                                 epuck->energy += dEnergy;
00335                                 energy -= dEnergy;
00336                                 if (energy < EPUCK_FEEDER_THRESHOLD_HIDE)
00337                                         owner->setColor(EPUCK_FEEDER_COLOR_INACTIVE);
00338                         }
00339                 }
00340                 
00341                 void finalize(double dt, World *w)
00342                 {
00343                         if ((energy < EPUCK_FEEDER_THRESHOLD_SHOW) && (energy+dt >= EPUCK_FEEDER_THRESHOLD_SHOW))
00344                                 owner->setColor(EPUCK_FEEDER_COLOR_ACTIVE);
00345                         energy += EPUCK_FEEDER_RECHARGE_RATE * dt;
00346                         if (energy > EPUCK_FEEDER_MAX_ENERGY)
00347                                 energy = EPUCK_FEEDER_MAX_ENERGY;
00348                 }
00349         };
00350         
00351         class EPuckFeeder : public Robot
00352         {
00353         public:
00354                 EPuckFeeding feeding;
00355         
00356         public:
00357                 EPuckFeeder() : feeding(this)
00358                 {
00359                         setRectangular(3.2, 3.2, EPUCK_FEEDER_HEIGHT, -1);
00360                         addLocalInteraction(&feeding);
00361                         setColor(EPUCK_FEEDER_COLOR_ACTIVE);
00362                 }
00363         };
00364         
00365         class Door: public PhysicalObject
00366         {
00367         public:
00368                 virtual void open() = 0;
00369                 virtual void close() = 0;
00370         };
00371         
00372         class SlidingDoor : public Door
00373         {
00374         protected:
00375                 Point closedPos;
00376                 Point openedPos;
00377                 double moveDuration;
00378                 enum Mode
00379                 {
00380                         MODE_CLOSED,
00381                         MODE_OPENING,
00382                         MODE_OPENED,
00383                         MODE_CLOSING
00384                 } mode;
00385                 double moveTimeLeft;
00386         
00387         public:
00388                 SlidingDoor(const Point& closedPos, const Point& openedPos, const Point& size, double height, double moveDuration) :
00389                         closedPos(closedPos),
00390                         openedPos(openedPos),
00391                         moveDuration(moveDuration),
00392                         mode(MODE_CLOSED),
00393                         moveTimeLeft(0)
00394                 {
00395                         setRectangular(size.x, size.y, height, -1);
00396                 }
00397                 
00398                 virtual void controlStep(double dt)
00399                 {
00400                         // cos interpolation between positions
00401                         double alpha;
00402                         switch (mode)
00403                         {
00404                                 case MODE_CLOSED:
00405                                 pos = closedPos;
00406                                 break;
00407                                 
00408                                 case MODE_OPENING:
00409                                 alpha = (cos((moveTimeLeft*M_PI)/moveDuration) + 1.) / 2.;
00410                                 pos = openedPos * alpha + closedPos * (1 - alpha);
00411                                 moveTimeLeft -= dt;
00412                                 if (moveTimeLeft < 0)
00413                                         mode = MODE_OPENED;
00414                                 break;
00415                                 
00416                                 case MODE_OPENED:
00417                                 pos = openedPos;
00418                                 break;
00419                                 
00420                                 case MODE_CLOSING:
00421                                 alpha = (cos((moveTimeLeft*M_PI)/moveDuration) + 1.) / 2.;
00422                                 pos = closedPos * alpha + openedPos * (1 - alpha);
00423                                 moveTimeLeft -= dt;
00424                                 if (moveTimeLeft < 0)
00425                                         mode = MODE_CLOSED;
00426                                 break;
00427                                 
00428                                 default:
00429                                 break;
00430                         }
00431                         PhysicalObject::controlStep(dt);
00432                 }
00433                 
00434                 virtual void open(void)
00435                 {
00436                         if (mode == MODE_CLOSED)
00437                         {
00438                                 moveTimeLeft = moveDuration;
00439                                 mode = MODE_OPENING;
00440                         }
00441                         else if (mode == MODE_CLOSING)
00442                         {
00443                                 moveTimeLeft = moveDuration - moveTimeLeft;
00444                                 mode = MODE_OPENING;
00445                         }
00446                 }
00447                 
00448                 virtual void close(void)
00449                 {
00450                         if (mode == MODE_OPENED)
00451                         {
00452                                 moveTimeLeft = moveDuration;
00453                                 mode = MODE_CLOSING;
00454                         }
00455                         else if (mode == MODE_OPENING)
00456                         {
00457                                 moveTimeLeft = moveDuration - moveTimeLeft;
00458                                 mode = MODE_CLOSING;
00459                         }
00460                 }
00461         };
00462         
00463         class AreaActivating : public LocalInteraction
00464         {
00465         public:
00466                 bool active;
00467                 Polygone activeArea;
00468                 
00469         public:
00470                 AreaActivating(Robot *owner, const Polygone& activeArea) :
00471                         active(false),
00472                         activeArea(activeArea)
00473                 {
00474                         r = activeArea.getBoundingRadius();
00475                         this->owner = owner;
00476                 }
00477                 
00478                 virtual void init(double dt, World *w)
00479                 {
00480                         active = false;
00481                 }
00482                 
00483                 virtual void objectStep (double dt, World *w, PhysicalObject *po)
00484                 {
00485                         if (po != owner && dynamic_cast<Robot*>(po))
00486                         {
00487                                 active |= activeArea.isPointInside(po->pos - owner->pos);
00488                         }
00489                 }
00490         };
00491         
00492         class ActivationObject: public Robot
00493         {
00494         protected:
00495                 AreaActivating areaActivating;
00496                 bool wasActive;
00497                 Door* attachedDoor;
00498         
00499         public:
00500                 ActivationObject(const Point& pos, const Point& size, const Polygone& activeArea, Door* attachedDoor) :
00501                         areaActivating(this, activeArea),
00502                         wasActive(false),
00503                         attachedDoor(attachedDoor)
00504                 {
00505                         this->pos = pos;
00506                         setRectangular(size.x, size.y, ACTIVATION_OBJECT_HEIGHT, -1);
00507                         addLocalInteraction(&areaActivating);
00508                         setColor(ACTIVATION_OBJECT_COLOR);
00509                 }
00510                 
00511                 virtual void controlStep(double dt)
00512                 {
00513                         Robot::controlStep(dt);
00514                         
00515                         if (areaActivating.active != wasActive)
00516                         {
00517                                 //std::cerr << "Door activity changed to " << areaActivating.active <<  "\n";
00518                                 if (areaActivating.active)
00519                                         attachedDoor->open();
00520                                 else
00521                                         attachedDoor->close();
00522                                 wasActive = areaActivating.active;
00523                         }
00524                 }
00525         };
00526 
00527         PlaygroundViewer::PlaygroundViewer(World* world) : ViewerWidget(world), stream(0), energyPool(INITIAL_POOL_ENERGY)
00528         {
00529                 font.setPixelSize(16);
00530                 #if QT_VERSION >= 0x040400
00531                 font.setLetterSpacing(QFont::PercentageSpacing, 130);
00532                 #elif (!defined(_MSC_VER))
00533                 #warning "Some feature have been disabled because you are using Qt < 4.4.0 !"
00534                 #endif
00535                 try
00536                 {
00537                         Dashel::Hub::connect(QString("tcpin:port=%1").arg(ASEBA_DEFAULT_PORT).toStdString());
00538                 }
00539                 catch (Dashel::DashelException e)
00540                 {
00541                         QMessageBox::critical(0, QApplication::tr("Aseba Playground"), QApplication::tr("Cannot create listening port %0: %1").arg(ASEBA_DEFAULT_PORT).arg(e.what()));
00542                         abort();
00543                 }
00544                 playgroundViewer = this;
00545                 
00546                 #ifdef USE_SDL
00547                 if((SDL_Init(SDL_INIT_JOYSTICK)==-1))
00548                 {
00549                         std::cerr << "Error : Could not initialize SDL: " << SDL_GetError() << std::endl;
00550                         return;
00551                 }
00552                 
00553                 int joystickCount = SDL_NumJoysticks();
00554                 for (int i = 0; i < joystickCount; ++i)
00555                 {
00556                         SDL_Joystick* joystick = SDL_JoystickOpen(i);
00557                         if (!joystick)
00558                         {
00559                                 std::cerr << "Error: Can't open joystick " << i << std::endl;
00560                                 continue;
00561                         }
00562                         if (SDL_JoystickNumAxes(joystick) < 2)
00563                         {
00564                                 std::cerr << "Error: not enough axis on joystick" << i<< std::endl;
00565                                 SDL_JoystickClose(joystick);
00566                                 continue;
00567                         }
00568                         joysticks.push_back(joystick);
00569                 }
00570                 #endif // USE_SDL
00571         }
00572         
00573         PlaygroundViewer::~PlaygroundViewer()
00574         {
00575                 #ifdef USE_SDL
00576                 for (int i = 0; i < joysticks.size(); ++i)
00577                         SDL_JoystickClose(joysticks[i]);
00578                 SDL_Quit();
00579                 #endif // USE_SDL
00580         }
00581         
00582         void PlaygroundViewer::renderObjectsTypesHook()
00583         {
00584                 managedObjectsAliases[&typeid(AsebaFeedableEPuck)] = &typeid(EPuck);
00585         }
00586         
00587         void PlaygroundViewer::sceneCompletedHook()
00588         {
00589                 // create a map with names and scores
00590                 //qglColor(QColor::fromRgbF(0, 0.38 ,0.61));
00591                 qglColor(Qt::black);
00592                 
00593                 
00594                 int i = 0;
00595                 QString scoreString("Id.: E./Score. - ");
00596                 int totalScore = 0;
00597                 for (World::ObjectsIterator it = world->objects.begin(); it != world->objects.end(); ++it)
00598                 {
00599                         AsebaFeedableEPuck *epuck = dynamic_cast<AsebaFeedableEPuck*>(*it);
00600                         if (epuck)
00601                         {
00602                                 totalScore += (int)epuck->score;
00603                                 if (i != 0)
00604                                         scoreString += " - ";
00605                                 scoreString += QString("%0: %1/%2").arg(epuck->vm.nodeId).arg(epuck->variables.energy).arg((int)epuck->score);
00606                                 renderText(epuck->pos.x, epuck->pos.y, 10, QString("%0").arg(epuck->vm.nodeId), font);
00607                                 i++;
00608                         }
00609                 }
00610                 
00611                 renderText(16, 22, scoreString, font);
00612                 
00613                 renderText(16, 42, QString("E. in pool: %0 - total score: %1").arg(energyPool).arg(totalScore), font);
00614         }
00615         
00616         void PlaygroundViewer::connectionCreated(Dashel::Stream *stream)
00617         {
00618                 std::string targetName = stream->getTargetName();
00619                 if (targetName.substr(0, targetName.find_first_of(':')) == "tcp")
00620                 {
00621                         qDebug() << "New client connected.";
00622                         if (this->stream)
00623                         {
00624                                 closeStream(this->stream);
00625                                 qDebug() << "Disconnected old client.";
00626                         }
00627                         this->stream = stream;
00628                 }
00629         }
00630         
00631         void PlaygroundViewer::incomingData(Dashel::Stream *stream)
00632         {
00633                 uint16 temp;
00634                 uint16 len;
00635                 uint16 incomingMessageSource;
00636                 std::valarray<uint8> incomingMessageData;
00637                 
00638                 stream->read(&temp, 2);
00639                 len = bswap16(temp);
00640                 stream->read(&temp, 2);
00641                 incomingMessageSource = bswap16(temp);
00642                 incomingMessageData.resize(len+2);
00643                 stream->read(&incomingMessageData[0], incomingMessageData.size());
00644                 
00645                 for (World::ObjectsIterator objectIt = world->objects.begin(); objectIt != world->objects.end(); ++objectIt)
00646                 {
00647                         AsebaFeedableEPuck *epuck = dynamic_cast<AsebaFeedableEPuck*>(*objectIt);
00648                         if (epuck)
00649                         {
00650                                 lastMessageSource = incomingMessageSource;
00651                                 lastMessageData.resize(incomingMessageData.size());
00652                                 memcpy(&lastMessageData[0], &incomingMessageData[0], incomingMessageData.size());
00653                                 AsebaProcessIncomingEvents(&(epuck->vm));
00654                         }
00655                 }
00656         }
00657         
00658         void PlaygroundViewer::connectionClosed(Dashel::Stream *stream, bool abnormal)
00659         {
00660                 if (stream == this->stream)
00661                 {
00662                         this->stream = 0;
00663                         // clear breakpoints
00664                         for (World::ObjectsIterator objectIt = world->objects.begin(); objectIt != world->objects.end(); ++objectIt)
00665                         {
00666                                 AsebaFeedableEPuck *epuck = dynamic_cast<AsebaFeedableEPuck*>(*objectIt);
00667                                 if (epuck)
00668                                         (epuck->vm).breakpointsCount = 0;
00669                         }
00670                 }
00671                 if (abnormal)
00672                         qDebug() << "Client has disconnected unexpectedly.";
00673                 else
00674                         qDebug() << "Client has disconnected properly.";
00675         }
00676         
00677         void PlaygroundViewer::timerEvent(QTimerEvent * event)
00678         {
00679                 // do a network step
00680                 Hub::step();
00681                 
00682                 // do a simulator/gui step
00683                 ViewerWidget::timerEvent(event);
00684         }
00685 }
00686 
00687 // Implementation of native function
00688 
00689 extern "C" void PlaygroundNative_energysend(AsebaVMState *vm)
00690 {
00691         int index = AsebaNativePopArg(vm);
00692         // find related VM
00693         for (Enki::World::ObjectsIterator objectIt = playgroundViewer->getWorld()->objects.begin(); objectIt != playgroundViewer->getWorld()->objects.end(); ++objectIt)
00694         {
00695                 Enki::AsebaFeedableEPuck *epuck = dynamic_cast<Enki::AsebaFeedableEPuck*>(*objectIt);
00696                 if (epuck && (&(epuck->vm) == vm) && (epuck->energy > EPUCK_INITIAL_ENERGY))
00697                 {
00698                         uint16 amount = vm->variables[index];
00699                         
00700                         unsigned toSend = std::min((unsigned)amount, (unsigned)epuck->energy);
00701                         playgroundViewer->energyPool += toSend;
00702                         epuck->energy -= toSend;
00703                 }
00704         }
00705 }
00706 
00707 extern "C" void PlaygroundNative_energyreceive(AsebaVMState *vm)
00708 {
00709         int index = AsebaNativePopArg(vm);
00710         // find related VM
00711         for (Enki::World::ObjectsIterator objectIt = playgroundViewer->getWorld()->objects.begin(); objectIt != playgroundViewer->getWorld()->objects.end(); ++objectIt)
00712         {
00713                 Enki::AsebaFeedableEPuck *epuck = dynamic_cast<Enki::AsebaFeedableEPuck*>(*objectIt);
00714                 if (epuck && (&(epuck->vm) == vm))
00715                 {
00716                         uint16 amount = vm->variables[index];
00717                         
00718                         unsigned toReceive = std::min((unsigned)amount, (unsigned)playgroundViewer->energyPool);
00719                         playgroundViewer->energyPool -= toReceive;
00720                         epuck->energy += toReceive;
00721                 }
00722         }
00723 }
00724 
00725 extern "C" void PlaygroundNative_energyamount(AsebaVMState *vm)
00726 {
00727         int index = AsebaNativePopArg(vm);
00728         vm->variables[index] = playgroundViewer->energyPool;
00729 }
00730 
00731 
00732 // Implementation of aseba glue code
00733 
00734 extern "C" void AsebaPutVmToSleep(AsebaVMState *vm) 
00735 {
00736 }
00737 
00738 extern "C" void AsebaSendBuffer(AsebaVMState *vm, const uint8* data, uint16 length)
00739 {
00740         Dashel::Stream* stream = playgroundViewer->stream;
00741         if (stream)
00742         {
00743                 try
00744                 {
00745                         uint16 temp;
00746                         
00747                         // this may happen if target has disconnected
00748                         temp = bswap16(length - 2);
00749                         stream->write(&temp, 2);
00750                         temp = bswap16(vm->nodeId);
00751                         stream->write(&temp, 2);
00752                         stream->write(data, length);
00753                         stream->flush();
00754                 }
00755                 catch (Dashel::DashelException e)
00756                 {
00757                         qDebug() << "Cannot write to socket: " << e.what();
00758                 }
00759         }
00760         
00761         // send to other robots too
00762         playgroundViewer->lastMessageSource = vm->nodeId;
00763         playgroundViewer->lastMessageData.resize(length);
00764         memcpy(&playgroundViewer->lastMessageData[0], data, length);
00765         for (Enki::World::ObjectsIterator objectIt = playgroundViewer->getWorld()->objects.begin(); objectIt != playgroundViewer->getWorld()->objects.end(); ++objectIt)
00766         {
00767                 Enki::AsebaFeedableEPuck *epuck = dynamic_cast<Enki::AsebaFeedableEPuck*>(*objectIt);
00768                 if (epuck && (&(epuck->vm) != vm))
00769                         AsebaProcessIncomingEvents(&(epuck->vm));
00770         }
00771 }
00772 
00773 extern "C" uint16 AsebaGetBuffer(AsebaVMState *vm, uint8* data, uint16 maxLength, uint16* source)
00774 {
00775         if (playgroundViewer->lastMessageData.size())
00776         {
00777                 *source = playgroundViewer->lastMessageSource;
00778                 memcpy(data, &playgroundViewer->lastMessageData[0], playgroundViewer->lastMessageData.size());
00779         }
00780         return playgroundViewer->lastMessageData.size();
00781 }
00782 
00783 // this is really ugly, but a necessary hack
00784 static char ePuckName[9] = "e-puck  ";
00785 
00786 extern "C" const AsebaVMDescription* AsebaGetVMDescription(AsebaVMState *vm)
00787 {
00788         ePuckName[7] = '0' + vm->nodeId;
00789         vmDescription.name = ePuckName;
00790         return &vmDescription;
00791 }
00792 
00793 static const AsebaLocalEventDescription localEvents[] = {
00794         { "ir_sensors", "New IR sensors values available" },
00795         {"camera", "New camera picture available"},
00796         { NULL, NULL }
00797 };
00798 
00799 extern "C" const AsebaLocalEventDescription * AsebaGetLocalEventsDescriptions(AsebaVMState *vm)
00800 {
00801         return localEvents;
00802 }
00803 
00804 extern "C" const AsebaNativeFunctionDescription * const * AsebaGetNativeFunctionsDescriptions(AsebaVMState *vm)
00805 {
00806         return nativeFunctionsDescriptions;
00807 }
00808 
00809 extern "C" void AsebaNativeFunction(AsebaVMState *vm, uint16 id)
00810 {
00811         nativeFunctions[id](vm);
00812 }
00813 
00814 extern "C" void AsebaWriteBytecode(AsebaVMState *vm)
00815 {
00816 }
00817 
00818 extern "C" void AsebaResetIntoBootloader(AsebaVMState *vm)
00819 {
00820 }
00821 
00822 extern "C" void AsebaAssert(AsebaVMState *vm, AsebaAssertReason reason)
00823 {
00824         std::cerr << "\nFatal error: ";
00825         switch (vm->nodeId)
00826         {
00827                 case 1: std::cerr << "left motor module"; break;
00828                 case 2: std::cerr << "right motor module"; break;
00829                 case 3: std::cerr << "proximity sensors module"; break;
00830                 case 4: std::cerr << "distance sensors module"; break;
00831                 default: std::cerr << "unknown module"; break;
00832         }
00833         std::cerr << " has produced exception: ";
00834         switch (reason)
00835         {
00836                 case ASEBA_ASSERT_UNKNOWN: std::cerr << "undefined"; break;
00837                 case ASEBA_ASSERT_UNKNOWN_UNARY_OPERATOR: std::cerr << "unknown unary operator"; break;
00838                 case ASEBA_ASSERT_UNKNOWN_BINARY_OPERATOR: std::cerr << "unknown binary operator"; break;
00839                 case ASEBA_ASSERT_UNKNOWN_BYTECODE: std::cerr << "unknown bytecode"; break;
00840                 case ASEBA_ASSERT_STACK_OVERFLOW: std::cerr << "stack overflow"; break;
00841                 case ASEBA_ASSERT_STACK_UNDERFLOW: std::cerr << "stack underflow"; break;
00842                 case ASEBA_ASSERT_OUT_OF_VARIABLES_BOUNDS: std::cerr << "out of variables bounds"; break;
00843                 case ASEBA_ASSERT_OUT_OF_BYTECODE_BOUNDS: std::cerr << "out of bytecode bounds"; break;
00844                 case ASEBA_ASSERT_STEP_OUT_OF_RUN: std::cerr << "step out of run"; break;
00845                 case ASEBA_ASSERT_BREAKPOINT_OUT_OF_BYTECODE_BOUNDS: std::cerr << "breakpoint out of bytecode bounds"; break;
00846                 case ASEBA_ASSERT_EMIT_BUFFER_TOO_LONG: std::cerr << "tried to emit a buffer too long"; break;
00847                 default: std::cerr << "unknown exception"; break;
00848         }
00849         std::cerr << ".\npc = " << vm->pc << ", sp = " << vm->sp;
00850         std::cerr << "\nResetting VM" << std::endl;
00851         assert(false);
00852         AsebaVMInit(vm);
00853 }
00854 
00855 
00856 int main(int argc, char *argv[])
00857 {
00858         QApplication app(argc, argv);
00859         
00860         /*
00861         // Translation support
00862         QTranslator qtTranslator;
00863         qtTranslator.load("qt_" + QLocale::system().name());
00864         app.installTranslator(&qtTranslator);
00865         
00866         QTranslator translator;
00867         translator.load(QString(":/asebachallenge_") + QLocale::system().name());
00868         app.installTranslator(&translator);
00869         */
00870         
00871         // create document
00872         QDomDocument domDocument("aseba-playground");
00873         
00874         // Get cmd line arguments
00875         QString fileName;
00876         bool ask = true;
00877         if (argc > 1)
00878         {
00879                 fileName = argv[1];
00880                 ask = false;
00881         }
00882         
00883         // Try to load xml config file
00884         do
00885         {
00886                 if (ask)
00887                 {
00888                         QString lastFileName = QSettings("EPFL-LSRO-Mobots", "Aseba Playground").value("last file").toString();
00889                         fileName = QFileDialog::getOpenFileName(0, app.tr("Open Scenario"), lastFileName, app.tr("playground scenario (*.playground)"));
00890                 }
00891                 ask = true;
00892                 
00893                 if (fileName.isEmpty())
00894                 {
00895                         std::cerr << "You must specify a valid setup scenario on the command line or choose one in the file dialog\n";
00896                         exit(1);
00897                 }
00898                 
00899                 QFile file(fileName);
00900                 if (file.open(QIODevice::ReadOnly))
00901                 {
00902                         QString errorStr;
00903                         int errorLine, errorColumn;
00904                         if (!domDocument.setContent(&file, false, &errorStr, &errorLine, &errorColumn))
00905                         {
00906                                 QMessageBox::information(0, "Aseba Playground",
00907                                                                                 app.tr("Parse error at file %1, line %2, column %3:\n%4")
00908                                                                                 .arg(fileName)
00909                                                                                 .arg(errorLine)
00910                                                                                 .arg(errorColumn)
00911                                                                                 .arg(errorStr));
00912                         }
00913                         else
00914                         {
00915                                 QSettings("EPFL-LSRO-Mobots", "Aseba Playground").setValue("last file", fileName);
00916                                 break;
00917                         }
00918                 }
00919         }
00920         while (true);
00921         
00922         // Scan for colors
00923         typedef QMap<QString, Enki::Color> ColorsMap;
00924         ColorsMap colorsMap;
00925         QDomElement colorE = domDocument.documentElement().firstChildElement("color");
00926         while (!colorE.isNull())
00927         {
00928                 colorsMap[colorE.attribute("name")] = Enki::Color(
00929                         colorE.attribute("r").toDouble(),
00930                         colorE.attribute("g").toDouble(),
00931                         colorE.attribute("b").toDouble()
00932                 );
00933                 
00934                 colorE = colorE.nextSiblingElement ("color");
00935         }
00936         
00937         // Scan for areas
00938         typedef QMap<QString, Enki::Polygone> AreasMap;
00939         AreasMap areasMap;
00940         QDomElement areaE = domDocument.documentElement().firstChildElement("area");
00941         while (!areaE.isNull())
00942         {
00943                 Enki::Polygone p;
00944                 QDomElement pointE = areaE.firstChildElement("point");
00945                 while (!pointE.isNull())
00946                 {
00947                         p.push_back(Enki::Point(
00948                                 pointE.attribute("x").toDouble(),
00949                                 pointE.attribute("y").toDouble()
00950                         ));
00951                         pointE = pointE.nextSiblingElement ("point");
00952                 }
00953                 areasMap[areaE.attribute("name")] = p;
00954                 areaE = areaE.nextSiblingElement ("area");
00955         }
00956         
00957         // Create the world
00958         QDomElement worldE = domDocument.documentElement().firstChildElement("world");
00959         Enki::Color worldColor(Enki::Color::gray);
00960         if (!colorsMap.contains(worldE.attribute("color")))
00961                 std::cerr << "Warning, world walls color " << worldE.attribute("color").toStdString() << " undefined\n";
00962         else
00963                 worldColor = colorsMap[worldE.attribute("color")];
00964         Enki::World world(
00965                 worldE.attribute("w").toDouble(),
00966                 worldE.attribute("h").toDouble(),
00967                 worldColor
00968         );
00969         
00970         
00971         // Scan for walls
00972         QDomElement wallE = domDocument.documentElement().firstChildElement("wall");
00973         while (!wallE.isNull())
00974         {
00975                 Enki::PhysicalObject* wall = new Enki::PhysicalObject();
00976                 if (!colorsMap.contains(wallE.attribute("color")))
00977                         std::cerr << "Warning, color " << wallE.attribute("color").toStdString() << " undefined\n";
00978                 else
00979                         wall->setColor(colorsMap[wallE.attribute("color")]);
00980                 wall->pos.x = wallE.attribute("x").toDouble();
00981                 wall->pos.y = wallE.attribute("y").toDouble();
00982                 wall->setRectangular(
00983                         wallE.attribute("l1").toDouble(),
00984                         wallE.attribute("l2").toDouble(),
00985                         wallE.attribute("h").toDouble(),
00986                         -1
00987                 );
00988                 world.addObject(wall);
00989                 
00990                 wallE  = wallE.nextSiblingElement ("wall");
00991         }
00992         
00993         // Scan for feeders
00994         QDomElement feederE = domDocument.documentElement().firstChildElement("feeder");
00995         while (!feederE.isNull())
00996         {
00997                 Enki::EPuckFeeder* feeder = new Enki::EPuckFeeder;
00998                 feeder->pos.x = feederE.attribute("x").toDouble();
00999                 feeder->pos.y = feederE.attribute("y").toDouble();
01000                 world.addObject(feeder);
01001         
01002                 feederE = feederE.nextSiblingElement ("feeder");
01003         }
01004         // TODO: if needed, custom color to feeder
01005         
01006         // Scan for doors
01007         typedef QMap<QString, Enki::SlidingDoor*> DoorsMap;
01008         DoorsMap doorsMap;
01009         QDomElement doorE = domDocument.documentElement().firstChildElement("door");
01010         while (!doorE.isNull())
01011         {
01012                 Enki::SlidingDoor *door = new Enki::SlidingDoor(
01013                         Enki::Point(
01014                                 doorE.attribute("closedX").toDouble(),
01015                                 doorE.attribute("closedY").toDouble()
01016                         ),
01017                         Enki::Point(
01018                                 doorE.attribute("openedX").toDouble(),
01019                                 doorE.attribute("openedY").toDouble()
01020                         ),
01021                         Enki::Point(
01022                                 doorE.attribute("l1").toDouble(),
01023                                 doorE.attribute("l2").toDouble()
01024                         ),
01025                         doorE.attribute("h").toDouble(),
01026                         doorE.attribute("moveDuration").toDouble()
01027                 );
01028                 if (!colorsMap.contains(doorE.attribute("color")))
01029                         std::cerr << "Warning, door color " << doorE.attribute("color").toStdString() << " undefined\n";
01030                 else
01031                         door->setColor(colorsMap[doorE.attribute("color")]);
01032                 doorsMap[doorE.attribute("name")] = door;
01033                 world.addObject(door);
01034                 
01035                 doorE = doorE.nextSiblingElement ("door");
01036         }
01037         
01038         // Scan for activation, and link them with areas and doors
01039         QDomElement activationE = domDocument.documentElement().firstChildElement("activation");
01040         while (!activationE.isNull())
01041         {
01042                 if (areasMap.find(activationE.attribute("area")) == areasMap.end())
01043                 {
01044                         std::cerr << "Warning, area " << activationE.attribute("area").toStdString() << " undefined\n";
01045                         activationE = activationE.nextSiblingElement ("activation");
01046                         continue;
01047                 }
01048                 
01049                 if (doorsMap.find(activationE.attribute("door")) == doorsMap.end())
01050                 {
01051                         std::cerr << "Warning, door " << activationE.attribute("door").toStdString() << " undefined\n";
01052                         activationE = activationE.nextSiblingElement ("activation");
01053                         continue;
01054                 }
01055                 
01056                 const Enki::Polygone& area = *areasMap.find(activationE.attribute("area"));
01057                 Enki::Door* door = *doorsMap.find(activationE.attribute("door"));
01058                 
01059                 Enki::ActivationObject* activation = new Enki::ActivationObject(
01060                         Enki::Point(
01061                                 activationE.attribute("x").toDouble(),
01062                                 activationE.attribute("y").toDouble()
01063                         ),
01064                         Enki::Point(
01065                                 activationE.attribute("l1").toDouble(),
01066                                 activationE.attribute("l2").toDouble()
01067                         ),
01068                         area,
01069                         door
01070                 );
01071                 
01072                 world.addObject(activation);
01073                 
01074                 activationE = activationE.nextSiblingElement ("activation");
01075         }
01076         
01077         // Scan for e-puck
01078         QDomElement ePuckE = domDocument.documentElement().firstChildElement("e-puck");
01079         int ePuckCount = 0;
01080         while (!ePuckE.isNull())
01081         {
01082                 char buffer[9];
01083                 strncpy(buffer, "e-puck  ", sizeof(buffer));
01084                 Enki::AsebaFeedableEPuck* epuck = new Enki::AsebaFeedableEPuck(ePuckCount + 1);
01085                 buffer[7] = '0' + ePuckCount++;
01086                 epuck->pos.x = ePuckE.attribute("x").toDouble();
01087                 epuck->pos.y = ePuckE.attribute("y").toDouble();
01088                 if (ePuckE.hasAttribute("joystick"))
01089                         epuck->joystick = ePuckE.attribute("joystick").toInt();
01090                 epuck->name = buffer;
01091                 world.addObject(epuck);
01092                 ePuckE = ePuckE.nextSiblingElement ("e-puck");
01093         }
01094         
01095         // Create viewer
01096         Enki::PlaygroundViewer viewer(&world);
01097         
01098         // Show and run
01099         viewer.setWindowTitle("Playground - Stephane Magnenat (code) - Basilio Noris (gfx)");
01100         viewer.show();
01101         
01102         return app.exec();
01103 }


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