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