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