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