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