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