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 #include "trail.h"
  41
  42 #include <QCoreApplication>
  43 #include <QEvent>
  44 #include <QKeyEvent>
  45 #include <QDebug>
  46 #include <QMessageBox>
  47
  48 RobomonAtlantis::RobomonAtlantis(QWidget *parent)
  49         : QWidget(parent)
  50 {
  51         QFont font;
  52         font.setPointSize(7);
  53         setFont(font);
  54
  55         debugWindowEnabled = false;
  56
  57         createLeftLayout();
  58         createRightLayout();
  59
  60         QHBoxLayout *mainLayout = new QHBoxLayout;
  61         mainLayout->addLayout(leftLayout);
  62         mainLayout->addLayout(rightLayout);
  63         setLayout(mainLayout);
  64
  65         createOrte();
  66         createRobots();
  67         createActions();
  68
  69         setFocusPolicy(Qt::StrongFocus);
  70         sharedMemoryOpened = false;
  71         WDBG("Youuuhouuuu!!");
  72 }
  73
  74 /**********************************************************************
  75  * GUI
  76  **********************************************************************/
  77 void RobomonAtlantis::createLeftLayout()
  78 {
  79         leftLayout = new QVBoxLayout();
  80
  81         createDebugGroupBox();
  82         debugWindowEnabled = true;
  83         createPlaygroundGroupBox();
  84         leftLayout->addWidget(playgroundGroupBox);
  85         //leftLayout->addWidget(debugGroupBox); // FIXME: move this to separate tab
  86 }
  87
  88 void RobomonAtlantis::createRightLayout()
  89 {
  90         rightLayout = new QVBoxLayout();
  91         QGridLayout *layout = new QGridLayout();
  92         QVBoxLayout *vlayout = new QVBoxLayout();
  93         //vlayout->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding);
  94
  95         createPositionGroupBox();
  96         createMiscGroupBox();
  97         createFSMGroupBox();
  98         createActuatorsGroupBox();
  99         createPowerGroupBox();
 100
 101         vlayout->addWidget(positionGroupBox);
 102         vlayout->addWidget(miscGroupBox);
 103         vlayout->addWidget(fsmGroupBox);
 104         vlayout->addWidget(powerGroupBox);
 105         layout->addLayout(vlayout, 0, 0);
 106         layout->addWidget(actuatorsGroupBox, 0, 1);
 107
 108         rightLayout->addLayout(layout);
 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         playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 123         layout->addWidget(playgroundSceneView);
 124
 125         playgroundGroupBox->setLayout(layout);
 126 }
 127
 128 void RobomonAtlantis::createPositionGroupBox()
 129 {
 130         positionGroupBox = new QGroupBox(tr("Position state"));
 131         positionGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 132         QGridLayout *layout = new QGridLayout();
 133
 134         actPosX = new QLineEdit();
 135         actPosY = new QLineEdit();
 136         actPosPhi = new QLineEdit();
 137
 138         estPosX = new QLineEdit();
 139         estPosY = new QLineEdit();
 140         estPosPhi = new QLineEdit();
 141
 142         actPosX->setReadOnly(true);
 143         actPosY->setReadOnly(true);
 144         actPosPhi->setReadOnly(true);
 145
 146         estPosX->setReadOnly(true);
 147         estPosY->setReadOnly(true);
 148         estPosPhi->setReadOnly(true);
 149
 150         layout->addWidget(MiscGui::createLabel("X"), 1, 0);
 151         layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
 152         layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
 153
 154         layout->addWidget(MiscGui::createLabel("X"), 5, 0);
 155         layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
 156         layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
 157
 158         layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
 159         layout->addWidget(actPosX, 1, 1);
 160         layout->addWidget(actPosY, 2, 1);
 161         layout->addWidget(actPosPhi, 3, 1);
 162
 163         layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 4, 1);
 164         layout->addWidget(estPosX, 5, 1);
 165         layout->addWidget(estPosY, 6, 1);
 166         layout->addWidget(estPosPhi, 7, 1);
 167
 168         positionGroupBox->setLayout(layout);
 169 }
 170
 171 void RobomonAtlantis::createMiscGroupBox()
 172 {
 173         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 174         miscGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
 175         QGridLayout *layout = new QGridLayout();
 176
 177         obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
 178         obstacleSimulationCheckBox->setShortcut(tr("o"));
 179         layout->addWidget(obstacleSimulationCheckBox);
 180
 181         startPlug = new QCheckBox("Start plug");
 182         layout->addWidget(startPlug);
 183
 184         puckInside = new QCheckBox("Puck inside");
 185         layout->addWidget(puckInside);
 186
 187         miscGroupBox->setLayout(layout);
 188 }
 189
 190 void RobomonAtlantis::createFSMGroupBox()
 191 {
 192         fsmGroupBox = new QGroupBox(tr("FSM"));
 193         fsmGroupBox->setSizePolicy(QSizePolicy::Expanding, 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         fsm_main_state->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
 200         layout->addWidget(fsm_main_state, 1, 1);
 201
 202         layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
 203         fsm_act_state = new QLabel();
 204         layout->addWidget(fsm_act_state, 2, 1);
 205
 206         layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
 207         fsm_motion_state = new QLabel();
 208         layout->addWidget(fsm_motion_state, 3, 1);
 209
 210         fsmGroupBox->setLayout(layout);
 211 }
 212
 213 void RobomonAtlantis::createDebugGroupBox()
 214 {
 215         debugGroupBox = new QGroupBox(tr("Debug window"));
 216         debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
 217         QHBoxLayout *layout = new QHBoxLayout();
 218
 219         debugWindow = new QTextEdit();
 220         debugWindow->setReadOnly(true);
 221
 222         layout->addWidget(debugWindow);
 223         debugGroupBox->setLayout(layout);
 224 }
 225
 226 void RobomonAtlantis::createActuatorsGroupBox()
 227 {
 228         actuatorsGroupBox = new QGroupBox(tr("Actuators"));
 229         actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
 230         QHBoxLayout *layout = new QHBoxLayout();
 231
 232         createMotorsGroupBox();
 233
 234         layout->setAlignment(Qt::AlignLeft);
 235         layout->addWidget(enginesGroupBox);
 236         actuatorsGroupBox->setLayout(layout);
 237 }
 238
 239 void RobomonAtlantis::createPowerGroupBox()
 240 {
 241         powerGroupBox = new QGroupBox(tr("Power management"));
 242         powerGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
 243         QGridLayout *layout = new QGridLayout();
 244
 245         voltage33CheckBox = new QCheckBox(tr("&3.3V"));
 246         voltage50CheckBox = new QCheckBox(tr("&5.0V"));
 247         voltage80CheckBox = new QCheckBox(tr("&8.0V"));
 248
 249         voltage33CheckBox->setShortcut(tr("3"));
 250         voltage50CheckBox->setShortcut(tr("5"));
 251         voltage80CheckBox->setShortcut(tr("8"));
 252
 253         layout->addWidget(voltage33CheckBox, 0, 0);
 254         layout->addWidget(voltage50CheckBox, 1, 0);
 255         layout->addWidget(voltage80CheckBox, 2, 0);
 256         layout->addWidget(MiscGui::createLabel("BAT"), 3, 0);
 257
 258         voltage33LineEdit = new QLineEdit();
 259         voltage50LineEdit = new QLineEdit();
 260         voltage80LineEdit = new QLineEdit();
 261         voltageBATLineEdit = new QLineEdit();
 262
 263         voltage33LineEdit->setReadOnly(true);
 264         voltage50LineEdit->setReadOnly(true);
 265         voltage80LineEdit->setReadOnly(true);
 266         voltageBATLineEdit->setReadOnly(true);
 267
 268         layout->addWidget(voltage33LineEdit, 0, 1);
 269         layout->addWidget(voltage50LineEdit, 1, 1);
 270         layout->addWidget(voltage80LineEdit, 2, 1);
 271         layout->addWidget(voltageBATLineEdit, 3, 1);
 272
 273         powerGroupBox->setLayout(layout);
 274 }
 275
 276 void RobomonAtlantis::createMotorsGroupBox()
 277 {
 278         enginesGroupBox = new QGroupBox(tr("Motors"));
 279         QVBoxLayout *layout = new QVBoxLayout();
 280         QHBoxLayout *layout1 = new QHBoxLayout();
 281         QHBoxLayout *layout2 = new QHBoxLayout();
 282
 283         leftMotorSlider = new QSlider(Qt::Vertical);
 284         rightMotorSlider = new QSlider(Qt::Vertical);
 285         bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
 286         stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
 287
 288         leftMotorSlider->setMinimum(-100);
 289         leftMotorSlider->setMaximum(100);
 290         leftMotorSlider->setTracking(false);
 291         leftMotorSlider->setTickPosition(QSlider::TicksLeft);
 292
 293         rightMotorSlider->setMinimum(-100);
 294         rightMotorSlider->setMaximum(100);
 295         rightMotorSlider->setTracking(false);
 296         rightMotorSlider->setTickPosition(QSlider::TicksRight);
 297
 298         stopMotorsPushButton->setMaximumWidth(90);
 299
 300         layout1->addWidget(leftMotorSlider);
 301         layout1->addWidget(MiscGui::createLabel("0"));
 302         layout1->addWidget(rightMotorSlider);
 303
 304         layout2->addWidget(bothMotorsCheckBox);
 305
 306         layout->addWidget(MiscGui::createLabel("100"));
 307         layout->addLayout(layout1);
 308         layout->addWidget(MiscGui::createLabel("-100"));
 309         layout->addLayout(layout2);
 310         layout->addWidget(stopMotorsPushButton);
 311         enginesGroupBox->setLayout(layout);
 312 }
 313
 314 void RobomonAtlantis::createRobots()
 315 {
 316         robotActPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
 317         robotActPos->setZValue(11);
 318         trailRefPos = new Trail(QPen(Qt::darkBlue));
 319         trailRefPos->setZValue(11);
 320
 321         robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed));
 322         robotEstPosOdo->setZValue(10);
 323         trailOdoPos = new Trail(QPen(Qt::white));
 324         trailOdoPos->setZValue(10);
 325         robotEstPosIndepOdo = new Robot("Odo", QPen(), QBrush(Qt::darkGray));
 326         robotEstPosIndepOdo->setZValue(10);
 327         trailPosIndepOdo = new Trail(QPen());
 328         trailPosIndepOdo->setZValue(10);
 329
 330         playgroundScene->addItem(robotActPos);
 331         playgroundScene->addItem(robotEstPosIndepOdo);
 332         playgroundScene->addItem(robotEstPosOdo);
 333
 334         showTrails(false);
 335
 336         playgroundScene->addItem(trailRefPos);
 337         playgroundScene->addItem(trailPosIndepOdo);
 338         playgroundScene->addItem(trailOdoPos);
 339
 340         hokuyoScan = new HokuyoScan();
 341         hokuyoScan->setZValue(10);
 342         playgroundScene->addItem(hokuyoScan);
 343
 344 }
 345
 346 /**********************************************************************
 347  * GUI actions
 348  **********************************************************************/
 349 void RobomonAtlantis::createActions()
 350 {
 351         /* power management */
 352         connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
 353                         this, SLOT(setVoltage33(int)));
 354         connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
 355                         this, SLOT(setVoltage50(int)));
 356         connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
 357                         this, SLOT(setVoltage80(int)));
 358
 359         /* motors */
 360         connect(leftMotorSlider, SIGNAL(valueChanged(int)),
 361                         this, SLOT(setLeftMotor(int)));
 362         connect(rightMotorSlider, SIGNAL(valueChanged(int)),
 363                         this, SLOT(setRightMotor(int)));
 364         connect(stopMotorsPushButton, SIGNAL(clicked()),
 365                         this, SLOT(stopMotors()));
 366
 367         connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
 368
 369         /* obstacle simulation */
 370         simulationEnabled = 0;
 371         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 372                         this, SLOT(setSimulation(int)));
 373         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 374                         this, SLOT(setObstacleSimulation(int)));
 375         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 376                         playgroundScene, SLOT(showObstacle(int)));
 377         connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
 378                         this, SLOT(changeObstacle(QPointF)));
 379 }
 380
 381 void RobomonAtlantis::setVoltage33(int state)
 382 {
 383         if (state)
 384                 orte.pwr_ctrl.voltage33 = true;
 385         else
 386                 orte.pwr_ctrl.voltage33 = false;
 387 }
 388
 389 void RobomonAtlantis::setVoltage50(int state)
 390 {
 391         if (state)
 392                 orte.pwr_ctrl.voltage50 = true;
 393         else
 394                 orte.pwr_ctrl.voltage50 = false;
 395 }
 396
 397 void RobomonAtlantis::setVoltage80(int state)
 398 {
 399         if (state)
 400                 orte.pwr_ctrl.voltage80 = true;
 401         else
 402                 orte.pwr_ctrl.voltage80 = false;
 403 }
 404
 405 void RobomonAtlantis::setLeftMotor(int value)
 406 {
 407         short int leftMotor;
 408         short int rightMotor;
 409
 410         if(bothMotorsCheckBox->isChecked())
 411                 rightMotorSlider->setValue(value);
 412
 413         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 414         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 415
 416         orte.motion_speed.left = leftMotor;
 417         orte.motion_speed.right = rightMotor;
 418
 419 }
 420
 421 void RobomonAtlantis::setRightMotor(int value)
 422 {
 423         short int leftMotor;
 424         short int rightMotor;
 425
 426         if(bothMotorsCheckBox->isChecked())
 427                 leftMotorSlider->setValue(value);
 428
 429         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 430         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 431
 432         orte.motion_speed.left = leftMotor;
 433         orte.motion_speed.right = rightMotor;
 434
 435 }
 436
 437 void RobomonAtlantis::stopMotors()
 438 {
 439         leftMotorSlider->setValue(0);
 440         rightMotorSlider->setValue(0);
 441 }
 442
 443 void RobomonAtlantis::showMap(bool show)
 444 {
 445         openSharedMemory();
 446
 447         if (sharedMemoryOpened == false)
 448                 return;
 449
 450         if (show) {
 451                 mapTimer = new QTimer(this);
 452                 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 453                 mapTimer->start(200);
 454         } else {
 455                 mapTimer->stop();
 456                 disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 457         }
 458         playgroundScene->showMap(show);
 459 }
 460
 461 void RobomonAtlantis::paintMap()
 462 {
 463         using namespace Qt;
 464         struct map *map = ShmapIsMapInit();
 465
 466         if (!map) return;
 467
 468         for(int i=0; i < MAP_WIDTH; i++) {
 469                 for(int j=0; j<MAP_HEIGHT; j++) {
 470                         QColor color;
 471
 472                         struct map_cell *cell = &map->cells[j][i];
 473                         color = lightGray;
 474
 475                         if ((cell->flags & MAP_FLAG_WALL) &&
 476                             (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
 477                                 color = darkYellow;
 478                         if (cell->flags & MAP_FLAG_IGNORE_OBST)
 479                                 color = darkGreen;
 480                         if (cell->flags & MAP_FLAG_SIMULATED_WALL)
 481                                 color = yellow;
 482                         if (cell->flags & MAP_FLAG_PATH)
 483                                 color = darkRed;
 484                         if (cell->flags & MAP_FLAG_START)
 485                                 color = red;
 486                         if (cell->flags & MAP_FLAG_GOAL)
 487                                 color = green;
 488                         if (cell->detected_obstacle) {
 489                                 QColor c1(color), c2(blue);
 490                                 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
 491                                 QColor c(c1.red()   + (int)(f*(c2.red()   - c1.red())),
 492                                          c1.green() + (int)(f*(c2.green() - c1.green())),
 493                                          c1.blue()  + (int)(f*(c2.blue()  - c1.blue())));
 494                                 color = c;
 495                         }
 496                         if (cell->flags & MAP_FLAG_DET_OBST)
 497                                 color = cyan;
 498
 499                         playgroundScene->setMapColor(i, j, color);
 500                 }
 501         }
 502 }
 503
 504 void RobomonAtlantis::setSimulation(int state)
 505 {
 506         if(state) {
 507                 robottype_publisher_hokuyo_scan_create(&orte,
 508                                                        dummy_publisher_callback, this);
 509         } else {
 510                 if (!simulationEnabled)
 511                         return;
 512                 robottype_publisher_hokuyo_scan_destroy(&orte);
 513         }
 514         simulationEnabled = state;
 515 }
 516
 517 /*!
 518         \fn RobomonAtlantis::setObstacleSimulation(int state)
 519  */
 520 void RobomonAtlantis::setObstacleSimulation(int state)
 521 {
 522         if (state) {
 523                 /* TODO Maybe it is possible to attach only once to Shmap */
 524                 ShmapInit(0);
 525                 obstacleSimulationTimer = new QTimer(this);
 526                 connect(obstacleSimulationTimer, SIGNAL(timeout()),
 527                         this, SLOT(simulateObstaclesHokuyo()));
 528                 obstacleSimulationTimer->start(100);
 529                 setMouseTracking(true);
 530         } else {
 531                 if (obstacleSimulationTimer)
 532                         delete obstacleSimulationTimer;
 533                 //double distance = 0.8;
 534         }
 535 }
 536
 537
 538 void RobomonAtlantis::simulateObstaclesHokuyo()
 539 {
 540         double distance, wall_distance;
 541         unsigned int i;
 542         uint16_t *hokuyo = orte.hokuyo_scan.data;
 543
 544         for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
 545                 wall_distance = distanceToWallHokuyo(i);
 546                 distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
 547                 if (wall_distance < distance)
 548                         distance = wall_distance;
 549                 hokuyo[i] = distance*1000;
 550         }
 551         ORTEPublicationSend(orte.publication_hokuyo_scan);
 552
 553 }
 554
 555 void RobomonAtlantis::changeObstacle(QPointF position)
 556 {
 557         if (!simulationEnabled) {
 558                 simulationEnabled = 1;
 559                 obstacleSimulationCheckBox->setChecked(true);
 560         }
 561
 562         simulatedObstacle.x = position.x();
 563         simulatedObstacle.y = position.y();
 564         simulateObstaclesHokuyo();
 565 }
 566
 567 /**********************************************************************
 568  * EVENTS
 569  **********************************************************************/
 570 bool RobomonAtlantis::event(QEvent *event)
 571 {
 572         switch (event->type()) {
 573                 case QEVENT(QEV_MOTION_STATUS):
 574                         emit motionStatusReceivedSignal();
 575                         break;
 576                 case QEVENT(QEV_HOKUYO_SCAN):
 577                         hokuyoScan->newScan(&orte.hokuyo_scan);
 578                         break;
 579                 case QEVENT(QEV_REFERENCE_POSITION):
 580                         emit actualPositionReceivedSignal();
 581                         break;
 582                 case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO):
 583                         estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3));
 584                         estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3));
 585                         estPosPhi->setText(QString("%1(%2)")
 586                                         .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0)
 587                                         .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1));
 588                         robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x,
 589                                 orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
 590                         trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x,
 591                                               orte.est_pos_indep_odo.y));
 592
 593 //                      hokuyoScan->setPosition(orte.est_pos_indep_odo.x,
 594 //                                              orte.est_pos_indep_odo.y,
 595 //                                              orte.est_pos_indep_odo.phi);
 596                         break;
 597                 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
 598                         robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
 599                                         orte.est_pos_odo.y, orte.est_pos_odo.phi);
 600                         trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
 601                                               orte.est_pos_odo.y));
 602                         break;
 603                 case QEVENT(QEV_POWER_VOLTAGE):
 604                         emit powerVoltageReceivedSignal();
 605                         break;
 606                 case QEVENT(QEV_FSM_MAIN):
 607                         fsm_main_state->setText(orte.fsm_main.state_name);
 608                         break;
 609                 case QEVENT(QEV_FSM_ACT):
 610                         fsm_act_state->setText(orte.fsm_act.state_name);
 611                         break;
 612                 case QEVENT(QEV_FSM_MOTION):
 613                         fsm_motion_state->setText(orte.fsm_motion.state_name);
 614                         break;
 615                 default:
 616                         if (event->type() == QEvent::Close)
 617                                 closeEvent((QCloseEvent *)event);
 618                         else if (event->type() == QEvent::KeyPress)
 619                                 keyPressEvent((QKeyEvent *)event);
 620                         else if (event->type() == QEvent::KeyRelease)
 621                                 keyReleaseEvent((QKeyEvent *)event);
 622                         else if (event->type() == QEvent::FocusIn)
 623                                 grabKeyboard();
 624                         else if (event->type() == QEvent::FocusOut)
 625                                 releaseKeyboard();
 626                         else {
 627                                 event->ignore();
 628                                 return false;
 629                         }
 630                         break;
 631         }
 632         event->accept();
 633         return true;
 634 }
 635
 636 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
 637 {
 638         double peak, gain;
 639
 640         if (event->isAutoRepeat()) {
 641                 switch (event->key()) {
 642                         case Qt::Key_Down:
 643                                 peak = leftMotorSlider->minimum()/2;
 644                                 if (leftMotorValue < peak ||
 645                                         rightMotorValue < peak)
 646                                         gain = 1.01;
 647                                 else
 648                                         gain = 1.3;
 649                                 leftMotorValue *= gain;
 650                                 rightMotorValue *= gain;
 651                                 leftMotorSlider->setValue((int)leftMotorValue);
 652                                 rightMotorSlider->setValue((int)rightMotorValue);
 653                                 break;
 654
 655                         case Qt::Key_Up:
 656                         case Qt::Key_Left:
 657                         case Qt::Key_Right:
 658                                 peak = leftMotorSlider->maximum()/2;
 659                                 if (leftMotorValue > peak ||
 660                                         rightMotorValue > peak)
 661                                         gain = 1.01;
 662                                 else
 663                                         gain = 1.3;
 664                                 leftMotorValue *= gain;
 665                                 rightMotorValue *= gain;
 666                                 leftMotorSlider->setValue((int)leftMotorValue);
 667                                 rightMotorSlider->setValue((int)rightMotorValue);
 668                                 break;
 669
 670                         default:
 671                                 event->ignore();
 672                                 break;
 673                 }
 674                 return;
 675         }
 676
 677         switch (event->key()) {
 678                 case Qt::Key_Up:
 679                         leftMotorValue = 1;
 680                         rightMotorValue = 1;
 681                         bothMotorsCheckBox->setChecked(true);
 682                         leftMotorSlider->setValue((int)leftMotorValue);
 683                         setLeftMotor((int)leftMotorValue);
 684                         break;
 685                 case Qt::Key_Down:
 686                         leftMotorValue = -1;
 687                         rightMotorValue = -1;
 688                         bothMotorsCheckBox->setChecked(true);
 689                         leftMotorSlider->setValue((int)leftMotorValue);
 690                         setLeftMotor((int)leftMotorValue);
 691                         break;
 692                 case Qt::Key_Left:
 693                         leftMotorValue = -1;
 694                         rightMotorValue = 1;
 695                         leftMotorSlider->setValue((int)leftMotorValue);
 696                         rightMotorSlider->setValue((int)rightMotorValue);
 697                         setLeftMotor((int)leftMotorValue);
 698                         setRightMotor((int)leftMotorValue);
 699                         break;
 700                 case Qt::Key_Right:
 701                         leftMotorValue = 1;
 702                         rightMotorValue = -1;
 703                         leftMotorSlider->setValue((int)leftMotorValue);
 704                         rightMotorSlider->setValue((int)rightMotorValue);
 705                         setLeftMotor((int)leftMotorValue);
 706                         setRightMotor((int)rightMotorValue);
 707                         break;
 708                 default:
 709                         event->ignore();
 710                         break;
 711         }
 712         event->accept();
 713 }
 714
 715 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
 716 {
 717         if (event->isAutoRepeat()) {
 718                 event->ignore();
 719                 return;
 720         }
 721
 722         switch (event->key()) {
 723                 case Qt::Key_Up:
 724                 case Qt::Key_Down:
 725                 case Qt::Key_Left:
 726                 case Qt::Key_Right:
 727                         leftMotorValue = 0;
 728                         rightMotorValue = 0;
 729                         bothMotorsCheckBox->setChecked(false);
 730                         leftMotorSlider->setValue((int)leftMotorValue);
 731                         rightMotorSlider->setValue((int)rightMotorValue);
 732                         break;
 733                 default:
 734                         event->ignore();
 735                         break;
 736         }
 737         event->accept();
 738 }
 739
 740 void RobomonAtlantis::closeEvent(QCloseEvent *)
 741 {
 742         robottype_roboorte_destroy(&orte);
 743 }
 744
 745 /**********************************************************************
 746  * ORTE
 747  **********************************************************************/
 748 void RobomonAtlantis::createOrte()
 749 {
 750         int rv;
 751
 752         orte.strength = 11;
 753
 754         rv = robottype_roboorte_init(&orte);
 755         if (rv) {
 756                 printf("RobomonAtlantis: Unable to initialize ORTE\n");
 757         }
 758
 759         /* publishers */
 760         robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
 761
 762         robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
 763         robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
 764
 765         /* subscribers */
 766         robottype_subscriber_pwr_voltage_create(&orte,
 767                                 receivePowerVoltageCallBack, this);
 768         robottype_subscriber_motion_status_create(&orte,
 769                                 receiveMotionStatusCallBack, this);
 770         robottype_subscriber_ref_pos_create(&orte,
 771                                 receiveActualPositionCallBack, this);
 772         robottype_subscriber_est_pos_odo_create(&orte,
 773                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
 774         robottype_subscriber_est_pos_indep_odo_create(&orte,
 775                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
 776         robottype_subscriber_hokuyo_scan_create(&orte,
 777                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
 778         robottype_subscriber_fsm_main_create(&orte,
 779                                              rcv_fsm_main_cb, this);
 780         robottype_subscriber_fsm_motion_create(&orte,
 781                                              rcv_fsm_motion_cb, this);
 782         robottype_subscriber_fsm_act_create(&orte,
 783                                              rcv_fsm_act_cb, this);
 784
 785         /* motors */
 786         orte.motion_speed.left = 0;
 787         orte.motion_speed.right = 0;
 788
 789         /* power management */
 790         orte.pwr_ctrl.voltage33 = true;
 791         orte.pwr_ctrl.voltage50 = true;
 792         orte.pwr_ctrl.voltage80 = true;
 793         voltage33CheckBox->setChecked(true);
 794         voltage50CheckBox->setChecked(true);
 795         voltage80CheckBox->setChecked(true);
 796
 797         act_init(&orte);
 798
 799         /* set actions to do when we receive data from orte */
 800         connect(this, SIGNAL(motionStatusReceivedSignal()),
 801                         this, SLOT(motionStatusReceived()));
 802         connect(this, SIGNAL(actualPositionReceivedSignal()),
 803                         this, SLOT(actualPositionReceived()));
 804         connect(this, SIGNAL(powerVoltageReceivedSignal()),
 805                         this, SLOT(powerVoltageReceived()));
 806 }
 807
 808 void RobomonAtlantis::motionStatusReceived()
 809 {
 810         WDBG("ORTE received: motion status");
 811 }
 812
 813 void RobomonAtlantis::actualPositionReceived()
 814 {
 815         actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
 816         actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
 817         actPosPhi->setText(QString("%1(%2)")
 818                         .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
 819                         .arg(orte.ref_pos.phi, 0, 'f', 1));
 820         robotActPos->moveRobot(orte.ref_pos.x,
 821                 orte.ref_pos.y, orte.ref_pos.phi);
 822         trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
 823 }
 824
 825 void RobomonAtlantis::powerVoltageReceived()
 826 {
 827         voltage33LineEdit->setText(QString("%1").arg(
 828                         orte.pwr_voltage.voltage33, 0, 'f', 3));
 829         voltage50LineEdit->setText(QString("%1").arg(
 830                         orte.pwr_voltage.voltage50, 0, 'f', 3));
 831         voltage80LineEdit->setText(QString("%1").arg(
 832                         orte.pwr_voltage.voltage80, 0, 'f', 3));
 833         voltageBATLineEdit->setText(QString("%1").arg(
 834                         orte.pwr_voltage.voltageBAT, 0, 'f', 3));
 835
 836 }
 837
 838 /**********************************************************************
 839  * MISCELLANEOUS
 840  **********************************************************************/
 841 void RobomonAtlantis::openSharedMemory()
 842 {
 843         int segmentId;
 844         int sharedSegmentSize;
 845
 846         if (sharedMemoryOpened)
 847                 return;
 848
 849         sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
 850
 851         /* Get segment identificator in a read only mode  */
 852         segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
 853         if(segmentId == -1) {
 854                 QMessageBox::critical(this, "robomon",
 855                                 "Unable to open shared memory segment!");
 856                 return;
 857         }
 858
 859         /* Init Shmap */
 860         ShmapInit(0);
 861
 862         /* Attach the shared memory segment */
 863         //map =  (_Map*)shmat (segmentId, (void*) 0, 0);
 864
 865         sharedMemoryOpened = true;
 866 }
 867
 868 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
 869 {
 870         double distance=4.0, min_distance=4.0;
 871         int i,j;
 872         Point wall;
 873         struct map *map = ShmapIsMapInit();
 874
 875         if (!map) return min_distance;
 876
 877         // Simulate obstacles
 878         for(j=0;j<MAP_HEIGHT;j++) {
 879                 for (i=0;i<MAP_WIDTH;i++) {
 880                         struct map_cell *cell = &map->cells[j][i];
 881                         if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
 882                                 // WALL
 883                                 ShmapCell2Point(i, j, &wall.x, &wall.y);
 884
 885                                 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
 886                                 if (distance<min_distance) min_distance = distance;
 887                         }
 888                 }
 889         }
 890
 891         return min_distance;
 892 }
 893
 894 /**
 895  * Calculation for Hokuyo simulation. Calculates distance that would
 896  * be returned by Hokuyo sensors, if there is only one obstacle (as
 897  * specified by parameters).
 898  *
 899  * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
 900  * @param obstacle Position of the obstacle (x, y in meters).
 901  * @param obstacleSize Size (diameter) of the obstacle in meters.
 902  *
 903  * @return Distance measured by sensors in meters.
 904  */
 905 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
 906 {
 907         struct robot_pos_type e = orte.est_pos_indep_odo;
 908         double sensor_a;
 909         struct sharp_pos s;
 910
 911         s.x = HOKUYO_CENTER_OFFSET_M;
 912         s.y = 0.0;
 913         s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
 914
 915         Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
 916                      e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
 917         sensor_a = e.phi + s.ang;
 918
 919         const double sensorRange = 4.0; /*[meters]*/
 920
 921         double distance, angle;
 922
 923         angle = sensor.angleTo(obstacle) - sensor_a;
 924         angle = fmod(angle, 2.0*M_PI);
 925         if (angle > +M_PI) angle -= 2.0*M_PI;
 926         if (angle < -M_PI) angle += 2.0*M_PI;
 927         angle = fabs(angle);
 928         distance = sensor.distanceTo(obstacle)-0.11;
 929         if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
 930                 // We can see the obstackle from here.
 931                 if (angle < M_PI/2.0) {
 932                     distance = distance/cos(angle);
 933                 }
 934                 if (distance > sensorRange)
 935                         distance = sensorRange;
 936         } else {
 937                 distance = sensorRange;
 938         }
 939
 940         return distance;
 941 }
 942
 943 void RobomonAtlantis::sendStart(int plug)
 944 {
 945         orte.robot_cmd.start = plug ? 0 : 1;
 946         ORTEPublicationSend(orte.publication_robot_cmd);
 947 }
 948
 949 void RobomonAtlantis::resetTrails()
 950 {
 951         trailRefPos->reset();
 952         trailPosIndepOdo->reset();
 953         trailOdoPos->reset();
 954 }
 955
 956 void RobomonAtlantis::showTrails(bool show)
 957 {
 958         trailRefPos->setVisible(show);
 959         trailPosIndepOdo->setVisible(show);
 960         trailOdoPos->setVisible(show);
 961 }