src/robomon/RobomonAtlantis.cpp

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