src/robomon/RobomonAtlantis.cpp

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