src/robomon/RobomonAtlantis.cpp

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