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