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