src/robomon/src2/SimMcl.cpp

   1 /*
   2  * SimMcl.cpp                           07/10/31
   3  *
   4  * A simulation for testing the Monte Carlo Localization algorithm.
   5  *
   6  * Copyright: (c) 2007 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
   9  * License: GNU GPL v.2
  10  */
  11
  12 #include <QtGui>
  13 #include <QKeyEvent>
  14 #include <QCloseEvent>
  15 #include <QEvent>
  16 #include <QGLWidget>
  17
  18 #include <math.h>
  19 #include <robomath.h>
  20 #include <robodim_eb2008.h>
  21 #include <mcl.h>
  22 #include <laser-nav.h>
  23 #include <roboorte_eb2007.h>
  24
  25 #include "PlaygroundScene.h"
  26 #include "MiscGui.h"
  27 #include "SimMcl.h"
  28 #include "GlWidget.h"
  29 #include "MclPainter.h"
  30 #include "AnglesHistogramPainter.h"
  31
  32 SimMcl::SimMcl(QWidget *parent)
  33         : QWidget(parent)
  34 {
  35         QFont font;
  36         font.setPointSize(8);
  37         setFont(font);
  38
  39         debugWindowEnabled = false;
  40         hasCurrentWidget = false;
  41         glWidgetInitialized = false;
  42         widgetInitialized = false;
  43         mclPainterInitialized = false;
  44         viewObjectsInitialized = false;
  45         objectsCountChanged = false;
  46         hasOpenGL = QGLFormat::hasOpenGL();
  47
  48         createLeftLayout();
  49         createRightLayout();
  50
  51         createOrte();
  52         createActions();
  53         initPln();
  54         initPlnData();
  55         initMcl();
  56
  57         QGridLayout *mainLayout = new QGridLayout;
  58         mainLayout->addLayout(leftLayout, 0, 0);
  59         mainLayout->addLayout(rightLayout, 0, 1);
  60         setLayout(mainLayout);
  61
  62         setFocusPolicy(Qt::StrongFocus);
  63         /*grabKeyboard();*/
  64         WDBG("Youuuhouuuu!!");
  65
  66         /* some default selection */
  67         openglCheckBox->setChecked(OPENGL_DEFAULT);
  68 //      paintObjectsRadioButton->setChecked(true);
  69         paintElementsRadioButton->setChecked(true);
  70         mclAnglesRadioButton->setChecked(true);
  71         mclBlueBeaconsRadioButton->setChecked(true);
  72 //      mclPositionsRadioButton->setChecked(true);
  73         aliasingCheckBox->setChecked(ALIASING_DEFAULT);
  74
  75         /* FIXME: initialization position of the measurement, used for testing */
  76         measPos.x = PLAYGROUND_WIDTH_M / 2.0;
  77         measPos.y = PLAYGROUND_HEIGHT_M / 2.0;
  78         measPos.angle = 0;
  79 }
  80
  81 /**********************************************************************
  82  * GUI
  83  **********************************************************************/
  84 void SimMcl::createLeftLayout()
  85 {
  86         leftLayout = new QVBoxLayout();
  87
  88         /* create a debug window and enable it */
  89         createDebugGroupBox();
  90         debugWindowEnabled = true;
  91         createPlaygroundGroupBox();
  92
  93         leftLayout->addWidget(playgroundGroupBox);
  94         leftLayout->addWidget(debugGroupBox);
  95 }
  96
  97 void SimMcl::createRightLayout()
  98 {
  99         rightLayout = new QVBoxLayout();
 100         QVBoxLayout *layout = new QVBoxLayout();
 101         QGridLayout *layout1 = new QGridLayout();
 102
 103         createPositionGroupBox();
 104         createGraphicGroupBox();
 105         createMclGroupBox();
 106         createMiscGroupBox();
 107
 108         layout1->addWidget(positionGroupBox, 0, 0);
 109         layout1->addWidget(graphicGroupBox, 0, 1);
 110
 111         layout->addLayout(layout1);
 112         layout->addWidget(mclGroupBox);
 113         layout->addWidget(miscGroupBox);
 114
 115         rightLayout->addLayout(layout);
 116 }
 117
 118 void SimMcl::createPlaygroundGroupBox()
 119 {
 120         playgroundGroupBox = new QGroupBox(tr("Playground"));
 121         playgroundLayout = new QHBoxLayout();
 122
 123         playgroundScene =
 124                 new PlaygroundScene(PlaygroundScene::PLAYGROUND_EUROBOT2007);
 125         playgroundScene->setItemIndexMethod(QGraphicsScene::NoIndex);
 126         playgroundSceneView = new QGraphicsView(playgroundScene);
 127         playgroundSceneView->setMinimumWidth((int)(playgroundScene->width())+15);
 128         playgroundSceneView->setMinimumHeight((int)(playgroundScene->height())+15);
 129         playgroundSceneView->setRenderHint(QPainter::Antialiasing);
 130         playgroundSceneView->setBackgroundBrush(QColor(230, 200, 167));
 131
 132         playgroundGroupBox->setLayout(playgroundLayout);
 133 }
 134
 135 void SimMcl::createDebugGroupBox()
 136 {
 137         debugGroupBox = new QGroupBox(tr("Debug window"));
 138         QHBoxLayout *layout = new QHBoxLayout();
 139
 140         debugWindow = new QTextEdit();
 141         debugWindow->setReadOnly(true);
 142
 143         layout->addWidget(debugWindow);
 144         debugGroupBox->setLayout(layout);
 145 }
 146
 147 void SimMcl::createPositionGroupBox()
 148 {
 149         positionGroupBox = new QGroupBox(tr("Position state"));
 150         QGridLayout *layout = new QGridLayout();
 151
 152         actPosX = new QLineEdit();
 153         actPosY = new QLineEdit();
 154         actPosPhi = new QLineEdit();
 155
 156         estPosX = new QLineEdit();
 157         estPosY = new QLineEdit();
 158         estPosPhi = new QLineEdit();
 159
 160         actPosX->setReadOnly(true);
 161         actPosY->setReadOnly(true);
 162         actPosPhi->setReadOnly(true);
 163
 164         estPosX->setReadOnly(true);
 165         estPosY->setReadOnly(true);
 166         estPosPhi->setReadOnly(true);
 167
 168         layout->addWidget(MiscGui::createLabel("X"), 1, 0);
 169         layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
 170         layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
 171
 172         layout->addWidget(MiscGui::createLabel("Actual", Qt::AlignLeft), 0, 1);
 173         layout->addWidget(actPosX, 1, 1);
 174         layout->addWidget(actPosY, 2, 1);
 175         layout->addWidget(actPosPhi, 3, 1);
 176
 177         layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 0, 2);
 178         layout->addWidget(estPosX, 1, 2);
 179         layout->addWidget(estPosY, 2, 2);
 180         layout->addWidget(estPosPhi, 3, 2);
 181
 182         positionGroupBox->setLayout(layout);
 183 }
 184
 185 void SimMcl::createGraphicGroupBox()
 186 {
 187         graphicGroupBox = new QGroupBox(tr("Graphic settings"));
 188         QVBoxLayout *layout = new QVBoxLayout();
 189
 190         paintObjectsRadioButton = new QRadioButton(tr("&Object style"));
 191         paintElementsRadioButton = new QRadioButton(tr("&Element style"));
 192         openglCheckBox = new QCheckBox(tr("Open&GL"));
 193         aliasingCheckBox = new QCheckBox(tr("A&liasing"));
 194
 195         paintElementsRadioButton->setShortcut(tr("e"));
 196         paintObjectsRadioButton->setShortcut(tr("o"));
 197         openglCheckBox->setShortcut(tr("g"));
 198         aliasingCheckBox->setShortcut(tr("l"));
 199
 200         layout->addWidget(paintElementsRadioButton);
 201         layout->addWidget(paintObjectsRadioButton);
 202         layout->addWidget(openglCheckBox);
 203         layout->addWidget(aliasingCheckBox);
 204
 205         graphicGroupBox->setLayout(layout);
 206 }
 207
 208 void SimMcl::createMclGroupBox()
 209 {
 210         mclGroupBox = new QGroupBox(tr("MCL"));
 211         mclParametersGroupBox = new QGroupBox(tr("Parameters"));
 212         mclStyleGroupBox = new QGroupBox(tr("Style"));
 213         mclBeaconsGroupBox = new QGroupBox(tr("Beacons"));
 214
 215         QVBoxLayout *vlayout = new QVBoxLayout();
 216         QHBoxLayout *hlayout = new QHBoxLayout();
 217         QHBoxLayout *hlayout1 = new QHBoxLayout();
 218         QGridLayout *layout = new QGridLayout();
 219         QVBoxLayout *layout1 = new QVBoxLayout();
 220         QVBoxLayout *layout2 = new QVBoxLayout();
 221         QVBoxLayout *layout3 = new QVBoxLayout();
 222
 223         mclWidthLineEdit = new QLineEdit();
 224         mclHeightLineEdit = new QLineEdit();
 225         mclXOffLineEdit = new QLineEdit();
 226         mclYOffLineEdit = new QLineEdit();
 227         mclGenDnoiseLineEdit = new QLineEdit();
 228         mclGenAnoiseLineEdit = new QLineEdit();
 229         mclMovDnoiseLineEdit = new QLineEdit();
 230         mclMovAnoiseLineEdit = new QLineEdit();
 231         mclWMinLineEdit = new QLineEdit();
 232         mclWMaxLineEdit = new QLineEdit();
 233         mclEvalSigmaLineEdit = new QLineEdit();
 234         mclAEvalSigmaLineEdit = new QLineEdit();
 235         mclMaxavdistLineEdit = new QLineEdit();
 236         mclMaxnoisecycleLineEdit = new QLineEdit();
 237         mclCountLineEdit = new QLineEdit();
 238         mclBeaconCount = new QLineEdit();
 239
 240         layout->addWidget(MiscGui::createLabel("width"), 0, 0);
 241         layout->addWidget(MiscGui::createLabel("height"), 1, 0);
 242         layout->addWidget(MiscGui::createLabel("x offset"), 2, 0);
 243         layout->addWidget(MiscGui::createLabel("y offset"), 3, 0);
 244         layout->addWidget(MiscGui::createLabel("gen_dnoise"), 4, 0);
 245         layout->addWidget(MiscGui::createLabel("gen_anoise"), 5, 0);
 246         layout->addWidget(MiscGui::createLabel("mov_dnoise"), 6, 0);
 247         layout->addWidget(MiscGui::createLabel("mov_anoise"), 7, 0);
 248
 249         layout->addWidget(mclWidthLineEdit, 0, 1);
 250         layout->addWidget(mclHeightLineEdit, 1, 1);
 251         layout->addWidget(mclXOffLineEdit, 2, 1);
 252         layout->addWidget(mclYOffLineEdit, 3, 1);
 253         layout->addWidget(mclGenDnoiseLineEdit, 4, 1);
 254         layout->addWidget(mclGenAnoiseLineEdit, 5, 1);
 255         layout->addWidget(mclMovDnoiseLineEdit, 6, 1);
 256         layout->addWidget(mclMovAnoiseLineEdit, 7, 1);
 257
 258         layout->addWidget(MiscGui::createLabel("count"), 0, 2);
 259         layout->addWidget(MiscGui::createLabel("w_min"), 1, 2);
 260         layout->addWidget(MiscGui::createLabel("w_max"), 2, 2);
 261         layout->addWidget(MiscGui::createLabel("eval sigma"), 3, 2);
 262         layout->addWidget(MiscGui::createLabel("aeval sigma"), 4, 2);
 263         layout->addWidget(MiscGui::createLabel("maxavdist"), 5, 2);
 264         layout->addWidget(MiscGui::createLabel("maxnoisecycle"), 6, 2);
 265         layout->addWidget(MiscGui::createLabel("beacon_cnt"), 7, 2);
 266
 267         layout->addWidget(mclCountLineEdit, 0, 3);
 268         layout->addWidget(mclWMinLineEdit, 1, 3);
 269         layout->addWidget(mclWMaxLineEdit, 2, 3);
 270         layout->addWidget(mclEvalSigmaLineEdit, 3, 3);
 271         layout->addWidget(mclAEvalSigmaLineEdit, 4, 3);
 272         layout->addWidget(mclMaxavdistLineEdit, 5, 3);
 273         layout->addWidget(mclMaxnoisecycleLineEdit, 6, 3);
 274         layout->addWidget(mclBeaconCount, 7, 3);
 275
 276         mclWidthLineEdit->setMinimumWidth(50);
 277         mclCountLineEdit->setMinimumWidth(50);
 278
 279         mclWidthLineEdit->setText("3.000");
 280         mclHeightLineEdit->setText("2.100");
 281         mclXOffLineEdit->setText("0.000");
 282         mclYOffLineEdit->setText("0.000");
 283         mclGenDnoiseLineEdit->setText("0.99");
 284         mclGenAnoiseLineEdit->setText("360");
 285         mclMovDnoiseLineEdit->setText("0.01");
 286         mclMovAnoiseLineEdit->setText("0.02");
 287         mclBeaconCount->setText("4");
 288
 289         mclCountLineEdit->setText("1000");
 290         mclWMinLineEdit->setText("0.25");
 291         mclWMaxLineEdit->setText("2.0");
 292         mclEvalSigmaLineEdit->setText("160");
 293         mclAEvalSigmaLineEdit->setText("0.007");
 294         mclMaxavdistLineEdit->setText("0.15");
 295         mclMaxnoisecycleLineEdit->setText("10");
 296
 297         resetMclPushButton = new QPushButton(tr("&Reset MCL"));
 298         resetMclPushButton->setShortcut(tr("r"));
 299         resetHistogramsPushButton = new QPushButton(tr("Reset &histograms"));
 300         resetHistogramsPushButton->setShortcut(tr("h"));
 301         generateMeasurementPushButton = new QPushButton(tr("Generate &measurement"));
 302         generateMeasurementPushButton->setShortcut(tr("m"));
 303         mclPositionsRadioButton = new QRadioButton(tr("&Position"));
 304         mclPositionsRadioButton->setShortcut(tr("p"));
 305         mclAnglesRadioButton = new QRadioButton(tr("&Angle"));
 306         mclAnglesRadioButton->setShortcut(tr("a"));
 307         mclBlueBeaconsRadioButton = new QRadioButton(tr("&Blue"));
 308         mclBlueBeaconsRadioButton->setShortcut(tr("b"));
 309         mclRedBeaconsRadioButton = new QRadioButton(tr("Re&d"));
 310         mclRedBeaconsRadioButton->setShortcut(tr("d"));
 311         resetHistogramsPeriodicallyCheckBox =
 312                 new QCheckBox(tr("Re&set histograms periodically"));
 313
 314         resetHistogramsPeriodicallyCheckBox->setShortcut(tr("s"));
 315
 316         layout1->addWidget(mclPositionsRadioButton);
 317         layout1->addWidget(mclAnglesRadioButton);
 318
 319         layout3->addWidget(mclBlueBeaconsRadioButton);
 320         layout3->addWidget(mclRedBeaconsRadioButton);
 321
 322         displayCountSlider = new QSlider(Qt::Horizontal);
 323         displayCountSlider->setTracking(false);
 324         displayCountSlider->setTickPosition(QSlider::TicksBelow);
 325         displayCountSlider->setMinimum(0);
 326         displayCountSlider->setMaximum(mclCountLineEdit->text().toInt());
 327         displayCountSlider->setValue(mclCountLineEdit->text().toInt());
 328         displayCount = displayCountSlider->value();
 329
 330         mclParametersGroupBox->setLayout(layout);
 331         mclStyleGroupBox->setLayout(layout1);
 332         mclBeaconsGroupBox->setLayout(layout3);
 333
 334         hlayout1->addWidget(mclStyleGroupBox);
 335         hlayout1->addWidget(mclBeaconsGroupBox);
 336
 337         layout2->addLayout(hlayout1);
 338         layout2->addWidget(resetMclPushButton);
 339         layout2->addWidget(resetHistogramsPushButton);
 340         layout2->addWidget(generateMeasurementPushButton);
 341         layout2->addWidget(resetHistogramsPeriodicallyCheckBox);
 342         layout2->addStretch(1);
 343
 344         hlayout->addWidget(mclParametersGroupBox);
 345         hlayout->addLayout(layout2);
 346
 347         vlayout->addLayout(hlayout);
 348         vlayout->addWidget(MiscGui::createLabel("Number of particles with "
 349                                 "highest weight to be displayed"));
 350         vlayout->addWidget(displayCountSlider);
 351
 352         mclGroupBox->setLayout(vlayout);
 353 }
 354
 355 void SimMcl::createMiscGroupBox()
 356 {
 357         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 358         QVBoxLayout *vlayout = new QVBoxLayout();
 359
 360         measuredAnglesHistogram =
 361                 new AnglesHistogramPainter(measuredAnglesFreq, ANGLE_FREQ_COUNT);
 362         estimatedAnglesHistogram =
 363                 new AnglesHistogramPainter(estimatedAnglesFreq, ANGLE_FREQ_COUNT);
 364         measuredAnglesWidget = new Widget(measuredAnglesHistogram, this);
 365         estimatedAnglesWidget = new Widget(estimatedAnglesHistogram, this);
 366
 367         vlayout->addWidget(measuredAnglesWidget);
 368         vlayout->addWidget(estimatedAnglesWidget);
 369         vlayout->addStretch(1);
 370
 371         miscGroupBox->setLayout(vlayout);
 372 }
 373
 374 void SimMcl::setWidgetCurrent(QWidget *widget)
 375 {
 376         if (hasCurrentWidget) {
 377                 currentWidget->hide();
 378                 playgroundLayout->removeWidget(currentWidget);
 379         }
 380
 381         currentWidget = widget;
 382         playgroundLayout->addWidget(widget);
 383         widget->show();
 384         hasCurrentWidget = true;
 385 }
 386
 387 /**********************************************************************
 388  * GUI actions
 389  **********************************************************************/
 390 void SimMcl::createActions()
 391 {
 392         connect(openglCheckBox, SIGNAL(stateChanged(int)),
 393                         this, SLOT(enableOpenGl(int)));
 394         connect(aliasingCheckBox, SIGNAL(stateChanged(int)),
 395                         this, SLOT(setAliasing(int)));
 396         connect(paintElementsRadioButton, SIGNAL(toggled(bool)),
 397                         this, SLOT(usePaintElements(bool)));
 398         connect(paintObjectsRadioButton, SIGNAL(toggled(bool)),
 399                         this, SLOT(usePaintObjects(bool)));
 400         connect(resetMclPushButton, SIGNAL(clicked()), this, SLOT(resetMcl()));
 401         connect(resetHistogramsPushButton, SIGNAL(clicked()),
 402                         this, SLOT(resetHistograms()));
 403         connect(resetHistogramsPeriodicallyCheckBox, SIGNAL(stateChanged(int)),
 404                         this, SLOT(resetHistogramsPeriodically(int)));
 405         connect(generateMeasurementPushButton, SIGNAL(clicked()),
 406                         this, SLOT(generateMeasurement()));
 407         connect(this, SIGNAL(updateMclSignal()), this, SLOT(updateMclPainter()));
 408         connect(mclCountLineEdit, SIGNAL(textChanged(QString)),
 409                         this, SLOT(mclCountChanged(QString)));
 410         connect(displayCountSlider, SIGNAL(valueChanged(int)),
 411                         this, SLOT(setDisplayCount(int)));
 412         connect(mclPositionsRadioButton, SIGNAL(toggled(bool)),
 413                         this, SLOT(usePositionOriented(bool)));
 414         connect(mclAnglesRadioButton, SIGNAL(toggled(bool)),
 415                         this, SLOT(useAngleOriented(bool)));
 416         connect(mclBlueBeaconsRadioButton, SIGNAL(toggled(bool)),
 417                         this, SLOT(useBlueBeacons(bool)));
 418         connect(mclRedBeaconsRadioButton, SIGNAL(toggled(bool)),
 419                         this, SLOT(useRedBeacons(bool)));
 420 }
 421
 422 bool SimMcl::enableOpenGl(int state)
 423 {
 424         bool rv;
 425
 426         if (state == Qt::Unchecked) {
 427                 WDBG("OpenGL support disabled.");
 428                 rv = false;
 429         }
 430
 431         if (state == Qt::Checked) {
 432                 if (!hasOpenGL) {
 433                         WDBG("This system does not support OpenGL objects. "
 434                                 "Drawing too many samples may take a while.");
 435                         openglCheckBox->setCheckState(Qt::Unchecked);
 436                         return false;
 437                 }
 438                 WDBG("This system supports OpenGL. OpenGL support enabled.");
 439                 rv = true;
 440         }
 441
 442         if (paintElementsRadioButton->isChecked())
 443                 usePaintElements(true);
 444
 445         return rv;
 446 }
 447
 448 void SimMcl::setAliasing(int state)
 449 {
 450         if (state == Qt::Checked) {
 451                 if (widgetInitialized)
 452                         widget->setAliasing(true);
 453                 measuredAnglesWidget->setAliasing(true);
 454                 estimatedAnglesWidget->setAliasing(true);
 455                 playgroundSceneView->setRenderHint(QPainter::Antialiasing);
 456         } else {
 457                 if (widgetInitialized)
 458                         widget->setAliasing(false);
 459                 measuredAnglesWidget->setAliasing(false);
 460                 estimatedAnglesWidget->setAliasing(false);
 461                 playgroundSceneView->setRenderHint(QPainter::Antialiasing, false);
 462         }
 463         updateMclPainter();
 464 }
 465
 466 void SimMcl::initPaintElements()
 467 {
 468         mclPainter = new MclPainter(&mcl, &displayCount);
 469         mclPainter->setSize(610, 425);
 470         mclPainterInitialized = true;
 471         widgetInitialized = false;
 472         glWidgetInitialized = false;
 473 }
 474
 475 void SimMcl::usePaintElements(bool state)
 476 {
 477         /* not checked */
 478         if (!state) {
 479                 return;
 480         }
 481
 482         /* initialize the painter */
 483         if (!mclPainterInitialized)
 484                 initPaintElements();
 485
 486         /* initialize widgets if not initialized */
 487         if (openglCheckBox->isChecked() && hasOpenGL &&
 488                 !glWidgetInitialized) {
 489                 glWidget = new GLWidget(mclPainter, this);
 490                 glWidgetInitialized = true;
 491         } else if (!widgetInitialized) {
 492                 widget = new Widget(mclPainter, this);
 493                 widgetInitialized = true;
 494         }
 495
 496         /* use OpenGL if required */
 497         if (openglCheckBox->isChecked()) {
 498                 setWidgetCurrent(glWidget);
 499         } else {
 500                 setWidgetCurrent(widget);
 501         }
 502
 503         updateMclPainter();
 504
 505         WDBG("Particles are represented as graphic elements (faster).");
 506 }
 507
 508 void SimMcl::initPaintObjects()
 509 {
 510         struct mcl_particle *parts = (struct mcl_particle *)mcl.parts;
 511
 512         robots = new SmallRobot[mcl.count];
 513
 514         for (int i=0; i<mcl.count; i++) {
 515                 robots[i].setZValue(10);
 516                 robots[i].mclPart = (struct mcl_particle *)&parts[i];
 517                 robots[i].widgetSize = QSize((int)playgroundScene->width(),
 518                                         (int)playgroundScene->height());
 519                 robots[i].playgroundSize =
 520                         QSizeF(PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M);
 521                 robots[i].init();
 522                 robots[i].setParent(this);
 523                 playgroundScene->addItem(&robots[i]);
 524                 robotsList.append(&robots[i]);
 525         }
 526         viewObjectsInitialized = true;
 527 }
 528
 529 void SimMcl::usePaintObjects(bool state)
 530 {
 531         /* not checked */
 532         if (!state) {
 533                 return;
 534         }
 535
 536         if (!viewObjectsInitialized)
 537                 initPaintObjects();
 538
 539         /* use OpenGL if required */
 540         if (openglCheckBox->isChecked()) {
 541                 playgroundSceneView->setViewport(
 542                         new QGLWidget(QGLFormat(QGL::SampleBuffers)));
 543         } else {
 544                 playgroundSceneView->setViewport(0);
 545         }
 546
 547         setWidgetCurrent(playgroundSceneView);
 548
 549         updateMclPainter();
 550
 551         WDBG("Particles are represented as objects. You can move over "
 552                 "particles to see more details. Drag and drop is enabled.");
 553 }
 554
 555 void SimMcl::removePaintObjects()
 556 {
 557         /* deallocate memory and remove object from the scene */
 558         while(!robotsList.empty()) {
 559                 QGraphicsItem *item = robotsList.takeFirst();
 560                 playgroundScene->removeItem(item);
 561         }
 562 }
 563
 564 void SimMcl::resetMcl()
 565 {
 566         /* if the object count changed, reinitialize the objects */
 567         if (objectsCountChanged) {
 568                 removePaintObjects();
 569                 viewObjectsInitialized = false;
 570                 mclPainterInitialized = false;
 571         }
 572
 573         /* mcl initialization */
 574         initMcl();
 575
 576         /* number of displayed particles */
 577         displayCountSlider->setMaximum(mclCountLineEdit->text().toInt());
 578         if (mclCountLineEdit->text().toInt() < displayCountSlider->value() ||
 579                 objectsCountChanged)
 580                 displayCountSlider->setValue(mclCountLineEdit->text().toInt());
 581         displayCount = displayCountSlider->value();
 582
 583         /* display depends on the selected style */
 584         if (paintElementsRadioButton->isChecked())
 585                 usePaintElements(true);
 586         else
 587                 usePaintObjects(true);
 588
 589         objectsCountChanged = false;
 590
 591         emit updateMclSignal();
 592 }
 593
 594 void SimMcl::resetHistograms()
 595 {
 596         resetHistogram(measuredAnglesFreq);
 597         resetHistogram(estimatedAnglesFreq);
 598         countMeasuredAnglesFrequency();
 599         measuredAnglesWidget->animate();
 600         countEstimatedAnglesFrequency();
 601         estimatedAnglesWidget->animate();
 602 }
 603
 604 void SimMcl::resetHistogramsPeriodically(int state)
 605 {
 606 }
 607
 608 void SimMcl::updateMclPainter()
 609 {
 610         updatePln();
 611         if (paintElementsRadioButton->isChecked()) {
 612                 if (openglCheckBox->isChecked()) {
 613                         glWidget->animate();
 614                 } else {
 615                         widget->animate();
 616                 }
 617         } else {
 618                 int count = mcl.count-displayCountSlider->value();
 619                 for (int i=0; i<mcl.count; i++) {
 620                         if (i < count) {
 621                                 robots[i].hide();
 622                         } else {
 623                                 robots[i].show();
 624                                 robots[i].updateRobot();
 625                         }
 626                 }
 627         }
 628
 629         /* count angles frequency and display histograms */
 630         countEstimatedAnglesFrequency();
 631         estimatedAnglesWidget->animate();
 632
 633         /* update estimated position fields */
 634         struct mcl_particle part = mcl_get_pos(&mcl);
 635
 636         estPosX->setText(QString("%1").arg(part.x, 0, 'f', 3));
 637         estPosY->setText(QString("%1").arg(part.y, 0, 'f', 3));
 638         estPosPhi->setText(QString("%1 (%2 rad)")
 639                         .arg(RAD2DEG(part.angle), 0, 'f', 0)
 640                         .arg(part.angle, 0, 'f', 2));
 641 }
 642
 643 void SimMcl::mclCountChanged(QString text)
 644 {
 645         objectsCountChanged = true;
 646 }
 647
 648 void SimMcl::setDisplayCount(int value)
 649 {
 650         WDBG(QString("Number of particles to be displayed: %1").arg(value));
 651         displayCount = displayCountSlider->value();
 652         if (!objectsCountChanged)
 653                 emit updateMclSignal();
 654 }
 655
 656 void SimMcl::robotMoved()
 657 {
 658         updatePln();
 659 }
 660
 661 void SimMcl::usePositionOriented(bool state)
 662 {
 663         if (!state)
 664                 return;
 665
 666         mcl.update = mcl_update;
 667         mcl.data = measuredPosition;
 668 }
 669
 670 void SimMcl::useAngleOriented(bool state)
 671 {
 672         if (!state)
 673                 return;
 674
 675         mcl.update = mcl_update2;
 676         mcl.data = &measuredAngles;
 677 }
 678
 679 void SimMcl::useBlueBeacons(bool state)
 680 {
 681         if (!state)
 682                 return;
 683
 684         mcl.beacon_color = BEACON_BLUE;
 685 }
 686
 687 void SimMcl::useRedBeacons(bool state)
 688 {
 689         if (!state)
 690                 return;
 691
 692         mcl.beacon_color = BEACON_RED;
 693 }
 694
 695 void SimMcl::generateMeasurement()
 696 {
 697         mcl_thetas theta;
 698         measPos.x = (double)(mcl.xoff+(qrand()&0xffff)/65535.0*(double)mcl.width);
 699         measPos.y = (double)(mcl.yoff+(qrand()&0xffff)/65535.0*(double)mcl.height);
 700         measPos.angle = DEG2RAD((double)((qrand()&0xffff)/65535.0*360));
 701         /* convert generated position to angles between reflectors */
 702         mcl_pos2ang(&measPos, theta, mcl.beacon_cnt, mcl.beacon_color);
 703         measuredAngles.count = mcl.beacon_cnt;
 704         for (int i=0; i<mcl.beacon_cnt; i++)
 705                 measuredAngles.val[i] = theta[i];
 706
 707         QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
 708                         .arg(measPos.x, 0, 'f', 3)
 709                         .arg(measPos.y, 0, 'f', 3)
 710                         .arg(RAD2DEG(measPos.angle), 0, 'f', 2);
 711
 712         dbg.append(QString("  ["));
 713         for (int i=0; i<mcl.beacon_cnt-1; i++)
 714                 dbg.append(QString("%1  ")
 715                 .arg(RAD2DEG(theta[i]), 0, 'f', 2));
 716         dbg.append(QString("%1")
 717                 .arg(RAD2DEG(theta[mcl.beacon_cnt-1]), 0, 'f', 2));
 718         dbg.append(QString("]"));
 719
 720         WDBG(dbg);
 721
 722         resetHistogram(measuredAnglesFreq);
 723         countMeasuredAnglesFrequency();
 724         measuredAnglesWidget->animate();
 725 }
 726 /**********************************************************************
 727  * EVENTS
 728  **********************************************************************/
 729 bool SimMcl::event(QEvent *event)
 730 {
 731         switch (event->type()) {
 732                 case QEVENT(QEV_LASER):
 733                         emit laserDataReceivedSignal();
 734                         countMeasuredAnglesFrequency();
 735                         measuredAnglesWidget->animate();
 736                         break;
 737                 case QEVENT(QEV_MOTION_IRC):
 738                         emit motionIrcReceivedSignal();
 739                         break;
 740                 default:
 741                         if (event->type() == QEvent::Close)
 742                                 closeEvent((QCloseEvent *)event);
 743                         else if (event->type() == QEvent::KeyPress)
 744                                 keyPressEvent((QKeyEvent *)event);
 745                         else if (event->type() == QEvent::KeyRelease)
 746                                 keyReleaseEvent((QKeyEvent *)event);
 747                         else if (event->type() == QEvent::FocusIn)
 748                                 grabKeyboard();
 749                         else if (event->type() == QEvent::FocusOut)
 750                                 releaseKeyboard();
 751                         else {
 752                                 event->ignore();
 753                                 return false;
 754                         }
 755                         break;
 756         }
 757         event->accept();
 758         return true;
 759 }
 760
 761 void SimMcl::keyPressEvent(QKeyEvent *event)
 762 {
 763         if (event->isAutoRepeat()) {
 764                 event->ignore();
 765                 return;
 766         }
 767
 768         switch (event->key()) {
 769                 case Qt::Key_Up:
 770                         moveParts(0.0, 0.002, 0.0);
 771                         break;
 772                 case Qt::Key_Down:
 773                         moveParts(0.0, -0.002, 0.0);
 774                         break;
 775                 case Qt::Key_Left:
 776                         moveParts(-0.002, 0.0, 0.0);
 777                         break;
 778                 case Qt::Key_Right:
 779                         moveParts(0.002, 0.0, 0.0);
 780                         break;
 781                 /*case Qt::Key_L:
 782                         emit laserDataReceivedSignal();
 783                         countMeasuredAnglesFrequency();
 784                         measuredAnglesWidget->animate();
 785                         break;*/
 786                 default:
 787                         event->ignore();
 788                         break;
 789         }
 790         event->accept();
 791 }
 792
 793 void SimMcl::keyReleaseEvent(QKeyEvent *event)
 794 {
 795         if (event->isAutoRepeat()) {
 796                 event->ignore();
 797                 return;
 798         }
 799
 800         switch (event->key()) {
 801                 case Qt::Key_Up:
 802                 case Qt::Key_Down:
 803                 case Qt::Key_Left:
 804                 case Qt::Key_Right:
 805                         /*WDBG("arrow key released");*/
 806                         break;
 807                 default:
 808                         event->ignore();
 809                         break;
 810         }
 811         event->accept();
 812 }
 813
 814 void SimMcl::closeEvent(QCloseEvent *event)
 815 {
 816         generic_roboorte_destroy(&orte_generic);
 817         eb2008_roboorte_destroy(&orte_eb2008);
 818 }
 819
 820 /**********************************************************************
 821  * ORTE
 822  **********************************************************************/
 823 void SimMcl::createOrte()
 824 {
 825         orte_generic.strength = 12;
 826         orte_eb2008.strength = 12;
 827
 828         generic_roboorte_init(&orte_generic);
 829         eb2008_roboorte_init(&orte_eb2008);
 830
 831         generic_subscriber_motion_irc_create(&orte_generic,
 832                                 rcv_motion_irc_cb, this);
 833         eb2008_subscriber_laser_data_create(&orte_eb2008,
 834                                 receiveLaserCallBack, this);
 835
 836         /* set actions to do when we receive data from orte */
 837         connect(this, SIGNAL(laserDataReceivedSignal()),
 838                         this, SLOT(laserReceived()));
 839         connect(this, SIGNAL(motionIrcReceivedSignal()),
 840                         this, SLOT(updateOdometry()));
 841 }
 842
 843 void SimMcl::laserReceived()
 844 {
 845         unsigned int times[LAS_CNT];
 846         int cnt;
 847
 848         /*WDBG("ORTE received: laser");*/
 849
 850         measuredAngles.count = 1;
 851         measuredAngles.val[0] = TIME2ANGLE(orte_eb2008.laser_data.period,
 852                                 orte_eb2008.laser_data.measure);
 853
 854         /*QString dbg = QString("theta=%1")
 855                         .arg(RAD2DEG(measuredAngles.val[0]), 0, 'f', 3);
 856         WDBG(dbg);*/
 857
 858         countMeasuredAnglesFrequency();
 859         measuredAnglesWidget->animate();
 860
 861         updateMcl();
 862 }
 863
 864 void SimMcl::updateOdometry()
 865 {
 866         static struct motion_irc_type curr_irc;
 867         static struct motion_irc_type prev_irc;
 868         static int firstRun = 1;
 869         /* spocitat prevodovy pomer */
 870         double n = (double)(28.0 / 1.0);
 871         /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
 872         double c = (M_PI*2*ROBOT_WHEEL_RADIUS_MM/1000) / (n * 4*500.0);
 873
 874         double aktk0 = 0;
 875         double aktk1 = 0;
 876         double deltaU = 0;
 877         double dk0 = 0;
 878         double dk1 = 0;
 879         double deltAlfa = 0;
 880         static double k0 = 0;
 881         static double k1 = 0;
 882
 883 //      WDBG("ORTE received: motion_irc");
 884
 885         curr_irc = orte_generic.motion_irc;
 886
 887         if(firstRun) {
 888                 prev_irc = curr_irc;
 889                 firstRun = 0;
 890         }
 891
 892         aktk0 = (prev_irc.left - curr_irc.left) >> 8;
 893         aktk1 = (curr_irc.right - prev_irc.right) >> 8;
 894         prev_irc = curr_irc;
 895         dk0 = aktk0;
 896         dk1 = aktk1;
 897
 898         k0 = aktk0;
 899         k1 = aktk1;
 900
 901         dk0 *= c;
 902         dk1 *= c;
 903         deltaU = (dk0 + dk1) / 2;
 904
 905         /* dk1 - dk0 */
 906         deltAlfa = (dk1 - dk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);
 907
 908         est_pos.angle += deltAlfa;
 909
 910         double dx, dy;
 911
 912         dx = deltaU * cos(est_pos.angle);
 913         dy = deltaU * sin(est_pos.angle);
 914
 915         if (fabs(dx) > 0.001 || fabs(dy) > 0.001 || fabs(deltAlfa) > 0.001) {
 916                 QString dbg = QString("dx=%1 dy=%2 dphi=%3 ")
 917                                 .arg(dx, 0, 'f', 3)
 918                                 .arg(dy, 0, 'f', 3)
 919                                 .arg(deltAlfa, 0, 'f', 2);
 920                 WDBG(dbg);
 921
 922                 mcl.move(&mcl, dx, dy, deltAlfa);
 923         }
 924 }
 925
 926 /**********************************************************************
 927  * PLN - Passive Laser Navigation
 928  **********************************************************************/
 929 void SimMcl::initPln()
 930 {
 931         /* set reference points. Used to calculate angles, position.. */
 932         pln_set_points();
 933 }
 934
 935 void SimMcl::updatePln()
 936 {
 937 }
 938
 939 void SimMcl::initPlnData()
 940 {
 941         measuredAngles.count = 0;
 942         for (int i=0; i<10; i++)
 943                 measuredAngles.val[i] = 0;
 944
 945         resetHistogram(measuredAnglesFreq);
 946         resetHistogram(estimatedAnglesFreq);
 947 }
 948
 949 void SimMcl::resetHistogram(struct angles_freq *data)
 950 {
 951         for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
 952                 data[i].angle = (int)i;
 953                 data[i].frequency = 0;
 954         }
 955 }
 956
 957 void SimMcl::countMeasuredAnglesFrequency()
 958 {
 959         double a;
 960
 961         if (resetHistogramsPeriodicallyCheckBox->isChecked())
 962                 resetHistogram(measuredAnglesFreq);
 963
 964         for (int i=0; i<measuredAngles.count; i++) {
 965                 /* normalize angles */
 966                 a = fabs(fmod(RAD2DEG(measuredAngles.val[i]), 360));
 967
 968                 for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
 969                         if ((measuredAnglesFreq[i].angle) == (int)a)
 970                                 measuredAnglesFreq[i].frequency++;
 971                 }
 972         }
 973
 974         angles_freq_sort(measuredAnglesFreq, ANGLE_FREQ_COUNT);
 975 }
 976
 977 void SimMcl::countEstimatedAnglesFrequency()
 978 {
 979         struct mcl_particle *parts = (struct mcl_particle *)mcl.parts;
 980         double a;
 981         mcl_thetas theta;
 982         int _angles[ANGLE_FREQ_COUNT];
 983
 984         if (resetHistogramsPeriodicallyCheckBox->isChecked())
 985                 resetHistogram(estimatedAnglesFreq);
 986
 987         for (int i=0; i<ANGLE_FREQ_COUNT; i++)
 988                 _angles[i] = 0;
 989
 990         for (int i=0; i<displayCountSlider->value(); i++) {
 991                 mcl_pos2ang(&parts[i], theta, mcl.beacon_cnt, mcl.beacon_color);
 992                 for (int e=0; e<mcl.beacon_cnt; e++) {
 993                         /* normalize angles */
 994                         a = fabs(fmod(RAD2DEG(theta[e]), 360));
 995                         _angles[(int)a]++;
 996                 }
 997         }
 998
 999         for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
1000                 estimatedAnglesFreq[i].angle = (double)i;
1001                 estimatedAnglesFreq[i].frequency = _angles[i];
1002         }
1003
1004         angles_freq_sort(estimatedAnglesFreq, ANGLE_FREQ_COUNT);
1005 }
1006
1007 /**********************************************************************
1008  * MCL
1009  **********************************************************************/
1010 void SimMcl::initMcl()
1011 {
1012         /* MCL initialization */
1013         mcl.xoff = mclXOffLineEdit->text().toFloat();/* in meters */
1014         mcl.yoff = mclYOffLineEdit->text().toFloat();/* in meters */
1015         mcl.width = mclWidthLineEdit->text().toFloat();/* in meters */
1016         mcl.height = mclHeightLineEdit->text().toFloat();/* in meters */
1017         /* the noises */
1018         mcl.gen_dnoise = mclGenDnoiseLineEdit->text().toFloat();
1019         mcl.gen_anoise = mclGenAnoiseLineEdit->text().toFloat();
1020         mcl.mov_dnoise = mclMovDnoiseLineEdit->text().toFloat();
1021         mcl.mov_anoise = mclMovAnoiseLineEdit->text().toFloat();
1022         mcl.w_min = mclWMinLineEdit->text().toFloat();
1023         mcl.w_max = mclWMaxLineEdit->text().toFloat();
1024         mcl.eval_sigma = mclEvalSigmaLineEdit->text().toFloat();
1025         mcl.aeval_sigma = mclAEvalSigmaLineEdit->text().toFloat();
1026         mcl.maxavdist = mclMaxavdistLineEdit->text().toFloat();
1027         /* bad cycles before reseting */
1028         mcl.maxnoisecycle = mclMaxnoisecycleLineEdit->text().toInt();
1029         mcl.beacon_cnt = mclBeaconCount->text().toInt();
1030         if (mclRedBeaconsRadioButton->isChecked()) {
1031                 mcl.beacon_color = BEACON_RED;
1032         } else {
1033                 mcl.beacon_color = BEACON_BLUE;
1034         }
1035
1036         /* amount of particles */
1037         mcl.count = mclCountLineEdit->text().toInt();
1038         if (objectsCountChanged || !viewObjectsInitialized
1039                 || !mclPainterInitialized) {
1040                 mcl.parts = (struct mcl_particle *)
1041                         malloc(sizeof(struct mcl_particle)*mcl.count);
1042         }
1043
1044         /* phases of the MCL */
1045         mcl.init = mcl_init;
1046         mcl.move = mcl_move;
1047         /* style of particles evaluation */
1048         if (mclPositionsRadioButton->isChecked()) {
1049                 /* position evaluation oriented update */
1050                 mcl.update = mcl_update;
1051                 mcl.data = measuredPosition;
1052         } else {
1053                 /* angles evalution oriented update */
1054                 mcl.update = mcl_update2;
1055                 mcl.data = &measuredAngles;
1056         }
1057         mcl.normalize = mcl_normalize;
1058         mcl.sort = mcl_partsort;
1059         mcl.resample = mcl_resample;
1060
1061         /* cycles of enumeration */
1062         mcl.cycle = 0;
1063         mcl.noisecycle = 0;
1064
1065         /* change flag */
1066         mcl.flag = MCL_RUN;
1067
1068         est_pos.x = 0;
1069         est_pos.y = 0;
1070         est_pos.angle = 0;
1071
1072         /* generate particles with noises */
1073         mcl.init(&mcl);
1074 }
1075
1076 void SimMcl::moveParts(double dx, double dy, double dangle)
1077 {
1078         int i=0;
1079         mcl_thetas theta;
1080
1081         mcl.cycle++;
1082         /*WDBG(QString("mcl cycle = %1").arg(mcl.cycle));*/
1083
1084         /* FIXME */
1085         /* normally, the [x,y,angle] are values measured by sensors. In this
1086          * example, the values are quantified as the movement without noises*/
1087         for (i=0; i<5; i++) {
1088                 measPos.x += dx;
1089                 measPos.y += dy;
1090                 measPos.angle += dangle;
1091
1092                 measuredPosition[0] = measPos.x;
1093                 measuredPosition[1] = measPos.y;
1094                 measuredPosition[2] = measPos.angle;
1095
1096                 mcl_pos2ang(&measPos, theta, mcl.beacon_cnt, mcl.beacon_color);
1097                 measuredAngles.count = mcl.beacon_cnt;
1098                 for (int i=0; i<mcl.beacon_cnt; i++)
1099                         measuredAngles.val[i] = theta[i];
1100                 QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
1101                                 .arg(measPos.x, 0, 'f', 3)
1102                                 .arg(measPos.y, 0, 'f', 3)
1103                                 .arg(RAD2DEG(measPos.angle), 0, 'f', 2);
1104
1105                 dbg.append(QString("  ["));
1106                 for (int i=0; i<mcl.beacon_cnt-1; i++)
1107                         dbg.append(QString("%1  ")
1108                         .arg(RAD2DEG(theta[i]), 0, 'f', 2));
1109                 dbg.append(QString("%1")
1110                         .arg(RAD2DEG(theta[mcl.beacon_cnt-1]), 0, 'f', 2));
1111                 dbg.append(QString("]"));
1112
1113                 WDBG(dbg);
1114                 countMeasuredAnglesFrequency();
1115                 measuredAnglesWidget->animate();
1116
1117         /* MCL */
1118                 mcl.move(&mcl, dx, dy, dangle);
1119         }
1120         /* /FIXME */
1121         mcl.update(&mcl, mcl.data);
1122         mcl.normalize(&mcl);
1123         mcl.sort(&mcl);
1124         mcl.resample(&mcl);
1125
1126         /* update MCL painting */
1127         emit updateMclSignal();
1128 }
1129
1130 void SimMcl::updateMcl()
1131 {
1132         mcl.cycle++;
1133
1134         /* MCL */
1135 //      mcl.move(&mcl, 0.00, 0.00, 0.00);
1136         mcl.update(&mcl, mcl.data);
1137         mcl.normalize(&mcl);
1138         mcl.sort(&mcl);
1139         mcl.resample(&mcl);
1140
1141         est_pos = mcl_get_pos(&mcl);
1142
1143         /* update MCL painting */
1144         emit updateMclSignal();
1145 }