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