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