src/robomon/RobomonAtlantis.cpp

   1 /*
   2  * RobomonAtlantis.cpp                  07/10/31
   3  *
   4  * Robot`s visualization and control GUI for robot of the
   5  * Eurobot 2008 (Mission to Mars).
   6  *
   7  * Copyright: (c) 2008 CTU Dragons
   8  *            CTU FEE - Department of Control Engineering
   9  * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
  10  * License: GNU GPL v.2
  11  */
  12
  13 #include <QtGui>
  14 #include <queue>
  15 #include <cstdlib>
  16 #include <sys/shm.h>
  17 #include <sys/stat.h>
  18 #include <sys/socket.h>
  19 #include <netinet/in.h>
  20 #include <arpa/inet.h>
  21 #include <stdio.h>
  22 #include <unistd.h>
  23 #include <math.h>
  24
  25 #include <orte.h>
  26 #include <path_planner.h>
  27 #include <robodim.h>
  28 #include <sharp.h>
  29 #include <trgen.h>
  30 #include <map.h>
  31 #include <robomath.h>
  32 #include <hokuyo.h>
  33 #include "PlaygroundScene.h"
  34 #include "MiscGui.h"
  35 #include "robomon_orte.h"
  36 #include "RobomonAtlantis.h"
  37
  38 #include <QCoreApplication>
  39 #include <QEvent>
  40 #include <QKeyEvent>
  41 #include <QDebug>
  42 #include <QMessageBox>
  43 #include <servos.h>
  44
  45 RobomonAtlantis::RobomonAtlantis(QWidget *parent)
  46         : QWidget(parent)
  47 {
  48         QFont font;
  49         font.setPointSize(7);
  50         setFont(font);
  51
  52         debugWindowEnabled = false;
  53
  54         createLeftLayout();
  55         createRightLayout();
  56
  57         QHBoxLayout *mainLayout = new QHBoxLayout;
  58         mainLayout->addLayout(leftLayout);
  59         mainLayout->addLayout(rightLayout);
  60         setLayout(mainLayout);
  61
  62         createOrte();
  63         createRobots();
  64         createActions();
  65
  66         setFocusPolicy(Qt::StrongFocus);
  67         sharedMemoryOpened = false;
  68         WDBG("Youuuhouuuu!!");
  69 }
  70
  71 /**********************************************************************
  72  * GUI
  73  **********************************************************************/
  74 void RobomonAtlantis::createLeftLayout()
  75 {
  76         leftLayout = new QVBoxLayout();
  77
  78         createDebugGroupBox();
  79         debugWindowEnabled = true;
  80         createPlaygroundGroupBox();
  81
  82         leftLayout->addWidget(playgroundGroupBox);
  83         leftLayout->addWidget(debugGroupBox);
  84 }
  85
  86 void RobomonAtlantis::createRightLayout()
  87 {
  88         rightLayout = new QVBoxLayout();
  89         QGridLayout *layout = new QGridLayout();
  90         QVBoxLayout *vlayout = new QVBoxLayout();
  91
  92         createPositionGroupBox();
  93         createMiscGroupBox();
  94         createActuatorsGroupBox();
  95         createDIOGroupBox();
  96         createPowerGroupBox();
  97         createSensorsGroupBox();
  98
  99         vlayout->addWidget(positionGroupBox);
 100         vlayout->addWidget(miscGroupBox);
 101         layout->addLayout(vlayout, 0, 0);
 102         layout->addWidget(actuatorsGroupBox, 0, 1);
 103 //      layout->addWidget(dioGroupBox, 0, 2);
 104
 105         rightLayout->addLayout(layout);
 106         rightLayout->addWidget(powerGroupBox);
 107         rightLayout->addWidget(sensorsGroupBox);
 108 }
 109
 110 void RobomonAtlantis::createPlaygroundGroupBox()
 111 {
 112         playgroundGroupBox = new QGroupBox(tr("Playground"));
 113         QHBoxLayout *layout = new QHBoxLayout();
 114
 115         playgroundScene =
 116                 new PlaygroundScene(PlaygroundScene::PLAYGROUND_EUROBOT2008);
 117         playgroundSceneView = new QGraphicsView(playgroundScene);
 118         playgroundSceneView->setMinimumWidth((int)(playgroundScene->width())+15);
 119         playgroundSceneView->setMinimumHeight((int)(playgroundScene->height())+15);
 120         layout->addWidget(playgroundSceneView);
 121
 122         playgroundGroupBox->setLayout(layout);
 123 }
 124
 125 void RobomonAtlantis::createPositionGroupBox()
 126 {
 127         positionGroupBox = new QGroupBox(tr("Position state"));
 128         QGridLayout *layout = new QGridLayout();
 129
 130         actPosX = new QLineEdit();
 131         actPosY = new QLineEdit();
 132         actPosPhi = new QLineEdit();
 133
 134         estPosX = new QLineEdit();
 135         estPosY = new QLineEdit();
 136         estPosPhi = new QLineEdit();
 137
 138         actPosX->setReadOnly(true);
 139         actPosY->setReadOnly(true);
 140         actPosPhi->setReadOnly(true);
 141
 142         estPosX->setReadOnly(true);
 143         estPosY->setReadOnly(true);
 144         estPosPhi->setReadOnly(true);
 145
 146         layout->addWidget(MiscGui::createLabel("X"), 1, 0);
 147         layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
 148         layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
 149
 150         layout->addWidget(MiscGui::createLabel("X"), 5, 0);
 151         layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
 152         layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
 153
 154         layout->addWidget(MiscGui::createLabel("Actual", Qt::AlignLeft), 0, 1);
 155         layout->addWidget(actPosX, 1, 1);
 156         layout->addWidget(actPosY, 2, 1);
 157         layout->addWidget(actPosPhi, 3, 1);
 158
 159         layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 4, 1);
 160         layout->addWidget(estPosX, 5, 1);
 161         layout->addWidget(estPosY, 6, 1);
 162         layout->addWidget(estPosPhi, 7, 1);
 163
 164         positionGroupBox->setLayout(layout);
 165 }
 166
 167 void RobomonAtlantis::createMiscGroupBox()
 168 {
 169         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 170         QGridLayout *layout = new QGridLayout();
 171
 172         showMapPushButton = new QPushButton(tr("Show &map"));
 173         showMapPushButton->setShortcut(tr("m"));
 174
 175         laserEngineCheckBox = new QCheckBox(tr("L&aser"));
 176         laserEngineCheckBox->setShortcut(tr("a"));
 177
 178         layout->addWidget(showMapPushButton, 0, 0);
 179         layout->addWidget(laserEngineCheckBox, 1, 0);
 180
 181         miscGroupBox->setLayout(layout);
 182 }
 183
 184 void RobomonAtlantis::createDebugGroupBox()
 185 {
 186         debugGroupBox = new QGroupBox(tr("Debug window"));
 187         QHBoxLayout *layout = new QHBoxLayout();
 188
 189         debugWindow = new QTextEdit();
 190         debugWindow->setReadOnly(true);
 191
 192         layout->addWidget(debugWindow);
 193         debugGroupBox->setLayout(layout);
 194 }
 195
 196 void RobomonAtlantis::createActuatorsGroupBox()
 197 {
 198         actuatorsGroupBox = new QGroupBox(tr("Actuators"));
 199         QHBoxLayout *layout = new QHBoxLayout();
 200
 201         createMotorsGroupBox();
 202         createServosGroupBox();
 203         createDrivesGroupBox();
 204
 205         layout->setAlignment(Qt::AlignLeft);
 206         layout->addWidget(enginesGroupBox);
 207         layout->addWidget(servosGroupBox);
 208         layout->addWidget(drivesGroupBox);
 209         actuatorsGroupBox->setLayout(layout);
 210 }
 211
 212 void RobomonAtlantis::createDIOGroupBox()
 213 {
 214         dioGroupBox = new QGroupBox(tr("DIO"));
 215         QGridLayout *layout = new QGridLayout();
 216         QFont font;
 217
 218         font.setPointSize(5);
 219         dioGroupBox->setFont(font);
 220
 221         for (int i=0; i<8; i++) {
 222                 diCheckBox[i] = new QCheckBox(QString("DI%1").arg(i));
 223                 doCheckBox[i] = new QCheckBox(QString("D0%1").arg(i));
 224                 layout->addWidget(diCheckBox[i], i, 0);
 225                 layout->addWidget(doCheckBox[i], i+8, 0);
 226         }
 227
 228         dioGroupBox->setMaximumWidth(70);
 229         dioGroupBox->setLayout(layout);
 230 }
 231
 232 void RobomonAtlantis::createPowerGroupBox()
 233 {
 234         powerGroupBox = new QGroupBox(tr("Power management"));
 235         QGridLayout *layout = new QGridLayout();
 236
 237         voltage33CheckBox = new QCheckBox(tr("&3.3V"));
 238         voltage50CheckBox = new QCheckBox(tr("&5.0V"));
 239         voltage80CheckBox = new QCheckBox(tr("&8.0V"));
 240
 241         voltage33CheckBox->setShortcut(tr("3"));
 242         voltage50CheckBox->setShortcut(tr("5"));
 243         voltage80CheckBox->setShortcut(tr("8"));
 244
 245         layout->addWidget(voltage33CheckBox, 0, 0);
 246         layout->addWidget(voltage50CheckBox, 0, 2);
 247         layout->addWidget(voltage80CheckBox, 0, 4); //1, 0);
 248         layout->addWidget(MiscGui::createLabel("BAT"), 0, 6); //1, 2);
 249
 250         voltage33LineEdit = new QLineEdit();
 251         voltage50LineEdit = new QLineEdit();
 252         voltage80LineEdit = new QLineEdit();
 253         voltageBATLineEdit = new QLineEdit();
 254
 255         voltage33LineEdit->setReadOnly(true);
 256         voltage50LineEdit->setReadOnly(true);
 257         voltage80LineEdit->setReadOnly(true);
 258         voltageBATLineEdit->setReadOnly(true);
 259
 260         layout->addWidget(voltage33LineEdit, 0, 1);
 261         layout->addWidget(voltage50LineEdit, 0, 3);
 262         layout->addWidget(voltage80LineEdit, 0, 5); //1, 1);
 263         layout->addWidget(voltageBATLineEdit, 0, 7); //1, 3);
 264
 265         powerGroupBox->setLayout(layout);
 266 }
 267
 268 void RobomonAtlantis::createSensorsGroupBox()
 269 {
 270         sensorsGroupBox = new QGroupBox(tr("Sensors"));
 271         QVBoxLayout *layout = new QVBoxLayout();
 272
 273         createSharpSensorsLayout();
 274
 275         layout->addLayout(sharpSensorsLayout);
 276         sensorsGroupBox->setLayout(layout);
 277 }
 278
 279 void RobomonAtlantis::createMotorsGroupBox()
 280 {
 281         enginesGroupBox = new QGroupBox(tr("Motors"));
 282         QVBoxLayout *layout = new QVBoxLayout();
 283         QHBoxLayout *layout1 = new QHBoxLayout();
 284         QHBoxLayout *layout2 = new QHBoxLayout();
 285
 286         leftMotorSlider = new QSlider(Qt::Vertical);
 287         rightMotorSlider = new QSlider(Qt::Vertical);
 288         bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
 289         stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
 290
 291         leftMotorSlider->setMinimum(-100);
 292         leftMotorSlider->setMaximum(100);
 293         leftMotorSlider->setTracking(false);
 294         leftMotorSlider->setTickPosition(QSlider::TicksLeft);
 295
 296         rightMotorSlider->setMinimum(-100);
 297         rightMotorSlider->setMaximum(100);
 298         rightMotorSlider->setTracking(false);
 299         rightMotorSlider->setTickPosition(QSlider::TicksRight);
 300
 301         stopMotorsPushButton->setMaximumWidth(90);
 302
 303         layout1->addWidget(leftMotorSlider);
 304         layout1->addWidget(MiscGui::createLabel("0"));
 305         layout1->addWidget(rightMotorSlider);
 306
 307         layout2->addWidget(bothMotorsCheckBox);
 308
 309         layout->addWidget(MiscGui::createLabel("100"));
 310         layout->addLayout(layout1);
 311         layout->addWidget(MiscGui::createLabel("-100"));
 312         layout->addLayout(layout2);
 313         layout->addWidget(stopMotorsPushButton);
 314         enginesGroupBox->setLayout(layout);
 315 }
 316
 317 void RobomonAtlantis::createServosGroupBox()
 318 {
 319         servosGroupBox = new QGroupBox(tr("Servos"));
 320         QGridLayout *layout = new QGridLayout();
 321
 322         leftBrushCheckBox = new QCheckBox(tr("&Left Brush"));
 323         rightBrushCheckBox = new QCheckBox(tr("&Right Brush"));
 324         pusherCheckBox = new QCheckBox(tr("&Pusher"));
 325
 326         leftBrushCheckBox->setShortcut(tr("l"));
 327         rightBrushCheckBox->setShortcut(tr("r"));
 328         pusherCheckBox->setShortcut(tr("p"));
 329
 330         layout->addWidget(leftBrushCheckBox, 0, 0);
 331         layout->addWidget(rightBrushCheckBox, 1, 0);
 332         layout->addWidget(pusherCheckBox, 2, 0);
 333         servosGroupBox->setLayout(layout);
 334 }
 335
 336
 337 void RobomonAtlantis::createDrivesGroupBox()
 338 {
 339         drivesGroupBox = new QGroupBox(tr("Drives"));
 340         QGridLayout *layout = new QGridLayout();
 341
 342         leftBrushDial = new QDial();
 343         rightBrushDial = new QDial();
 344         bagrDial = new QDial();
 345         carouselDial = new QDial();
 346
 347         leftBrushDriveCheckBox = new QCheckBox(tr("L&eft brush"));
 348         rightBrushDriveCheckBox = new QCheckBox(tr("Ri&ght brush"));
 349         bagrDriveCheckBox = new QCheckBox(tr("B&agr"));
 350
 351         leftBrushDial->setRange(0,200);
 352         leftBrushDial->setValue(100);
 353         rightBrushDial->setRange(0,200);
 354         rightBrushDial->setValue(100);
 355         bagrDial->setRange(0,200);
 356         bagrDial->setValue(100);
 357         carouselDial->setRange(0,4);
 358         carouselDial->setNotchesVisible(1);
 359
 360         layout->addWidget(leftBrushDial, 0, 0);
 361         layout->addWidget(leftBrushDriveCheckBox, 0, 1);
 362         layout->addWidget(rightBrushDial, 1, 0);
 363         layout->addWidget(rightBrushDriveCheckBox, 1, 1);
 364         layout->addWidget(bagrDial, 2, 0);
 365         layout->addWidget(bagrDriveCheckBox, 2, 1);
 366         layout->addWidget(carouselDial, 3, 0);
 367         drivesGroupBox->setLayout(layout);
 368 }
 369
 370 void RobomonAtlantis::createSharpSensorsLayout()
 371 {
 372         sharpSensorsLayout = new QVBoxLayout();
 373         QGridLayout *layout3 = new QGridLayout();
 374
 375         obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
 376
 377         obstacleSimulationCheckBox->setShortcut(tr("o"));
 378
 379         layout3->addWidget(obstacleSimulationCheckBox, 0, 1);
 380
 381         sharpSensorsLayout->addLayout(layout3);
 382 }
 383
 384 void RobomonAtlantis::createRobots()
 385 {
 386         robotActPos = new Robot(NULL, playgroundScene, Qt::black);
 387         robotActPos->setZValue(10);
 388         robotEstPos = new Robot(NULL, playgroundScene, Qt::green);
 389         robotEstPos->setZValue(10);
 390
 391         playgroundScene->addItem(robotActPos);
 392         playgroundScene->addItem(robotEstPos);
 393 }
 394
 395 /**********************************************************************
 396  * GUI actions
 397  **********************************************************************/
 398 void RobomonAtlantis::createActions()
 399 {
 400         /* power management */
 401         connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
 402                         this, SLOT(setVoltage33(int)));
 403         connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
 404                         this, SLOT(setVoltage50(int)));
 405         connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
 406                         this, SLOT(setVoltage80(int)));
 407
 408         /* motors */
 409         connect(leftMotorSlider, SIGNAL(valueChanged(int)),
 410                         this, SLOT(setLeftMotor(int)));
 411         connect(rightMotorSlider, SIGNAL(valueChanged(int)),
 412                         this, SLOT(setRightMotor(int)));
 413         connect(stopMotorsPushButton, SIGNAL(clicked()),
 414                         this, SLOT(stopMotors()));
 415
 416         /* servos */
 417         connect(leftBrushCheckBox, SIGNAL(stateChanged(int)),
 418                         this, SLOT(moveLeftBrush(int)));
 419         connect(rightBrushCheckBox, SIGNAL(stateChanged(int)),
 420                         this, SLOT(moveRightBrush(int)));
 421
 422         /* drives */
 423         connect(leftBrushDial, SIGNAL(valueChanged(int)),
 424                         this, SLOT(setDrives(int)));
 425         connect(rightBrushDial, SIGNAL(valueChanged(int)),
 426                         this, SLOT(setDrives(int)));
 427         connect(bagrDial, SIGNAL(valueChanged(int)),
 428                         this, SLOT(setDrives(int)));
 429         connect(carouselDial, SIGNAL(valueChanged(int)),
 430                         this, SLOT(setDrives(int)));
 431         connect(leftBrushDriveCheckBox, SIGNAL(stateChanged(int)),
 432                         this, SLOT(setLeftBrushDriveCB(int)));
 433         connect(rightBrushDriveCheckBox, SIGNAL(stateChanged(int)),
 434                         this, SLOT(setRightBrushDriveCB(int)));
 435         connect(bagrDriveCheckBox, SIGNAL(stateChanged(int)),
 436                         this, SLOT(setBagrDriveCB(int)));
 437         /* DIO */
 438         for (int i=0; i<8; i++)
 439                 connect(doCheckBox[0], SIGNAL(stateChanged(int)),
 440                                         this, SLOT(setDO(int)));
 441
 442         /* laser control */
 443         connect(laserEngineCheckBox, SIGNAL(stateChanged(int)),
 444                         this, SLOT(setLaser(int)));
 445
 446         /* path planning map */
 447         connect(showMapPushButton, SIGNAL(clicked()),
 448                         this, SLOT(showMap()));
 449
 450         /* obstacle simulation */
 451         simulationEnabled = 0;
 452         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 453                         this, SLOT(setSimulation(int)));
 454         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 455                         this, SLOT(setObstacleSimulation(int)));
 456         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 457                         playgroundScene, SLOT(showObstacle(int)));
 458         connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
 459                         this, SLOT(changeObstacle(QPointF)));
 460 }
 461
 462 void RobomonAtlantis::setVoltage33(int state)
 463 {
 464         if (state)
 465                 orte.pwr_ctrl.voltage33 = true;
 466         else
 467                 orte.pwr_ctrl.voltage33 = false;
 468 }
 469
 470 void RobomonAtlantis::setVoltage50(int state)
 471 {
 472         if (state)
 473                 orte.pwr_ctrl.voltage50 = true;
 474         else
 475                 orte.pwr_ctrl.voltage50 = false;
 476 }
 477
 478 void RobomonAtlantis::setVoltage80(int state)
 479 {
 480         if (state)
 481                 orte.pwr_ctrl.voltage80 = true;
 482         else
 483                 orte.pwr_ctrl.voltage80 = false;
 484 }
 485
 486 void RobomonAtlantis::setLeftMotor(int value)
 487 {
 488         short int leftMotor;
 489         short int rightMotor;
 490
 491         if(bothMotorsCheckBox->isChecked())
 492                 rightMotorSlider->setValue(value);
 493
 494         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 495         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 496
 497         orte.motion_speed.left = leftMotor;
 498         orte.motion_speed.right = rightMotor;
 499
 500 }
 501
 502 void RobomonAtlantis::setRightMotor(int value)
 503 {
 504         short int leftMotor;
 505         short int rightMotor;
 506
 507         if(bothMotorsCheckBox->isChecked())
 508                 leftMotorSlider->setValue(value);
 509
 510         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 511         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 512
 513         orte.motion_speed.left = leftMotor;
 514         orte.motion_speed.right = rightMotor;
 515
 516 }
 517
 518 void RobomonAtlantis::stopMotors()
 519 {
 520         leftMotorSlider->setValue(0);
 521         rightMotorSlider->setValue(0);
 522 }
 523
 524 void RobomonAtlantis::moveServos(int value)
 525 {
 526         /* FIXME: servos action comes here */
 527 }
 528
 529 void RobomonAtlantis::moveLeftBrush(int state)
 530 {
 531         if (state)
 532                 orte.servos.brush_left = BRUSH_LEFT_OPEN;
 533         else
 534                 orte.servos.brush_left = BRUSH_LEFT_CLOSE;
 535 }
 536
 537 void RobomonAtlantis::moveRightBrush(int state)
 538 {
 539         if (state)
 540                 orte.servos.brush_right = BRUSH_RIGHT_OPEN;
 541         else
 542                 orte.servos.brush_right = BRUSH_RIGHT_CLOSE;
 543 }
 544
 545 void RobomonAtlantis::setDrives(int state)
 546 {
 547         orte.drives.brush_left = leftBrushDial->value();
 548         orte.drives.brush_right = rightBrushDial->value();
 549         orte.drives.bagr = bagrDial->value();
 550         orte.drives.carousel_pos = carouselDial->value();
 551 }
 552
 553 void RobomonAtlantis::setLeftBrushDriveCB(int state) {
 554
 555         if(state)
 556                 orte.drives.brush_left = LEFT_BRUSH_DRIVE_ON;
 557         else
 558                 orte.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
 559 }
 560
 561
 562 void RobomonAtlantis::setRightBrushDriveCB(int state) {
 563
 564         if(state)
 565                 orte.drives.brush_right = RIGHT_BRUSH_DRIVE_ON;
 566         else
 567                 orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
 568 }
 569
 570
 571 void RobomonAtlantis::setBagrDriveCB(int state) {
 572
 573         if(state)
 574                 orte.drives.bagr = BAGR_DRIVE_ON;
 575         else
 576                 orte.drives.bagr = BAGR_DRIVE_OFF;
 577 }
 578
 579 void RobomonAtlantis::setDO(int state)
 580 {
 581         /* FIXME: digital output control comes here */
 582 }
 583
 584 void RobomonAtlantis::setLaser(int state) {
 585
 586         if(state)
 587                 orte.laser_cmd.speed = LASER_DRIVE_ON;
 588         else
 589                 orte.laser_cmd.speed = LASER_DRIVE_OFF;
 590         ORTEPublicationSend(orte.publication_laser_cmd);
 591 }
 592
 593 void RobomonAtlantis::showMap()
 594 {
 595         openSharedMemory();
 596
 597         if (sharedMemoryOpened == false)
 598                 return;
 599
 600         mapTimer = new QTimer(this);
 601         connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 602         mapTimer->start(200);
 603
 604         showMapPushButton->setText(tr("Show Playground"));
 605         showMapPushButton->setShortcut(tr("p"));
 606         disconnect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
 607         connect(showMapPushButton, SIGNAL(clicked()),
 608                                 this, SLOT(showPlayground()));
 609 }
 610
 611 void RobomonAtlantis::showPlayground()
 612 {
 613         mapTimer->stop();
 614         disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 615
 616         /* FIXME: */
 617
 618         showMapPushButton->setText(tr("Show &map"));
 619         showMapPushButton->setShortcut(tr("m"));
 620         disconnect(showMapPushButton, SIGNAL(clicked()),
 621                                 this, SLOT(showPlayground()));
 622         connect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
 623 }
 624
 625 void RobomonAtlantis::paintMap()
 626 {
 627         using namespace Qt;
 628         static QGraphicsRectItem *rects[MAP_WIDTH][MAP_HEIGHT];
 629         static bool firstMap = true;
 630         int x, y;
 631         struct map *map = ShmapIsMapInit();
 632
 633         if (!map) return;
 634
 635         for(int i=0; i < MAP_WIDTH; i++) {
 636                 for(int j=0; j<MAP_HEIGHT; j++) {
 637                         x = TOP_LEFT_X+i*cellSize;
 638                         y = TOP_LEFT_Y+j*cellSize;
 639
 640                         if(firstMap) {
 641                                 rects[i][j] = playgroundScene->addRect(
 642                                                 QRectF(x,y,cellSize,cellSize),
 643                                                 QPen(QBrush(Qt::black),
 644                                                 1,
 645                                                 Qt::SolidLine,
 646                                                 Qt::FlatCap,
 647                                                 Qt::BevelJoin),
 648                                                 QBrush(Qt::lightGray));
 649                                 rects[i][j]->setZValue(4);
 650                         }
 651
 652                         QColor color;
 653
 654                         struct map_cell *cell = &map->cells[j][i];
 655                         color = lightGray;
 656                         if (cell->flags & MAP_FLAG_WALL)
 657                                 color = darkYellow;
 658                         if (cell->flags & MAP_FLAG_IGNORE_OBST)
 659                                 color = darkGreen;
 660                         if (cell->flags & MAP_FLAG_SIMULATED_WALL)
 661                                 color = yellow;
 662                         if (cell->flags & MAP_FLAG_PATH)
 663                                 color = darkRed;
 664                         if (cell->flags & MAP_FLAG_START)
 665                                 color = red;
 666                         if (cell->flags & MAP_FLAG_GOAL)
 667                                 color = green;
 668                         if (cell->detected_obstacle) {
 669                                 QColor c1(color), c2(blue);
 670                                 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
 671                                 QColor c(c1.red()   + (int)(f*(c2.red()   - c1.red())),
 672                                          c1.green() + (int)(f*(c2.green() - c1.green())),
 673                                          c1.blue()  + (int)(f*(c2.blue()  - c1.blue())));
 674                                 color = c;
 675                         }
 676                         if (cell->flags & MAP_FLAG_DET_OBST)
 677                                 color = cyan;
 678
 679                         rects[i][j]->setBrush(QBrush(color));
 680                 }
 681         }
 682         firstMap = false;
 683 }
 684
 685 void RobomonAtlantis::setSimulation(int state)
 686 {
 687         if(state) {
 688                 robottype_publisher_hokuyo_scan_create(&orte,
 689                                                        dummy_publisher_callback, this);
 690         } else {
 691                 if (!simulationEnabled)
 692                         return;
 693                 robottype_publisher_hokuyo_scan_destroy(&orte);
 694         }
 695         simulationEnabled = state;
 696 }
 697
 698 /*!
 699         \fn RobomonAtlantis::setObstacleSimulation(int state)
 700  */
 701 void RobomonAtlantis::setObstacleSimulation(int state)
 702 {
 703         if (state) {
 704                 /* TODO Maybe it is possible to attach only once to Shmap */
 705                 ShmapInit(0);
 706                 obstacleSimulationTimer = new QTimer(this);
 707                 connect(obstacleSimulationTimer, SIGNAL(timeout()),
 708                         this, SLOT(simulateObstaclesHokuyo()));
 709                 obstacleSimulationTimer->start(100);
 710                 setMouseTracking(true);
 711         } else {
 712                 if (obstacleSimulationTimer)
 713                         delete obstacleSimulationTimer;
 714                 double distance = 0.8;
 715         }
 716 }
 717
 718
 719 void RobomonAtlantis::simulateObstaclesHokuyo()
 720 {
 721         double distance, wall_distance;
 722         int i;
 723         uint16_t *hokuyo[34] = { // FIXME should be HOKUYO_CLUSTER_CNT
 724                 &orte.hokuyo_scan.data1,
 725                 &orte.hokuyo_scan.data2,
 726                 &orte.hokuyo_scan.data3,
 727                 &orte.hokuyo_scan.data4,
 728                 &orte.hokuyo_scan.data5,
 729                 &orte.hokuyo_scan.data6,
 730                 &orte.hokuyo_scan.data7,
 731                 &orte.hokuyo_scan.data8,
 732                 &orte.hokuyo_scan.data9,
 733                 &orte.hokuyo_scan.data10,
 734                 &orte.hokuyo_scan.data11,
 735                 &orte.hokuyo_scan.data12,
 736                 &orte.hokuyo_scan.data13,
 737                 &orte.hokuyo_scan.data14,
 738                 &orte.hokuyo_scan.data15,
 739                 &orte.hokuyo_scan.data16,
 740                 &orte.hokuyo_scan.data17,
 741                 &orte.hokuyo_scan.data18,
 742                 &orte.hokuyo_scan.data19,
 743                 &orte.hokuyo_scan.data20,
 744                 &orte.hokuyo_scan.data21,
 745                 &orte.hokuyo_scan.data22,
 746                 &orte.hokuyo_scan.data23,
 747                 &orte.hokuyo_scan.data24,
 748                 &orte.hokuyo_scan.data25,
 749                 &orte.hokuyo_scan.data26,
 750                 &orte.hokuyo_scan.data27,
 751                 &orte.hokuyo_scan.data28,
 752                 &orte.hokuyo_scan.data29,
 753                 &orte.hokuyo_scan.data30,
 754                 &orte.hokuyo_scan.data31,
 755                 &orte.hokuyo_scan.data32,
 756                 &orte.hokuyo_scan.data33,
 757                 &orte.hokuyo_scan.data34,
 758         };
 759
 760
 761         for (i=0; i<HOKUYO_CLUSTER_CNT; i++) {
 762                 wall_distance = distanceToWallHokuyo(i);
 763                 distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
 764                 if (wall_distance < distance)
 765                         distance = wall_distance;
 766                 *hokuyo[i] = (uint16_t)(distance*1000);
 767         }
 768         ORTEPublicationSend(orte.publication_hokuyo_scan);
 769
 770 }
 771
 772 void RobomonAtlantis::changeObstacle(QPointF position)
 773 {
 774         if (!simulationEnabled) {
 775                 simulationEnabled = 1;
 776                 obstacleSimulationCheckBox->setChecked(true);
 777         }
 778
 779         simulatedObstacle.x = position.x();
 780         simulatedObstacle.y = position.y();
 781         simulateObstaclesHokuyo();
 782 }
 783
 784 /**********************************************************************
 785  * EVENTS
 786  **********************************************************************/
 787 bool RobomonAtlantis::event(QEvent *event)
 788 {
 789         switch (event->type()) {
 790                 case QEVENT(QEV_MOTION_STATUS):
 791                         emit motionStatusReceivedSignal();
 792                         break;
 793                 case QEVENT(QEV_ACTUAL_POSITION):
 794                         emit actualPositionReceivedSignal();
 795                         break;
 796                 case QEVENT(QEV_ESTIMATED_POSITION):
 797                         emit estimatedPositionReceivedSignal();
 798                         break;
 799                 case QEVENT(QEV_DI):
 800                         emit diReceivedSignal();
 801                         break;
 802                 case QEVENT(QEV_ACCELEROMETER):
 803                         emit accelerometerReceivedSignal();
 804                         break;
 805                 case QEVENT(QEV_ACCUMULATOR):
 806                         emit accumulatorReceivedSignal();
 807                         break;
 808                 case QEVENT(QEV_POWER_VOLTAGE):
 809                         emit powerVoltageReceivedSignal();
 810                         break;
 811                 default:
 812                         if (event->type() == QEvent::Close)
 813                                 closeEvent((QCloseEvent *)event);
 814                         else if (event->type() == QEvent::KeyPress)
 815                                 keyPressEvent((QKeyEvent *)event);
 816                         else if (event->type() == QEvent::KeyRelease)
 817                                 keyReleaseEvent((QKeyEvent *)event);
 818                         else if (event->type() == QEvent::FocusIn)
 819                                 grabKeyboard();
 820                         else if (event->type() == QEvent::FocusOut)
 821                                 releaseKeyboard();
 822                         else {
 823                                 event->ignore();
 824                                 return false;
 825                         }
 826                         break;
 827         }
 828         event->accept();
 829         return true;
 830 }
 831
 832 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
 833 {
 834         double peak, gain;
 835
 836         if (event->isAutoRepeat()) {
 837                 switch (event->key()) {
 838                         case Qt::Key_Down:
 839                                 peak = leftMotorSlider->minimum()/2;
 840                                 if (leftMotorValue < peak ||
 841                                         rightMotorValue < peak)
 842                                         gain = 1.01;
 843                                 else
 844                                         gain = 1.3;
 845                                 leftMotorValue *= gain;
 846                                 rightMotorValue *= gain;
 847                                 leftMotorSlider->setValue((int)leftMotorValue);
 848                                 rightMotorSlider->setValue((int)rightMotorValue);
 849                                 break;
 850
 851                         case Qt::Key_Up:
 852                         case Qt::Key_Left:
 853                         case Qt::Key_Right:
 854                                 peak = leftMotorSlider->maximum()/2;
 855                                 if (leftMotorValue > peak ||
 856                                         rightMotorValue > peak)
 857                                         gain = 1.01;
 858                                 else
 859                                         gain = 1.3;
 860                                 leftMotorValue *= gain;
 861                                 rightMotorValue *= gain;
 862                                 leftMotorSlider->setValue((int)leftMotorValue);
 863                                 rightMotorSlider->setValue((int)rightMotorValue);
 864                                 break;
 865
 866                         default:
 867                                 event->ignore();
 868                                 break;
 869                 }
 870                 return;
 871         }
 872
 873         switch (event->key()) {
 874                 case Qt::Key_Up:
 875                         leftMotorValue = 1;
 876                         rightMotorValue = 1;
 877                         bothMotorsCheckBox->setChecked(true);
 878                         leftMotorSlider->setValue((int)leftMotorValue);
 879                         setLeftMotor((int)leftMotorValue);
 880                         break;
 881                 case Qt::Key_Down:
 882                         leftMotorValue = -1;
 883                         rightMotorValue = -1;
 884                         bothMotorsCheckBox->setChecked(true);
 885                         leftMotorSlider->setValue((int)leftMotorValue);
 886                         setLeftMotor((int)leftMotorValue);
 887                         break;
 888                 case Qt::Key_Left:
 889                         leftMotorValue = -1;
 890                         rightMotorValue = 1;
 891                         leftMotorSlider->setValue((int)leftMotorValue);
 892                         rightMotorSlider->setValue((int)rightMotorValue);
 893                         setLeftMotor((int)leftMotorValue);
 894                         setRightMotor((int)leftMotorValue);
 895                         break;
 896                 case Qt::Key_Right:
 897                         leftMotorValue = 1;
 898                         rightMotorValue = -1;
 899                         leftMotorSlider->setValue((int)leftMotorValue);
 900                         rightMotorSlider->setValue((int)rightMotorValue);
 901                         setLeftMotor((int)leftMotorValue);
 902                         setRightMotor((int)rightMotorValue);
 903                         break;
 904                 default:
 905                         event->ignore();
 906                         break;
 907         }
 908         event->accept();
 909 }
 910
 911 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
 912 {
 913         if (event->isAutoRepeat()) {
 914                 event->ignore();
 915                 return;
 916         }
 917
 918         switch (event->key()) {
 919                 case Qt::Key_Up:
 920                 case Qt::Key_Down:
 921                 case Qt::Key_Left:
 922                 case Qt::Key_Right:
 923                         leftMotorValue = 0;
 924                         rightMotorValue = 0;
 925                         bothMotorsCheckBox->setChecked(false);
 926                         leftMotorSlider->setValue((int)leftMotorValue);
 927                         rightMotorSlider->setValue((int)rightMotorValue);
 928                         break;
 929                 default:
 930                         event->ignore();
 931                         break;
 932         }
 933         event->accept();
 934 }
 935
 936 void RobomonAtlantis::closeEvent(QCloseEvent *event)
 937 {
 938         robottype_roboorte_destroy(&orte);
 939 }
 940
 941 /**********************************************************************
 942  * ORTE
 943  **********************************************************************/
 944 void RobomonAtlantis::createOrte()
 945 {
 946         int rv;
 947
 948         orte.strength = 11;
 949
 950         rv = robottype_roboorte_init(&orte);
 951         if (rv) {
 952                 printf("RobomonAtlantis: Unable to initialize ORTE\n");
 953         }
 954
 955         /* publishers */
 956         robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
 957
 958         robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
 959
 960         robottype_publisher_servos_create(&orte, dummy_publisher_callback, NULL);
 961         robottype_publisher_drives_create(&orte, dummy_publisher_callback, NULL);
 962         robottype_publisher_laser_cmd_create(&orte, NULL, NULL);
 963
 964         /* subscribers */
 965         robottype_subscriber_pwr_voltage_create(&orte,
 966                                 receivePowerVoltageCallBack, this);
 967         robottype_subscriber_motion_status_create(&orte,
 968                                 receiveMotionStatusCallBack, this);
 969         robottype_subscriber_ref_pos_create(&orte,
 970                                 receiveActualPositionCallBack, this);
 971         robottype_subscriber_est_pos_create(&orte,
 972                                 receiveEstimatedPositionCallBack, this);
 973
 974         /*createDISubscriber(this, &orteData);*/
 975         /*createAccelerometerSubscriber(this, &orteData);*/
 976         /*createAccumulatorSubscriber(this, &orteData);*/
 977         /* motors */
 978         orte.motion_speed.left = 0;
 979         orte.motion_speed.right = 0;
 980         /* power management */
 981         orte.pwr_ctrl.voltage33 = true;
 982         orte.pwr_ctrl.voltage50 = true;
 983         orte.pwr_ctrl.voltage80 = true;
 984         voltage33CheckBox->setChecked(true);
 985         voltage50CheckBox->setChecked(true);
 986         voltage80CheckBox->setChecked(true);
 987
 988         /* servos */
 989         leftBrushCheckBox->setChecked(false);
 990         rightBrushCheckBox->setChecked(false);
 991
 992         setDrives(0);
 993         setLaser(0);
 994
 995         /* set actions to do when we receive data from orte */
 996         connect(this, SIGNAL(motionStatusReceivedSignal()),
 997                         this, SLOT(motionStatusReceived()));
 998         connect(this, SIGNAL(actualPositionReceivedSignal()),
 999                         this, SLOT(actualPositionReceived()));
1000         connect(this, SIGNAL(estimatedPositionReceivedSignal()),
1001                         this, SLOT(estimatedPositionReceived()));
1002         connect(this, SIGNAL(diReceivedSignal()),
1003                         this, SLOT(diReceived()));
1004         connect(this, SIGNAL(accelerometerReceivedSignal()),
1005                         this, SLOT(accelerometerReceived()));
1006         connect(this, SIGNAL(accumulatorReceivedSignal()),
1007                         this, SLOT(accumulatorReceived()));
1008         connect(this, SIGNAL(powerVoltageReceivedSignal()),
1009                         this, SLOT(powerVoltageReceived()));
1010 }
1011
1012 void RobomonAtlantis::motionStatusReceived()
1013 {
1014         WDBG("ORTE received: motion status");
1015 }
1016
1017 void RobomonAtlantis::actualPositionReceived()
1018 {
1019         actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
1020         actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
1021         actPosPhi->setText(QString("%1(%2)")
1022                         .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
1023                         .arg(orte.ref_pos.phi, 0, 'f', 1));
1024         robotActPos->moveRobot(orte.ref_pos.x,
1025                 orte.ref_pos.y, orte.ref_pos.phi);
1026 }
1027
1028 void RobomonAtlantis::estimatedPositionReceived()
1029 {
1030         estPosX->setText(QString("%1").arg(orte.est_pos.x, 0, 'f', 3));
1031         estPosY->setText(QString("%1").arg(orte.est_pos.y, 0, 'f', 3));
1032         estPosPhi->setText(QString("%1(%2)")
1033                         .arg(DEGREES(orte.est_pos.phi), 0, 'f', 0)
1034                         .arg(orte.est_pos.phi, 0, 'f', 1));
1035         robotEstPos->moveRobot(orte.est_pos.x,
1036                 orte.est_pos.y, orte.est_pos.phi);
1037 }
1038
1039 void RobomonAtlantis::diReceived()
1040 {
1041         WDBG("ORTE received: DI");
1042 }
1043
1044 void RobomonAtlantis::accelerometerReceived()
1045 {
1046         WDBG("ORTE received: accelerometer");
1047 }
1048
1049 void RobomonAtlantis::accumulatorReceived()
1050 {
1051         WDBG("ORTE received: accumulator");
1052 }
1053
1054 void RobomonAtlantis::powerVoltageReceived()
1055 {
1056         voltage33LineEdit->setText(QString("%1").arg(
1057                         orte.pwr_voltage.voltage33, 0, 'f', 3));
1058         voltage50LineEdit->setText(QString("%1").arg(
1059                         orte.pwr_voltage.voltage50, 0, 'f', 3));
1060         voltage80LineEdit->setText(QString("%1").arg(
1061                         orte.pwr_voltage.voltage80, 0, 'f', 3));
1062         voltageBATLineEdit->setText(QString("%1").arg(
1063                         orte.pwr_voltage.voltageBAT, 0, 'f', 3));
1064
1065 }
1066
1067 /**********************************************************************
1068  * MISCELLANEOUS
1069  **********************************************************************/
1070 void RobomonAtlantis::openSharedMemory()
1071 {
1072         int segmentId;
1073         int sharedSegmentSize;
1074
1075         if (sharedMemoryOpened)
1076                 return;
1077
1078         cellSize = PG_X / MAP_WIDTH;
1079
1080         sharedSegmentSize = sizeof(char) * MAP_WIDTH * MAP_HEIGHT;
1081
1082         /* Get segment identificator in a read only mode  */
1083         segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
1084         if(segmentId == -1) {
1085                 QMessageBox::critical(this, "robomon",
1086                                 "Unable to open shared memory segment!");
1087                 return;
1088         }
1089
1090         /* Init Shmap */
1091         ShmapInit(0);
1092
1093         /* Attach the shared memory segment */
1094         //map =  (_Map*)shmat (segmentId, (void*) 0, 0);
1095
1096         sharedMemoryOpened = true;
1097 }
1098
1099 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
1100 {
1101         double distance=4.0, min_distance=4.0;
1102         int i,j;
1103         Point wall;
1104         struct map *map = ShmapIsMapInit();
1105
1106         if (!map) return min_distance;
1107
1108         // Simulate obstacles
1109         for(j=0;j<MAP_HEIGHT;j++) {
1110                 for (i=0;i<MAP_WIDTH;i++) {
1111                         struct map_cell *cell = &map->cells[j][i];
1112                         if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
1113                                 // WALL
1114                                 ShmapCell2Point(i, j, &wall.x, &wall.y);
1115
1116                                 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
1117                                 if (distance<min_distance) min_distance = distance;
1118                         }
1119                 }
1120         }
1121
1122         return min_distance;
1123 }
1124
1125 /**
1126  * Calculation for Hokuyo simulation. Calculates distance that would
1127  * be returned by Hokuyo sensors, if there is only one obstacle (as
1128  * specified by parameters).
1129  *
1130  * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
1131  * @param obstacle Position of the obstacle (x, y in meters).
1132  * @param obstacleSize Size (diameter) of the obstacle in meters.
1133  *
1134  * @return Distance measured by sensors in meters.
1135  */
1136 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
1137 {
1138         struct est_pos_type e = orte.est_pos;
1139         double sensor_a;
1140         struct sharp_pos s;
1141
1142         s.x = HOKUYO_CENTER_OFFSET_M;
1143         s.y = 0.0;
1144         s.ang = HOKUYO_CLUSTER_TO_RAD(beamnum);
1145
1146         Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
1147                      e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
1148         sensor_a = e.phi + s.ang;
1149
1150         const double sensorRange = 4.0; /*[meters]*/
1151
1152         double distance, angle;
1153
1154         angle = sensor.angleTo(obstacle) - sensor_a;
1155         angle = fmod(angle, 2.0*M_PI);
1156         if (angle > +M_PI) angle -= 2.0*M_PI;
1157         if (angle < -M_PI) angle += 2.0*M_PI;
1158         angle = fabs(angle);
1159         distance = sensor.distanceTo(obstacle)-0.11;
1160         if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
1161                 // We can see the obstackle from here.
1162                 if (angle < M_PI/2.0) {
1163                     distance = distance/cos(angle);
1164                 }
1165                 if (distance > sensorRange)
1166                         distance = sensorRange;
1167         } else {
1168                 distance = sensorRange;
1169         }
1170
1171         return distance;
1172 }