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