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