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 <lidar_params.h>
  34 #include <actuators.h>
  35 #include "PlaygroundScene.h"
  36 #include "MiscGui.h"
  37 #include "robomon_orte.h"
  38 #include "RobomonAtlantis.h"
  39 #include "playgroundview.h"
  40 #include "trail.h"
  41
  42 #include <QCoreApplication>
  43 #include <QEvent>
  44 #include <QKeyEvent>
  45 #include <QDebug>
  46 #include <QMessageBox>
  47
  48 RobomonAtlantis::RobomonAtlantis(QStatusBar *_statusBar)
  49         : QWidget(0), statusBar(_statusBar)
  50 {
  51         QFont font;
  52         font.setPointSize(7);
  53         setFont(font);
  54
  55         debugWindowEnabled = false;
  56
  57         createLeftLayout();
  58         createRightLayout();
  59
  60         QHBoxLayout *mainLayout = new QHBoxLayout;
  61         mainLayout->addLayout(leftLayout);
  62         mainLayout->addLayout(rightLayout);
  63         setLayout(mainLayout);
  64
  65         createOrte();
  66         createRobots();
  67         createActions();
  68         createMap();
  69
  70 //      connect(vidle, SIGNAL(valueChanged(int)),
  71 //              robotEstPosBest, SLOT(setVidle(int)));
  72
  73         setFocusPolicy(Qt::StrongFocus);
  74         sharedMemoryOpened = false;
  75         WDBG("Youuuhouuuu!!");
  76 }
  77
  78 /**********************************************************************
  79  * GUI
  80  **********************************************************************/
  81 void RobomonAtlantis::createLeftLayout()
  82 {
  83         leftLayout = new QVBoxLayout();
  84
  85         createDebugGroupBox();
  86         debugWindowEnabled = true;
  87         createPlaygroundGroupBox();
  88         leftLayout->addWidget(playgroundGroupBox);
  89         //leftLayout->addWidget(debugGroupBox); // FIXME: move this to separate tab
  90 }
  91
  92 void RobomonAtlantis::createRightLayout()
  93 {
  94         rightLayout = new QVBoxLayout();
  95
  96         createPositionGroupBox();
  97         createMiscGroupBox();
  98         createFSMGroupBox();
  99         createActuatorsGroupBox();
 100         createPowerGroupBox();
 101
 102         rightLayout->addWidget(positionGroupBox);
 103         rightLayout->addWidget(miscGroupBox);
 104         rightLayout->addWidget(fsmGroupBox);
 105         //rightLayout->addWidget(powerGroupBox);
 106         rightLayout->addWidget(actuatorsGroupBox);
 107 }
 108
 109 void RobomonAtlantis::createPlaygroundGroupBox()
 110 {
 111         playgroundGroupBox = new QGroupBox(tr("Playground"));
 112         QHBoxLayout *layout = new QHBoxLayout();
 113
 114         playgroundScene = new PlaygroundScene();
 115         playgroundSceneView = new PlaygroundView(playgroundScene);
 116         //playgroundSceneView->setMinimumWidth(630);
 117         //playgroundSceneView->setMinimumHeight(445);
 118         playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
 119         playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
 120         playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 121         playgroundSceneView->setMouseTracking(true);
 122         layout->addWidget(playgroundSceneView);
 123
 124         playgroundGroupBox->setLayout(layout);
 125 }
 126
 127 void RobomonAtlantis::createPositionGroupBox()
 128 {
 129         positionGroupBox = new QGroupBox(tr("Position state"));
 130         positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding);
 131         QGridLayout *layout = new QGridLayout();
 132
 133         actPosX = new QLineEdit();
 134         actPosY = new QLineEdit();
 135         actPosPhi = new QLineEdit();
 136
 137         estPosX = new QLineEdit();
 138         estPosY = new QLineEdit();
 139         estPosPhi = new QLineEdit();
 140
 141         actPosX->setReadOnly(true);
 142         actPosY->setReadOnly(true);
 143         actPosPhi->setReadOnly(true);
 144
 145         estPosX->setReadOnly(true);
 146         estPosY->setReadOnly(true);
 147         estPosPhi->setReadOnly(true);
 148
 149         layout->addWidget(MiscGui::createLabel("X"), 1, 0);
 150         layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
 151         layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
 152
 153         layout->addWidget(MiscGui::createLabel("X"), 5, 0);
 154         layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
 155         layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
 156
 157         layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
 158         layout->addWidget(actPosX, 1, 1);
 159         layout->addWidget(actPosY, 2, 1);
 160         layout->addWidget(actPosPhi, 3, 1);
 161
 162         layout->addWidget(MiscGui::createLabel("Estimated (indep. odo.)", Qt::AlignLeft), 4, 1);
 163         layout->addWidget(estPosX, 5, 1);
 164         layout->addWidget(estPosY, 6, 1);
 165         layout->addWidget(estPosPhi, 7, 1);
 166
 167         positionGroupBox->setLayout(layout);
 168 }
 169
 170 void RobomonAtlantis::createMiscGroupBox()
 171 {
 172         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 173         miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 174         QGridLayout *layout = new QGridLayout();
 175
 176         obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
 177         obstacleSimulationCheckBox->setShortcut(tr("o"));
 178         layout->addWidget(obstacleSimulationCheckBox);
 179
 180         startPlug = new QCheckBox("&Start plug");
 181         layout->addWidget(startPlug);
 182
 183         colorChoser = new QCheckBox("&Team color");
 184         layout->addWidget(colorChoser);
 185
 186         strategyButton= new QPushButton(tr("Strategy"));
 187         layout->addWidget(strategyButton);
 188
 189         miscGroupBox->setLayout(layout);
 190 }
 191
 192 void RobomonAtlantis::createFSMGroupBox()
 193 {
 194         fsmGroupBox = new QGroupBox(tr("FSM"));
 195         fsmGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 196         QGridLayout *layout = new QGridLayout();
 197
 198         layout->addWidget(MiscGui::createLabel("Main:"), 1, 0);
 199         fsm_main_state = new QLabel();
 200         fsm_main_state->setMinimumWidth(100);
 201         fsm_main_state->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
 202         layout->addWidget(fsm_main_state, 1, 1);
 203
 204         layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
 205         fsm_act_state = new QLabel();
 206         layout->addWidget(fsm_act_state, 2, 1);
 207
 208         layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
 209         fsm_motion_state = new QLabel();
 210         layout->addWidget(fsm_motion_state, 3, 1);
 211
 212         fsmGroupBox->setLayout(layout);
 213 }
 214
 215 void RobomonAtlantis::createDebugGroupBox()
 216 {
 217         debugGroupBox = new QGroupBox(tr("Debug window"));
 218         debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
 219         QHBoxLayout *layout = new QHBoxLayout();
 220
 221         debugWindow = new QTextEdit();
 222         debugWindow->setReadOnly(true);
 223
 224         layout->addWidget(debugWindow);
 225         debugGroupBox->setLayout(layout);
 226 }
 227
 228 void RobomonAtlantis::createActuatorsGroupBox()
 229 {
 230         actuatorsGroupBox = new QGroupBox(tr("Actuators"));
 231         actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
 232         QHBoxLayout *layout = new QHBoxLayout();
 233 //      vidle = new QDial();
 234
 235 //      vidle->setMinimum(VIDLE_VYSIP);
 236 //      vidle->setMaximum((VIDLE_UP-VIDLE_VYSIP)+VIDLE_VYSIP);
 237 //      vidle->setEnabled(true);
 238
 239         //createMotorsGroupBox();
 240
 241         layout->setAlignment(Qt::AlignLeft);
 242 //      layout->addWidget(vidle);
 243         //layout->addWidget(enginesGroupBox);
 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 LidarScan(hokuyo_params);
 318         hokuyoScan->setZValue(10);
 319         playgroundScene->addItem(hokuyoScan);
 320
 321         sickScan = new LidarScan(sick_params);
 322         sickScan->setZValue(10);
 323         playgroundScene->addItem(sickScan);
 324
 325 }
 326
 327 void RobomonAtlantis::createMap()
 328 {
 329         mapImage = new Map();
 330         mapImage->setZValue(5);
 331         mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
 332
 333
 334         playgroundScene->addItem(mapImage);
 335 }
 336
 337 /**********************************************************************
 338  * GUI actions
 339  **********************************************************************/
 340 void RobomonAtlantis::createActions()
 341 {
 342         /* power management */
 343         connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
 344                         this, SLOT(setVoltage33(int)));
 345         connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
 346                         this, SLOT(setVoltage50(int)));
 347         connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
 348                         this, SLOT(setVoltage80(int)));
 349
 350         /* motors */
 351 //      connect(leftMotorSlider, SIGNAL(valueChanged(int)),
 352 //                      this, SLOT(setLeftMotor(int)));
 353 //      connect(rightMotorSlider, SIGNAL(valueChanged(int)),
 354 //                      this, SLOT(setRightMotor(int)));
 355 //      connect(stopMotorsPushButton, SIGNAL(clicked()),
 356 //                      this, SLOT(stopMotors()));
 357
 358         connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
 359         connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int)));
 360         connect(strategyButton, SIGNAL(pressed()), this, SLOT(changeStrategy_1()));
 361         connect(strategyButton, SIGNAL(released()), this, SLOT(changeStrategy_0()));
 362
 363         /* obstacle simulation */
 364         simulationEnabled = 0;
 365         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 366                         this, SLOT(setSimulation(int)));
 367         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 368                         this, SLOT(setObstacleSimulation(int)));
 369         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 370                         playgroundScene, SLOT(showObstacle(int)));
 371         connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
 372                         this, SLOT(changeObstacle(QPointF)));
 373 }
 374
 375 void RobomonAtlantis::changeStrategy_1()
 376 {
 377         orte.robot_switches.strategy = true;
 378         ORTEPublicationSend(orte.publication_robot_switches);
 379 }
 380
 381 void RobomonAtlantis::changeStrategy_0()
 382 {
 383         orte.robot_switches.strategy = false;
 384         ORTEPublicationSend(orte.publication_robot_switches);
 385 }
 386
 387 void RobomonAtlantis::setVoltage33(int state)
 388 {
 389         if (state)
 390                 orte.pwr_ctrl.voltage33 = true;
 391         else
 392                 orte.pwr_ctrl.voltage33 = false;
 393 }
 394
 395 void RobomonAtlantis::setVoltage50(int state)
 396 {
 397         if (state)
 398                 orte.pwr_ctrl.voltage50 = true;
 399         else
 400                 orte.pwr_ctrl.voltage50 = false;
 401 }
 402
 403 void RobomonAtlantis::setVoltage80(int state)
 404 {
 405         if (state)
 406                 orte.pwr_ctrl.voltage80 = true;
 407         else
 408                 orte.pwr_ctrl.voltage80 = false;
 409 }
 410
 411 // void RobomonAtlantis::setLeftMotor(int value)
 412 // {
 413 //      short int leftMotor;
 414 //      short int rightMotor;
 415
 416 //      if(bothMotorsCheckBox->isChecked())
 417 //              rightMotorSlider->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::setRightMotor(int value)
 428 // {
 429 //      short int leftMotor;
 430 //      short int rightMotor;
 431
 432 //      if(bothMotorsCheckBox->isChecked())
 433 //              leftMotorSlider->setValue(value);
 434
 435 //      leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 436 //      rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 437
 438 //      orte.motion_speed.left = leftMotor;
 439 //      orte.motion_speed.right = rightMotor;
 440
 441 // }
 442
 443 // void RobomonAtlantis::stopMotors()
 444 // {
 445 //      leftMotorSlider->setValue(0);
 446 //      rightMotorSlider->setValue(0);
 447 // }
 448
 449 void RobomonAtlantis::useOpenGL(bool use)
 450 {
 451         playgroundSceneView->useOpenGL(&use);
 452 }
 453
 454 void RobomonAtlantis::showMap(bool show)
 455 {
 456         openSharedMemory();
 457
 458         if (sharedMemoryOpened == false)
 459                 return;
 460
 461         if (show) {
 462                 mapTimer = new QTimer(this);
 463                 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 464                 mapTimer->start(200);
 465         } else {
 466                 if(mapTimer != NULL) {
 467                         mapTimer->stop();
 468                         disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 469                 }
 470         }
 471         mapImage->setVisible(show);
 472 }
 473
 474 void RobomonAtlantis::paintMap()
 475 {
 476         using namespace Qt;
 477         struct map *map = ShmapIsMapInit();
 478
 479         if (!map) return;
 480
 481         for(int i = 0; i < MAP_WIDTH; i++) {
 482                 for(int j = 0; j < MAP_HEIGHT; j++) {
 483                         QColor color;
 484
 485                         struct map_cell *cell = &map->cells[j][i];
 486                         color = lightGray;
 487
 488                         if (cell->flags & MAP_FLAG_WALL)
 489                                 color = darkYellow;
 490                         if (cell->flags & MAP_FLAG_IGNORE_OBST)
 491                                 color = darkGreen;
 492                         if (cell->flags & MAP_FLAG_SIMULATED_WALL)
 493                                 color = yellow;
 494                         if (cell->flags & MAP_FLAG_PATH)
 495                                 color = darkRed;
 496                         if (cell->flags & MAP_FLAG_START)
 497                                 color = red;
 498                         if (cell->flags & MAP_FLAG_GOAL)
 499                                 color = green;
 500                         if (cell->flags & MAP_FLAG_PLAN_MARGIN) {
 501                                 QColor c(240, 170, 50); /* orange */
 502                                 color = c;
 503                         }
 504                         if (cell->detected_obstacle) {
 505                                 QColor c1(color), c2(blue);
 506                                 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
 507                                 QColor c(c1.red()   + (int)(f*(c2.red()   - c1.red())),
 508                                          c1.green() + (int)(f*(c2.green() - c1.green())),
 509                                          c1.blue()  + (int)(f*(c2.blue()  - c1.blue())));
 510                                 color = c;
 511                         }
 512                         if (cell->flags & MAP_FLAG_DET_OBST)
 513                                 color = cyan;
 514
 515                         color.setAlpha(200);
 516                         mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
 517                 }
 518         }
 519 }
 520
 521 void RobomonAtlantis::setSimulation(int state)
 522 {
 523         if(state) {
 524                 robottype_publisher_sick_scan_create(&orte, NULL, this);
 525                 robottype_publisher_hokuyo_scan_create(&orte, NULL, this);
 526         } else {
 527                 if (!simulationEnabled)
 528                         return;
 529                 robottype_publisher_sick_scan_destroy(&orte);
 530                 robottype_publisher_hokuyo_scan_destroy(&orte);
 531         }
 532         simulationEnabled = state;
 533 }
 534
 535 /*!
 536         \fn RobomonAtlantis::setObstacleSimulation(int state)
 537  */
 538 void RobomonAtlantis::setObstacleSimulation(int state)
 539 {
 540         if (state) {
 541                 /* TODO Maybe it is possible to attach only once to Shmap */
 542                 ShmapInit(0);
 543                 obstacleSimulationTimer = new QTimer(this);
 544                 connect(obstacleSimulationTimer, SIGNAL(timeout()),
 545                         this, SLOT(simulateObstaclesHokuyo()));
 546                 connect(obstacleSimulationTimer, SIGNAL(timeout()),
 547                         this, SLOT(simulateObstaclesSick()));
 548                 obstacleSimulationTimer->start(100);
 549                 setMouseTracking(true);
 550                 hokuyoScan->setVisible(true);
 551                 sickScan->setVisible(true);
 552         } else {
 553                 if (obstacleSimulationTimer)
 554                         delete obstacleSimulationTimer;
 555                 // Hide scans of lidars
 556                 hokuyoScan->setVisible(false);
 557                 sickScan->setVisible(false);
 558         }
 559 }
 560
 561
 562 void RobomonAtlantis::simulateObstaclesLidar(const struct lidar_params lidar)
 563 {
 564         double distance, wall_distance;
 565         unsigned int i;
 566         unsigned int data_lenght = 0;
 567         uint16_t *lidar_data = NULL;
 568         switch (lidar.type) {
 569                 case HOKUYO:
 570                         lidar_data = orte.hokuyo_scan.data;
 571                         data_lenght = hokuyo_params.data_lenght;
 572                         break;
 573                 case SICK_TIM3XX:
 574                         lidar_data = orte.sick_scan.data;
 575                         data_lenght = sick_params.data_lenght;
 576                         break;
 577                 default:
 578                         return;
 579         }
 580
 581         for (i = 0; i < data_lenght; i++) {
 582                 wall_distance = distanceToWallLidar(lidar, i);
 583                 distance = distanceToCircularObstacleLidar(lidar, i, simulatedObstacle, SIM_OBST_SIZE_M);
 584                 if (wall_distance < distance)
 585                         distance = wall_distance;
 586                 lidar_data[i] = distance*1000;
 587         }
 588
 589         switch (lidar.type) {
 590                 case HOKUYO:
 591                         orte.hokuyo_scan.data_lenght = hokuyo_params.data_lenght;
 592                         orte.hokuyo_scan.lidar_type = hokuyo_params.type;
 593                         ORTEPublicationSend(orte.publication_hokuyo_scan);
 594                         break;
 595                 case SICK_TIM3XX:
 596                         orte.sick_scan.data_lenght = sick_params.data_lenght;
 597                         orte.sick_scan.lidar_type = sick_params.type;
 598                         ORTEPublicationSend(orte.publication_sick_scan);
 599                         break;
 600                 default:
 601                         return;
 602         }
 603 }
 604
 605 void RobomonAtlantis::simulateObstaclesHokuyo()
 606 {
 607         simulateObstaclesLidar(hokuyo_params);
 608 }
 609
 610 void RobomonAtlantis::simulateObstaclesSick()
 611 {
 612         simulateObstaclesLidar(sick_params);
 613 }
 614
 615 void RobomonAtlantis::changeObstacle(QPointF position)
 616 {
 617         if (!simulationEnabled) {
 618                 simulationEnabled = 1;
 619                 obstacleSimulationCheckBox->setChecked(true);
 620         }
 621
 622         simulatedObstacle.x = position.x();
 623         simulatedObstacle.y = position.y();
 624         simulateObstaclesLidar(hokuyo_params);
 625         simulateObstaclesLidar(sick_params);
 626 }
 627
 628 /**********************************************************************
 629  * EVENTS
 630  **********************************************************************/
 631 bool RobomonAtlantis::event(QEvent *event)
 632 {
 633         switch (event->type()) {
 634                 case QEVENT(QEV_MOTION_STATUS):
 635                         emit motionStatusReceivedSignal();
 636                         break;
 637                 case QEVENT(QEV_SICK_SCAN):
 638                         sickScan->newScan(&orte.sick_scan);
 639                         break;
 640                 case QEVENT(QEV_HOKUYO_SCAN):
 641                         hokuyoScan->newScan(&orte.hokuyo_scan);
 642                         break;
 643                 case QEVENT(QEV_REFERENCE_POSITION):
 644                         emit actualPositionReceivedSignal();
 645                         break;
 646                 case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO):
 647                         estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3));
 648                         estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3));
 649                         estPosPhi->setText(QString("%1(%2)")
 650                                         .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0)
 651                                         .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1));
 652                         robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x,
 653                                 orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
 654                         trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x,
 655                                               orte.est_pos_indep_odo.y));
 656                         break;
 657                 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
 658                         robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
 659                                         orte.est_pos_odo.y, orte.est_pos_odo.phi);
 660                         trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
 661                                               orte.est_pos_odo.y));
 662                         break;
 663                 case QEVENT(QEV_ESTIMATED_POSITION_BEST):
 664                         robotEstPosBest->moveRobot(orte.est_pos_best.x,
 665                                         orte.est_pos_best.y, orte.est_pos_best.phi);
 666                         trailEstPosBest->addPoint(QPointF(orte.est_pos_best.x,
 667                                               orte.est_pos_best.y));
 668                         hokuyoScan->setPosition(orte.est_pos_best.x,
 669                                                 orte.est_pos_best.y,
 670                                                 orte.est_pos_best.phi);
 671                         sickScan->setPosition(orte.est_pos_best.x,
 672                                                 orte.est_pos_best.y,
 673                                                 orte.est_pos_best.phi);
 674                         break;
 675                 case QEVENT(QEV_POWER_VOLTAGE):
 676                         emit powerVoltageReceivedSignal();
 677                         break;
 678                 case QEVENT(QEV_FSM_MAIN):
 679                         fsm_main_state->setText(orte.fsm_main.state_name);
 680                         break;
 681                 case QEVENT(QEV_FSM_ACT):
 682                         fsm_act_state->setText(orte.fsm_act.state_name);
 683                         break;
 684                 case QEVENT(QEV_FSM_MOTION):
 685                         fsm_motion_state->setText(orte.fsm_motion.state_name);
 686                         break;
 687                 default:
 688                         if (event->type() == QEvent::Close)
 689                                 closeEvent((QCloseEvent *)event);
 690                         else if (event->type() == QEvent::KeyPress)
 691                                 keyPressEvent((QKeyEvent *)event);
 692                         else if (event->type() == QEvent::KeyRelease)
 693                                 keyReleaseEvent((QKeyEvent *)event);
 694                         else if (event->type() == QEvent::FocusIn)
 695                                 grabKeyboard();
 696                         else if (event->type() == QEvent::FocusOut)
 697                                 releaseKeyboard();
 698                         else {
 699                                 event->ignore();
 700                                 return false;
 701                         }
 702                         break;
 703         }
 704         event->accept();
 705         return true;
 706 }
 707
 708 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
 709 {
 710 //      double peak, gain;
 711
 712         if (event->isAutoRepeat()) {
 713                 switch (event->key()) {
 714 //                      case Qt::Key_Down:
 715 //                              peak = leftMotorSlider->minimum()/2;
 716 //                              if (leftMotorValue < peak ||
 717 //                                      rightMotorValue < peak)
 718 //                                      gain = 1.01;
 719 //                              else
 720 //                                      gain = 1.3;
 721 //                              leftMotorValue *= gain;
 722 //                              rightMotorValue *= gain;
 723 //                              leftMotorSlider->setValue((int)leftMotorValue);
 724 //                              rightMotorSlider->setValue((int)rightMotorValue);
 725 //                              break;
 726
 727 //                      case Qt::Key_Up:
 728 //                      case Qt::Key_Left:
 729 //                      case Qt::Key_Right:
 730 //                              peak = leftMotorSlider->maximum()/2;
 731 //                              if (leftMotorValue > peak ||
 732 //                                      rightMotorValue > peak)
 733 //                                      gain = 1.01;
 734 //                              else
 735 //                                      gain = 1.3;
 736 //                              leftMotorValue *= gain;
 737 //                              rightMotorValue *= gain;
 738 //                              leftMotorSlider->setValue((int)leftMotorValue);
 739 //                              rightMotorSlider->setValue((int)rightMotorValue);
 740 //                              break;
 741
 742                         default:
 743                                 event->ignore();
 744                                 break;
 745                 }
 746                 return;
 747         }
 748
 749         switch (event->key()) {
 750 //              case Qt::Key_Up:
 751 //                      leftMotorValue = 1;
 752 //                      rightMotorValue = 1;
 753 //                      bothMotorsCheckBox->setChecked(true);
 754 //                      leftMotorSlider->setValue((int)leftMotorValue);
 755 //                      setLeftMotor((int)leftMotorValue);
 756 //                      break;
 757 //              case Qt::Key_Down:
 758 //                      leftMotorValue = -1;
 759 //                      rightMotorValue = -1;
 760 //                      bothMotorsCheckBox->setChecked(true);
 761 //                      leftMotorSlider->setValue((int)leftMotorValue);
 762 //                      setLeftMotor((int)leftMotorValue);
 763 //                      break;
 764 //              case Qt::Key_Left:
 765 //                      leftMotorValue = -1;
 766 //                      rightMotorValue = 1;
 767 //                      leftMotorSlider->setValue((int)leftMotorValue);
 768 //                      rightMotorSlider->setValue((int)rightMotorValue);
 769 //                      setLeftMotor((int)leftMotorValue);
 770 //                      setRightMotor((int)leftMotorValue);
 771 //                      break;
 772 //              case Qt::Key_Right:
 773 //                      leftMotorValue = 1;
 774 //                      rightMotorValue = -1;
 775 //                      leftMotorSlider->setValue((int)leftMotorValue);
 776 //                      rightMotorSlider->setValue((int)rightMotorValue);
 777 //                      setLeftMotor((int)leftMotorValue);
 778 //                      setRightMotor((int)rightMotorValue);
 779 //                      break;
 780                 default:
 781                         event->ignore();
 782                         break;
 783         }
 784         event->accept();
 785 }
 786
 787 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
 788 {
 789         if (event->isAutoRepeat()) {
 790                 event->ignore();
 791                 return;
 792         }
 793
 794         switch (event->key()) {
 795 //              case Qt::Key_Up:
 796 //              case Qt::Key_Down:
 797 //              case Qt::Key_Left:
 798 //              case Qt::Key_Right:
 799 //                      leftMotorValue = 0;
 800 //                      rightMotorValue = 0;
 801 //                      bothMotorsCheckBox->setChecked(false);
 802 //                      leftMotorSlider->setValue((int)leftMotorValue);
 803 //                      rightMotorSlider->setValue((int)rightMotorValue);
 804 //                      break;
 805                 default:
 806                         event->ignore();
 807                         break;
 808         }
 809         event->accept();
 810 }
 811
 812 void RobomonAtlantis::closeEvent(QCloseEvent *)
 813 {
 814         robottype_roboorte_destroy(&orte);
 815 }
 816
 817 /**********************************************************************
 818  * ORTE
 819  **********************************************************************/
 820 void RobomonAtlantis::createOrte()
 821 {
 822         int rv;
 823
 824         orte.strength = 11;
 825
 826         memset(&orte, 0, sizeof(orte));
 827         rv = robottype_roboorte_init(&orte);
 828         if (rv) {
 829                 printf("RobomonAtlantis: Unable to initialize ORTE\n");
 830         }
 831
 832         /* publishers */
 833         robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
 834
 835         robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
 836         robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
 837         robottype_publisher_robot_switches_create(&orte, dummy_publisher_callback, &orte);
 838
 839         /* subscribers */
 840         robottype_subscriber_pwr_voltage_create(&orte,
 841                                 receivePowerVoltageCallBack, this);
 842         robottype_subscriber_motion_status_create(&orte,
 843                                 receiveMotionStatusCallBack, this);
 844         robottype_subscriber_ref_pos_create(&orte,
 845                                 receiveActualPositionCallBack, this);
 846         robottype_subscriber_est_pos_odo_create(&orte,
 847                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
 848         robottype_subscriber_est_pos_indep_odo_create(&orte,
 849                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
 850         robottype_subscriber_est_pos_best_create(&orte,
 851                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
 852         robottype_subscriber_sick_scan_create(&orte,
 853                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_SICK_SCAN));
 854         robottype_subscriber_hokuyo_scan_create(&orte,
 855                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
 856         robottype_subscriber_fsm_main_create(&orte,
 857                                              rcv_fsm_main_cb, this);
 858         robottype_subscriber_fsm_motion_create(&orte,
 859                                              rcv_fsm_motion_cb, this);
 860         robottype_subscriber_fsm_act_create(&orte,
 861                                              rcv_fsm_act_cb, this);
 862
 863         /* motors */
 864         orte.motion_speed.left = 0;
 865         orte.motion_speed.right = 0;
 866
 867         /* power management */
 868         orte.pwr_ctrl.voltage33 = true;
 869         orte.pwr_ctrl.voltage50 = true;
 870         orte.pwr_ctrl.voltage80 = true;
 871         voltage33CheckBox->setChecked(true);
 872         voltage50CheckBox->setChecked(true);
 873         voltage80CheckBox->setChecked(true);
 874
 875         act_init(&orte);
 876
 877         /* set actions to do when we receive data from orte */
 878         connect(this, SIGNAL(motionStatusReceivedSignal()),
 879                         this, SLOT(motionStatusReceived()));
 880         connect(this, SIGNAL(actualPositionReceivedSignal()),
 881                         this, SLOT(actualPositionReceived()));
 882         connect(this, SIGNAL(powerVoltageReceivedSignal()),
 883                         this, SLOT(powerVoltageReceived()));
 884 }
 885
 886 void RobomonAtlantis::motionStatusReceived()
 887 {
 888         WDBG("ORTE received: motion status");
 889 }
 890
 891 void RobomonAtlantis::actualPositionReceived()
 892 {
 893         actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
 894         actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
 895         actPosPhi->setText(QString("%1(%2)")
 896                         .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
 897                         .arg(orte.ref_pos.phi, 0, 'f', 1));
 898         robotRefPos->moveRobot(orte.ref_pos.x,
 899                 orte.ref_pos.y, orte.ref_pos.phi);
 900         trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
 901 }
 902
 903 void RobomonAtlantis::powerVoltageReceived()
 904 {
 905         voltage33LineEdit->setText(QString("%1").arg(
 906                         orte.pwr_voltage.voltage33, 0, 'f', 3));
 907         voltage50LineEdit->setText(QString("%1").arg(
 908                         orte.pwr_voltage.voltage50, 0, 'f', 3));
 909         voltage80LineEdit->setText(QString("%1").arg(
 910                         orte.pwr_voltage.voltage80, 0, 'f', 3));
 911         voltageBATLineEdit->setText(QString("%1").arg(
 912                         orte.pwr_voltage.voltageBAT, 0, 'f', 3));
 913
 914 }
 915
 916 /**********************************************************************
 917  * MISCELLANEOUS
 918  **********************************************************************/
 919 void RobomonAtlantis::openSharedMemory()
 920 {
 921         int segmentId;
 922         int sharedSegmentSize;
 923
 924         if (sharedMemoryOpened)
 925                 return;
 926
 927         sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
 928
 929         /* Get segment identificator in a read only mode  */
 930         segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
 931         if(segmentId == -1) {
 932                 statusBar->showMessage("No external map found - creating a new map.");
 933         }
 934
 935         /* Init Shmap */
 936         ShmapInit(0);
 937
 938         /* Attach the shared memory segment */
 939         //map =  (_Map*)shmat (segmentId, (void*) 0, 0);
 940
 941         sharedMemoryOpened = true;
 942 }
 943
 944 double RobomonAtlantis::distanceToWallLidar(const struct lidar_params lidar, int beamnum)
 945 {
 946         double distance = 4.0, min_distance = 4.0;
 947         int i, j;
 948         Point wall;
 949         struct map *map = ShmapIsMapInit();
 950
 951         if (!map)
 952                 return min_distance;
 953
 954         // Simulate obstacles
 955         for(j = 0; j < MAP_HEIGHT; j++) {
 956                 for (i = 0; i < MAP_WIDTH; i++) {
 957                         struct map_cell *cell = &map->cells[j][i];
 958                         if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
 959                                 // WALL
 960                                 ShmapCell2Point(i, j, &wall.x, &wall.y);
 961
 962                                 distance = distanceToObstacleLidar(lidar, beamnum, wall, MAP_CELL_SIZE_M);
 963                                 if (distance < min_distance)
 964                                         min_distance = distance;
 965                         }
 966                 }
 967         }
 968
 969         return min_distance;
 970 }
 971
 972 double RobomonAtlantis::distanceToCircularObstacleLidar(const struct lidar_params lidar, int beamnum, Point center, double diameter)
 973 {
 974         struct robot_pos_type e = orte.est_pos_best;
 975         double sensor_a;
 976         struct sharp_pos s;
 977
 978         s.x = lidar.center_offset_m;
 979         s.y = 0.0;
 980         s.ang = index2rad(lidar, beamnum);
 981
 982         Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
 983                      e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
 984         sensor_a = e.phi + s.ang;
 985
 986         const double sensorRange = 4.0; /*[meters]*/
 987
 988         double distance = sensorRange;
 989         double angle;
 990
 991         angle = sensor.angleTo(center) - sensor_a;
 992         angle = fmod(angle, 2.0 * M_PI);
 993         if (angle > +M_PI) angle -= 2.0 * M_PI;
 994         if (angle < -M_PI) angle += 2.0 * M_PI;
 995         angle = fabs(angle);
 996
 997         double k = tan(sensor_a);
 998         double r = diameter / 2.0;
 999
1000         double A = 1 + k * k;
1001         double B = 2 * (sensor.y * k - center.x - k * k * sensor.x - center.y * k);
1002         double C = center.x * center.x + center.y * center.y +
1003                 k * k * sensor.x * sensor.x - 2*sensor.y*k*sensor.x +
1004                 sensor.y * sensor.y + 2 * k * sensor.x *center.y -
1005                 2 * sensor.y * center.y - r * r;
1006
1007         double D = B * B - 4 * A * C;
1008
1009         if (D > 0) {
1010                 Point ob1, ob2;
1011
1012                 ob1.x = (-B + sqrt(D)) / (2 * A);
1013                 ob2.x = (-B - sqrt(D)) / (2 * A);
1014                 ob1.y = k * (ob1.x - sensor.x) + sensor.y;
1015                 ob2.y = k * (ob2.x - sensor.x) + sensor.y;
1016
1017                 double distance1 = sensor.distanceTo(ob1);
1018                 double distance2 = sensor.distanceTo(ob2);
1019                 distance = (distance1 < distance2) ? distance1 : distance2;
1020         } else if (D == 0) {
1021                 Point ob;
1022                 ob.x = -B / (2 * A);
1023                 ob.y = k * (ob.x - sensor.x) + sensor.y;
1024                 distance = sensor.distanceTo(ob);
1025         }
1026         distance = distance + (drand48() - 0.5) * 3.0e-2;
1027         if (D < 0 || angle > atan(r / distance))
1028                 distance = sensorRange;
1029         if (distance > sensorRange)
1030                 distance = sensorRange;
1031
1032         return distance;
1033 }
1034
1035 /**
1036  * Calculation for Lidar simulation. Calculates distance that would
1037  * be returned by Lidar sensors, if there is only one obstacle (as
1038  * specified by parameters).
1039  *
1040  * @param beamnum Lidar's bean number [0..LIDAR_CLUSTER_CNT]
1041  * @param obstacle Position of the obstacle (x, y in meters).
1042  * @param obstacleSize Size (diameter) of the obstacle in meters.
1043  *
1044  * @return Distance measured by sensors in meters.
1045  */
1046 double RobomonAtlantis::distanceToObstacleLidar(const struct lidar_params lidar, int beamnum, Point obstacle, double obstacleSize)
1047 {
1048         struct robot_pos_type e = orte.est_pos_best;
1049         double sensor_a;
1050         struct sharp_pos s;
1051
1052         s.x = lidar.center_offset_m;
1053         s.y = 0.0;
1054         s.ang = index2rad(lidar, beamnum);
1055
1056         Point sensor(e.x + s.x * cos(e.phi) - s.y * sin(e.phi),
1057                      e.y + s.x * sin(e.phi) + s.y * cos(e.phi));
1058         sensor_a = e.phi + s.ang;
1059
1060         const double sensorRange = 4.0; /*[meters]*/
1061
1062         double distance, angle;
1063
1064         angle = sensor.angleTo(obstacle) - sensor_a;
1065         angle = fmod(angle, 2.0 * M_PI);
1066         if (angle > +M_PI) angle -= 2.0 * M_PI;
1067         if (angle < -M_PI) angle += 2.0 * M_PI;
1068         angle = fabs(angle);
1069         distance = sensor.distanceTo(obstacle) - obstacleSize/2.0;
1070         if (angle < atan(obstacleSize/2.0 / distance)) {
1071                 // We can see the obstackle from here.
1072                 if (angle < M_PI/2.0) {
1073                     distance = distance / cos(angle);
1074                 }
1075                 if (distance > sensorRange)
1076                         distance = sensorRange;
1077         } else {
1078                 distance = sensorRange;
1079         }
1080
1081         return distance;
1082 }
1083
1084 void RobomonAtlantis::sendStart(int plug)
1085 {
1086         orte.robot_cmd.start_condition = plug ? 0 : 1;
1087         ORTEPublicationSend(orte.publication_robot_cmd);
1088 }
1089
1090 void RobomonAtlantis::setTeamColor(int plug)
1091 {
1092         orte.robot_switches.team_color = plug ? 1 : 0;
1093         ORTEPublicationSend(orte.publication_robot_switches);
1094 }
1095
1096 void RobomonAtlantis::resetTrails()
1097 {
1098         trailRefPos->reset();
1099         trailEstPosBest->reset();
1100         trailPosIndepOdo->reset();
1101         trailOdoPos->reset();
1102 }
1103
1104 void RobomonAtlantis::showTrails(bool show)
1105 {
1106         trailRefPos->setVisible(show && robotRefPos->isVisible());
1107         trailEstPosBest->setVisible(show && robotEstPosBest->isVisible());
1108         trailPosIndepOdo->setVisible(show && robotEstPosIndepOdo->isVisible());
1109         trailOdoPos->setVisible(show && robotEstPosOdo->isVisible());
1110 }
1111
1112 void RobomonAtlantis::showShapeDetect(bool show)
1113 {
1114     hokuyoScan->showShapeDetect = show;
1115 }