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 #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
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
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
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;
00206 sint16 speedR;
00207 sint16 colorR;
00208 sint16 colorG;
00209 sint16 colorB;
00210 sint16 prox[8];
00211 sint16 camR[60];
00212 sint16 camG[60];
00213 sint16 camB[60];
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
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
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
00282 FeedableEPuck::controlStep(dt);
00283
00284
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
00303 AsebaVMRun(&vm, 1000);
00304
00305
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
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
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
00590
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
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
00680 Hub::step();
00681
00682
00683 ViewerWidget::timerEvent(event);
00684 }
00685 }
00686
00687
00688
00689 extern "C" void PlaygroundNative_energysend(AsebaVMState *vm)
00690 {
00691 int index = AsebaNativePopArg(vm);
00692
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
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
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
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
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
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
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872 QDomDocument domDocument("aseba-playground");
00873
00874
00875 QString fileName;
00876 bool ask = true;
00877 if (argc > 1)
00878 {
00879 fileName = argv[1];
00880 ask = false;
00881 }
00882
00883
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
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
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
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
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
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
01005
01006
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
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
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
01096 Enki::PlaygroundViewer viewer(&world);
01097
01098
01099 viewer.setWindowTitle("Playground - Stephane Magnenat (code) - Basilio Noris (gfx)");
01100 viewer.show();
01101
01102 return app.exec();
01103 }