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