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