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