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