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         updateMclCheckBox =
 314                 new QCheckBox(tr("Update MCL"));
 315
 316         resetHistogramsPeriodicallyCheckBox->setShortcut(tr("s"));
 317         updateMclCheckBox->setShortcut(tr("u"));
 318
 319         layout1->addWidget(mclPositionsRadioButton);
 320         layout1->addWidget(mclAnglesRadioButton);
 321
 322         layout3->addWidget(mclBlueBeaconsRadioButton);
 323         layout3->addWidget(mclRedBeaconsRadioButton);
 324
 325         displayCountSlider = new QSlider(Qt::Horizontal);
 326         displayCountSlider->setTracking(false);
 327         displayCountSlider->setTickPosition(QSlider::TicksBelow);
 328         displayCountSlider->setMinimum(0);
 329         displayCountSlider->setMaximum(mclCountLineEdit->text().toInt());
 330         displayCountSlider->setValue(mclCountLineEdit->text().toInt());
 331         displayCount = displayCountSlider->value();
 332
 333         mclParametersGroupBox->setLayout(layout);
 334         mclStyleGroupBox->setLayout(layout1);
 335         mclBeaconsGroupBox->setLayout(layout3);
 336
 337         hlayout1->addWidget(mclStyleGroupBox);
 338         hlayout1->addWidget(mclBeaconsGroupBox);
 339
 340         layout2->addLayout(hlayout1);
 341         layout2->addWidget(resetMclPushButton);
 342         layout2->addWidget(resetHistogramsPushButton);
 343         layout2->addWidget(generateMeasurementPushButton);
 344         layout2->addWidget(resetHistogramsPeriodicallyCheckBox);
 345         layout2->addWidget(updateMclCheckBox);
 346         layout2->addStretch(1);
 347
 348         hlayout->addWidget(mclParametersGroupBox);
 349         hlayout->addLayout(layout2);
 350
 351         vlayout->addLayout(hlayout);
 352         vlayout->addWidget(MiscGui::createLabel("Number of particles with "
 353                                 "highest weight to be displayed"));
 354         vlayout->addWidget(displayCountSlider);
 355
 356         mclGroupBox->setLayout(vlayout);
 357 }
 358
 359 void SimMcl::createMiscGroupBox()
 360 {
 361         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 362         QVBoxLayout *vlayout = new QVBoxLayout();
 363
 364         measuredAnglesHistogram =
 365                 new AnglesHistogramPainter(measuredAnglesFreq, ANGLE_FREQ_COUNT);
 366         estimatedAnglesHistogram =
 367                 new AnglesHistogramPainter(estimatedAnglesFreq, ANGLE_FREQ_COUNT);
 368         measuredAnglesWidget = new Widget(measuredAnglesHistogram, this);
 369         estimatedAnglesWidget = new Widget(estimatedAnglesHistogram, this);
 370
 371         vlayout->addWidget(measuredAnglesWidget);
 372         vlayout->addWidget(estimatedAnglesWidget);
 373         vlayout->addStretch(1);
 374
 375         miscGroupBox->setLayout(vlayout);
 376 }
 377
 378 void SimMcl::setWidgetCurrent(QWidget *widget)
 379 {
 380         if (hasCurrentWidget) {
 381                 currentWidget->hide();
 382                 playgroundLayout->removeWidget(currentWidget);
 383         }
 384
 385         currentWidget = widget;
 386         playgroundLayout->addWidget(widget);
 387         widget->show();
 388         hasCurrentWidget = true;
 389 }
 390
 391 /**********************************************************************
 392  * GUI actions
 393  **********************************************************************/
 394 void SimMcl::createActions()
 395 {
 396         connect(openglCheckBox, SIGNAL(stateChanged(int)),
 397                         this, SLOT(enableOpenGl(int)));
 398         connect(aliasingCheckBox, SIGNAL(stateChanged(int)),
 399                         this, SLOT(setAliasing(int)));
 400         connect(paintElementsRadioButton, SIGNAL(toggled(bool)),
 401                         this, SLOT(usePaintElements(bool)));
 402         connect(paintObjectsRadioButton, SIGNAL(toggled(bool)),
 403                         this, SLOT(usePaintObjects(bool)));
 404         connect(resetMclPushButton, SIGNAL(clicked()), this, SLOT(resetMcl()));
 405         connect(resetHistogramsPushButton, SIGNAL(clicked()),
 406                         this, SLOT(resetHistograms()));
 407         connect(resetHistogramsPeriodicallyCheckBox, SIGNAL(stateChanged(int)),
 408                         this, SLOT(resetHistogramsPeriodically(int)));
 409         connect(generateMeasurementPushButton, SIGNAL(clicked()),
 410                         this, SLOT(generateMeasurement()));
 411         connect(this, SIGNAL(updateMclSignal()), this, SLOT(updateMclPainter()));
 412         connect(mclCountLineEdit, SIGNAL(textChanged(QString)),
 413                         this, SLOT(mclCountChanged(QString)));
 414         connect(displayCountSlider, SIGNAL(valueChanged(int)),
 415                         this, SLOT(setDisplayCount(int)));
 416         connect(mclPositionsRadioButton, SIGNAL(toggled(bool)),
 417                         this, SLOT(usePositionOriented(bool)));
 418         connect(mclAnglesRadioButton, SIGNAL(toggled(bool)),
 419                         this, SLOT(useAngleOriented(bool)));
 420         connect(mclBlueBeaconsRadioButton, SIGNAL(toggled(bool)),
 421                         this, SLOT(useBlueBeacons(bool)));
 422         connect(mclRedBeaconsRadioButton, SIGNAL(toggled(bool)),
 423                         this, SLOT(useRedBeacons(bool)));
 424 }
 425
 426 bool SimMcl::enableOpenGl(int state)
 427 {
 428         bool rv;
 429
 430         if (state == Qt::Unchecked) {
 431                 WDBG("OpenGL support disabled.");
 432                 rv = false;
 433         }
 434
 435         if (state == Qt::Checked) {
 436                 if (!hasOpenGL) {
 437                         WDBG("This system does not support OpenGL objects. "
 438                                 "Drawing too many samples may take a while.");
 439                         openglCheckBox->setCheckState(Qt::Unchecked);
 440                         return false;
 441                 }
 442                 WDBG("This system supports OpenGL. OpenGL support enabled.");
 443                 rv = true;
 444         }
 445
 446         if (paintElementsRadioButton->isChecked())
 447                 usePaintElements(true);
 448
 449         return rv;
 450 }
 451
 452 void SimMcl::setAliasing(int state)
 453 {
 454         if (state == Qt::Checked) {
 455                 if (widgetInitialized)
 456                         widget->setAliasing(true);
 457                 measuredAnglesWidget->setAliasing(true);
 458                 estimatedAnglesWidget->setAliasing(true);
 459                 playgroundSceneView->setRenderHint(QPainter::Antialiasing);
 460         } else {
 461                 if (widgetInitialized)
 462                         widget->setAliasing(false);
 463                 measuredAnglesWidget->setAliasing(false);
 464                 estimatedAnglesWidget->setAliasing(false);
 465                 playgroundSceneView->setRenderHint(QPainter::Antialiasing, false);
 466         }
 467         updateMclPainter();
 468 }
 469
 470 void SimMcl::initPaintElements()
 471 {
 472         mclPainter = new MclPainter(&mcl, &displayCount);
 473         mclPainter->setSize(610, 425);
 474         mclPainterInitialized = true;
 475         widgetInitialized = false;
 476         glWidgetInitialized = false;
 477 }
 478
 479 void SimMcl::usePaintElements(bool state)
 480 {
 481         /* not checked */
 482         if (!state) {
 483                 return;
 484         }
 485
 486         /* initialize the painter */
 487         if (!mclPainterInitialized)
 488                 initPaintElements();
 489
 490         /* initialize widgets if not initialized */
 491         if (openglCheckBox->isChecked() && hasOpenGL &&
 492                 !glWidgetInitialized) {
 493                 glWidget = new GLWidget(mclPainter, this);
 494                 glWidgetInitialized = true;
 495         } else if (!widgetInitialized) {
 496                 widget = new Widget(mclPainter, this);
 497                 widgetInitialized = true;
 498         }
 499
 500         /* use OpenGL if required */
 501         if (openglCheckBox->isChecked()) {
 502                 setWidgetCurrent(glWidget);
 503         } else {
 504                 setWidgetCurrent(widget);
 505         }
 506
 507         updateMclPainter();
 508
 509         WDBG("Particles are represented as graphic elements (faster).");
 510 }
 511
 512 void SimMcl::initPaintObjects()
 513 {
 514         struct mcl_particle *parts = (struct mcl_particle *)mcl.parts;
 515
 516         robots = new SmallRobot[mcl.count];
 517
 518         for (int i=0; i<mcl.count; i++) {
 519                 robots[i].setZValue(10);
 520                 robots[i].mclPart = (struct mcl_particle *)&parts[i];
 521                 robots[i].widgetSize = QSize((int)playgroundScene->width(),
 522                                         (int)playgroundScene->height());
 523                 robots[i].playgroundSize =
 524                         QSizeF(PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M);
 525                 robots[i].init();
 526                 robots[i].setParent(this);
 527                 playgroundScene->addItem(&robots[i]);
 528                 robotsList.append(&robots[i]);
 529         }
 530         viewObjectsInitialized = true;
 531 }
 532
 533 void SimMcl::usePaintObjects(bool state)
 534 {
 535         /* not checked */
 536         if (!state) {
 537                 return;
 538         }
 539
 540         if (!viewObjectsInitialized)
 541                 initPaintObjects();
 542
 543         /* use OpenGL if required */
 544         if (openglCheckBox->isChecked()) {
 545                 playgroundSceneView->setViewport(
 546                         new QGLWidget(QGLFormat(QGL::SampleBuffers)));
 547         } else {
 548                 playgroundSceneView->setViewport(0);
 549         }
 550
 551         setWidgetCurrent(playgroundSceneView);
 552
 553         updateMclPainter();
 554
 555         WDBG("Particles are represented as objects. You can move over "
 556                 "particles to see more details. Drag and drop is enabled.");
 557 }
 558
 559 void SimMcl::removePaintObjects()
 560 {
 561         /* deallocate memory and remove object from the scene */
 562         while(!robotsList.empty()) {
 563                 QGraphicsItem *item = robotsList.takeFirst();
 564                 playgroundScene->removeItem(item);
 565         }
 566 }
 567
 568 void SimMcl::resetMcl()
 569 {
 570         /* if the object count changed, reinitialize the objects */
 571         if (objectsCountChanged) {
 572                 removePaintObjects();
 573                 viewObjectsInitialized = false;
 574                 mclPainterInitialized = false;
 575         }
 576
 577         /* mcl initialization */
 578         initMcl();
 579
 580         /* number of displayed particles */
 581         displayCountSlider->setMaximum(mclCountLineEdit->text().toInt());
 582         if (mclCountLineEdit->text().toInt() < displayCountSlider->value() ||
 583                 objectsCountChanged)
 584                 displayCountSlider->setValue(mclCountLineEdit->text().toInt());
 585         displayCount = displayCountSlider->value();
 586
 587         /* display depends on the selected style */
 588         if (paintElementsRadioButton->isChecked())
 589                 usePaintElements(true);
 590         else
 591                 usePaintObjects(true);
 592
 593         objectsCountChanged = false;
 594
 595         emit updateMclSignal();
 596 }
 597
 598 void SimMcl::resetHistograms()
 599 {
 600         resetHistogram(measuredAnglesFreq);
 601         resetHistogram(estimatedAnglesFreq);
 602         countMeasuredAnglesFrequency();
 603         measuredAnglesWidget->animate();
 604         countEstimatedAnglesFrequency();
 605         estimatedAnglesWidget->animate();
 606 }
 607
 608 void SimMcl::resetHistogramsPeriodically(int state)
 609 {
 610 }
 611
 612 void SimMcl::updateMclPainter()
 613 {
 614         updatePln();
 615         if (paintElementsRadioButton->isChecked()) {
 616                 if (openglCheckBox->isChecked()) {
 617                         glWidget->animate();
 618                 } else {
 619                         widget->animate();
 620                 }
 621         } else {
 622                 int count = mcl.count-displayCountSlider->value();
 623                 for (int i=0; i<mcl.count; i++) {
 624                         if (i < count) {
 625                                 robots[i].hide();
 626                         } else {
 627                                 robots[i].show();
 628                                 robots[i].updateRobot();
 629                         }
 630                 }
 631         }
 632
 633         /* count angles frequency and display histograms */
 634         countEstimatedAnglesFrequency();
 635         estimatedAnglesWidget->animate();
 636
 637         /* update estimated position fields */
 638         struct mcl_particle part = mcl_get_pos(&mcl);
 639
 640         estPosX->setText(QString("%1").arg(part.x, 0, 'f', 3));
 641         estPosY->setText(QString("%1").arg(part.y, 0, 'f', 3));
 642         estPosPhi->setText(QString("%1 (%2 rad)")
 643                         .arg(RAD2DEG(part.angle), 0, 'f', 0)
 644                         .arg(part.angle, 0, 'f', 2));
 645 }
 646
 647 void SimMcl::mclCountChanged(QString text)
 648 {
 649         objectsCountChanged = true;
 650 }
 651
 652 void SimMcl::setDisplayCount(int value)
 653 {
 654         WDBG(QString("Number of particles to be displayed: %1").arg(value));
 655         displayCount = displayCountSlider->value();
 656         if (!objectsCountChanged)
 657                 emit updateMclSignal();
 658 }
 659
 660 void SimMcl::robotMoved()
 661 {
 662         updatePln();
 663 }
 664
 665 void SimMcl::usePositionOriented(bool state)
 666 {
 667         if (!state)
 668                 return;
 669
 670         mcl.update = mcl_update;
 671         mcl.data = measuredPosition;
 672 }
 673
 674 void SimMcl::useAngleOriented(bool state)
 675 {
 676         if (!state)
 677                 return;
 678
 679         mcl.update = mcl_update2;
 680         mcl.data = &measuredAngles;
 681 }
 682
 683 void SimMcl::useBlueBeacons(bool state)
 684 {
 685         if (!state)
 686                 return;
 687
 688         mcl.beacon_color = BEACON_BLUE;
 689 }
 690
 691 void SimMcl::useRedBeacons(bool state)
 692 {
 693         if (!state)
 694                 return;
 695
 696         mcl.beacon_color = BEACON_RED;
 697 }
 698
 699 void SimMcl::generateMeasurement()
 700 {
 701         mcl_thetas theta;
 702         measPos.x = (double)(mcl.xoff+(qrand()&0xffff)/65535.0*(double)mcl.width);
 703         measPos.y = (double)(mcl.yoff+(qrand()&0xffff)/65535.0*(double)mcl.height);
 704         measPos.angle = DEG2RAD((double)((qrand()&0xffff)/65535.0*360));
 705         /* convert generated position to angles between reflectors */
 706         mcl_pos2ang(&measPos, theta, mcl.beacon_cnt, mcl.beacon_color);
 707         measuredAngles.count = mcl.beacon_cnt;
 708         for (int i=0; i<mcl.beacon_cnt; i++)
 709                 measuredAngles.val[i] = theta[i];
 710
 711         QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
 712                         .arg(measPos.x, 0, 'f', 3)
 713                         .arg(measPos.y, 0, 'f', 3)
 714                         .arg(RAD2DEG(measPos.angle), 0, 'f', 2);
 715
 716         dbg.append(QString("  ["));
 717         for (int i=0; i<mcl.beacon_cnt-1; i++)
 718                 dbg.append(QString("%1  ")
 719                 .arg(RAD2DEG(theta[i]), 0, 'f', 2));
 720         dbg.append(QString("%1")
 721                 .arg(RAD2DEG(theta[mcl.beacon_cnt-1]), 0, 'f', 2));
 722         dbg.append(QString("]"));
 723
 724         WDBG(dbg);
 725
 726         resetHistogram(measuredAnglesFreq);
 727         countMeasuredAnglesFrequency();
 728         measuredAnglesWidget->animate();
 729 }
 730 /**********************************************************************
 731  * EVENTS
 732  **********************************************************************/
 733 bool SimMcl::event(QEvent *event)
 734 {
 735         switch (event->type()) {
 736                 case QEVENT(QEV_LASER):
 737                         emit laserDataReceivedSignal();
 738                         countMeasuredAnglesFrequency();
 739                         measuredAnglesWidget->animate();
 740                         break;
 741                 case QEVENT(QEV_MOTION_IRC):
 742                         emit motionIrcReceivedSignal();
 743                         break;
 744                 default:
 745                         if (event->type() == QEvent::Close)
 746                                 closeEvent((QCloseEvent *)event);
 747                         else if (event->type() == QEvent::KeyPress)
 748                                 keyPressEvent((QKeyEvent *)event);
 749                         else if (event->type() == QEvent::KeyRelease)
 750                                 keyReleaseEvent((QKeyEvent *)event);
 751                         else if (event->type() == QEvent::FocusIn)
 752                                 grabKeyboard();
 753                         else if (event->type() == QEvent::FocusOut)
 754                                 releaseKeyboard();
 755                         else {
 756                                 event->ignore();
 757                                 return false;
 758                         }
 759                         break;
 760         }
 761         event->accept();
 762         return true;
 763 }
 764
 765 void SimMcl::keyPressEvent(QKeyEvent *event)
 766 {
 767         if (event->isAutoRepeat()) {
 768                 event->ignore();
 769                 return;
 770         }
 771
 772         switch (event->key()) {
 773                 case Qt::Key_Up:
 774                         moveParts(0.0, 0.002, 0.0);
 775                         break;
 776                 case Qt::Key_Down:
 777                         moveParts(0.0, -0.002, 0.0);
 778                         break;
 779                 case Qt::Key_Left:
 780                         moveParts(-0.002, 0.0, 0.0);
 781                         break;
 782                 case Qt::Key_Right:
 783                         moveParts(0.002, 0.0, 0.0);
 784                         break;
 785                 /*case Qt::Key_L:
 786                         emit laserDataReceivedSignal();
 787                         countMeasuredAnglesFrequency();
 788                         measuredAnglesWidget->animate();
 789                         break;*/
 790                 default:
 791                         event->ignore();
 792                         break;
 793         }
 794         event->accept();
 795 }
 796
 797 void SimMcl::keyReleaseEvent(QKeyEvent *event)
 798 {
 799         if (event->isAutoRepeat()) {
 800                 event->ignore();
 801                 return;
 802         }
 803
 804         switch (event->key()) {
 805                 case Qt::Key_Up:
 806                 case Qt::Key_Down:
 807                 case Qt::Key_Left:
 808                 case Qt::Key_Right:
 809                         /*WDBG("arrow key released");*/
 810                         break;
 811                 default:
 812                         event->ignore();
 813                         break;
 814         }
 815         event->accept();
 816 }
 817
 818 void SimMcl::closeEvent(QCloseEvent *event)
 819 {
 820         generic_roboorte_destroy(&orte_generic);
 821         eb2008_roboorte_destroy(&orte_eb2008);
 822 }
 823
 824 /**********************************************************************
 825  * ORTE
 826  **********************************************************************/
 827 void SimMcl::createOrte()
 828 {
 829         orte_generic.strength = 12;
 830         orte_eb2008.strength = 12;
 831
 832         generic_roboorte_init(&orte_generic);
 833         eb2008_roboorte_init(&orte_eb2008);
 834
 835         generic_subscriber_motion_irc_create(&orte_generic,
 836                                 rcv_motion_irc_cb, this);
 837         eb2008_subscriber_laser_data_create(&orte_eb2008,
 838                                 receiveLaserCallBack, this);
 839
 840         /* set actions to do when we receive data from orte */
 841         connect(this, SIGNAL(laserDataReceivedSignal()),
 842                         this, SLOT(laserReceived()));
 843         connect(this, SIGNAL(motionIrcReceivedSignal()),
 844                         this, SLOT(updateOdometry()));
 845 }
 846
 847 void SimMcl::laserReceived()
 848 {
 849         unsigned int times[LAS_CNT];
 850         int cnt;
 851
 852         /*WDBG("ORTE received: laser");*/
 853
 854         measuredAngles.count = 1;
 855         measuredAngles.val[0] = TIME2ANGLE(orte_eb2008.laser_data.period,
 856                                 orte_eb2008.laser_data.measure);
 857
 858         /*QString dbg = QString("theta=%1")
 859                         .arg(RAD2DEG(measuredAngles.val[0]), 0, 'f', 3);
 860         WDBG(dbg);*/
 861
 862         countMeasuredAnglesFrequency();
 863         measuredAnglesWidget->animate();
 864
 865         updateMcl();
 866 }
 867
 868 void SimMcl::updateOdometry()
 869 {
 870         static struct motion_irc_type curr_irc;
 871         static struct motion_irc_type prev_irc;
 872         static int firstRun = 1;
 873         /* spocitat prevodovy pomer */
 874         double n = (double)(28.0 / 1.0);
 875         /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
 876         double c = (M_PI*2*ROBOT_WHEEL_RADIUS_MM/1000) / (n * 4*500.0);
 877
 878         double aktk0 = 0;
 879         double aktk1 = 0;
 880         double deltaU = 0;
 881         double dk0 = 0;
 882         double dk1 = 0;
 883         double deltAlfa = 0;
 884         static double k0 = 0;
 885         static double k1 = 0;
 886
 887 //      WDBG("ORTE received: motion_irc");
 888
 889         curr_irc = orte_generic.motion_irc;
 890
 891         if(firstRun) {
 892                 prev_irc = curr_irc;
 893                 firstRun = 0;
 894         }
 895
 896         aktk0 = (prev_irc.left - curr_irc.left) >> 8;
 897         aktk1 = (curr_irc.right - prev_irc.right) >> 8;
 898         prev_irc = curr_irc;
 899         dk0 = aktk0;
 900         dk1 = aktk1;
 901
 902         k0 = aktk0;
 903         k1 = aktk1;
 904
 905         dk0 *= c;
 906         dk1 *= c;
 907         deltaU = (dk0 + dk1) / 2;
 908
 909         /* dk1 - dk0 */
 910         deltAlfa = (dk1 - dk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);
 911
 912         est_pos.angle += deltAlfa;
 913
 914         double dx, dy;
 915         static double adx = 0, ady = 0, adalpha = 0;
 916
 917         dx = deltaU * cos(est_pos.angle);
 918         dy = deltaU * sin(est_pos.angle);
 919
 920         dy = dx*cos(-est_pos.angle) - dy*sin(-est_pos.angle);
 921         dy = dy*cos(-est_pos.angle) + dx*sin(-est_pos.angle);
 922
 923         adx += dx;
 924         ady += dy;
 925         adalpha += deltAlfa;
 926
 927         if (fabs(dx) > 0.001 || fabs(dy) > 0.001 || fabs(deltAlfa) > 0.001) {
 928                 QString dbg = QString("dx=%1 dy=%2 dphi=%3 ")
 929                                 .arg(adx, 0, 'f', 3)
 930                                 .arg(ady, 0, 'f', 3)
 931                                 .arg(adalpha, 0, 'f', 2);
 932                 WDBG(dbg);
 933
 934                 mcl.move(&mcl, dx, dy, deltAlfa);
 935         }
 936 }
 937
 938 /**********************************************************************
 939  * PLN - Passive Laser Navigation
 940  **********************************************************************/
 941 void SimMcl::initPln()
 942 {
 943         /* set reference points. Used to calculate angles, position.. */
 944         pln_set_points();
 945 }
 946
 947 void SimMcl::updatePln()
 948 {
 949 }
 950
 951 void SimMcl::initPlnData()
 952 {
 953         measuredAngles.count = 0;
 954         for (int i=0; i<10; i++)
 955                 measuredAngles.val[i] = 0;
 956
 957         resetHistogram(measuredAnglesFreq);
 958         resetHistogram(estimatedAnglesFreq);
 959 }
 960
 961 void SimMcl::resetHistogram(struct angles_freq *data)
 962 {
 963         for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
 964                 data[i].angle = (int)i;
 965                 data[i].frequency = 0;
 966         }
 967 }
 968
 969 void SimMcl::countMeasuredAnglesFrequency()
 970 {
 971         double a;
 972
 973         if (resetHistogramsPeriodicallyCheckBox->isChecked())
 974                 resetHistogram(measuredAnglesFreq);
 975
 976         for (int i=0; i<measuredAngles.count; i++) {
 977                 /* normalize angles */
 978                 a = fabs(fmod(RAD2DEG(measuredAngles.val[i]), 360));
 979
 980                 for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
 981                         if ((measuredAnglesFreq[i].angle) == (int)a)
 982                                 measuredAnglesFreq[i].frequency++;
 983                 }
 984         }
 985
 986         angles_freq_sort(measuredAnglesFreq, ANGLE_FREQ_COUNT);
 987 }
 988
 989 void SimMcl::countEstimatedAnglesFrequency()
 990 {
 991         struct mcl_particle *parts = (struct mcl_particle *)mcl.parts;
 992         double a;
 993         mcl_thetas theta;
 994         int _angles[ANGLE_FREQ_COUNT];
 995
 996         if (resetHistogramsPeriodicallyCheckBox->isChecked())
 997                 resetHistogram(estimatedAnglesFreq);
 998
 999         for (int i=0; i<ANGLE_FREQ_COUNT; i++)
1000                 _angles[i] = 0;
1001
1002         for (int i=0; i<displayCountSlider->value(); i++) {
1003                 mcl_pos2ang(&parts[i], theta, mcl.beacon_cnt, mcl.beacon_color);
1004                 for (int e=0; e<mcl.beacon_cnt; e++) {
1005                         /* normalize angles */
1006                         a = fabs(fmod(RAD2DEG(theta[e]), 360));
1007                         _angles[(int)a]++;
1008                 }
1009         }
1010
1011         for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
1012                 estimatedAnglesFreq[i].angle = (double)i;
1013                 estimatedAnglesFreq[i].frequency = _angles[i];
1014         }
1015
1016         angles_freq_sort(estimatedAnglesFreq, ANGLE_FREQ_COUNT);
1017 }
1018
1019 /**********************************************************************
1020  * MCL
1021  **********************************************************************/
1022 void SimMcl::initMcl()
1023 {
1024         /* MCL initialization */
1025         mcl.xoff = mclXOffLineEdit->text().toFloat();/* in meters */
1026         mcl.yoff = mclYOffLineEdit->text().toFloat();/* in meters */
1027         mcl.width = mclWidthLineEdit->text().toFloat();/* in meters */
1028         mcl.height = mclHeightLineEdit->text().toFloat();/* in meters */
1029         /* the noises */
1030         mcl.gen_dnoise = mclGenDnoiseLineEdit->text().toFloat();
1031         mcl.gen_anoise = mclGenAnoiseLineEdit->text().toFloat();
1032         mcl.mov_dnoise = mclMovDnoiseLineEdit->text().toFloat();
1033         mcl.mov_anoise = mclMovAnoiseLineEdit->text().toFloat();
1034         mcl.w_min = mclWMinLineEdit->text().toFloat();
1035         mcl.w_max = mclWMaxLineEdit->text().toFloat();
1036         mcl.eval_sigma = mclEvalSigmaLineEdit->text().toFloat();
1037         mcl.aeval_sigma = mclAEvalSigmaLineEdit->text().toFloat();
1038         mcl.maxavdist = mclMaxavdistLineEdit->text().toFloat();
1039         /* bad cycles before reseting */
1040         mcl.maxnoisecycle = mclMaxnoisecycleLineEdit->text().toInt();
1041         mcl.beacon_cnt = mclBeaconCount->text().toInt();
1042         if (mclRedBeaconsRadioButton->isChecked()) {
1043                 mcl.beacon_color = BEACON_RED;
1044         } else {
1045                 mcl.beacon_color = BEACON_BLUE;
1046         }
1047
1048         /* amount of particles */
1049         mcl.count = mclCountLineEdit->text().toInt();
1050         if (objectsCountChanged || !viewObjectsInitialized
1051                 || !mclPainterInitialized) {
1052                 mcl.parts = (struct mcl_particle *)
1053                         malloc(sizeof(struct mcl_particle)*mcl.count);
1054         }
1055
1056         /* phases of the MCL */
1057         mcl.init = mcl_init;
1058         mcl.move = mcl_move;
1059         /* style of particles evaluation */
1060         if (mclPositionsRadioButton->isChecked()) {
1061                 /* position evaluation oriented update */
1062                 mcl.update = mcl_update;
1063                 mcl.data = measuredPosition;
1064         } else {
1065                 /* angles evalution oriented update */
1066                 mcl.update = mcl_update2;
1067                 mcl.data = &measuredAngles;
1068         }
1069         mcl.normalize = mcl_normalize;
1070         mcl.sort = mcl_partsort;
1071         mcl.resample = mcl_resample;
1072
1073         /* cycles of enumeration */
1074         mcl.cycle = 0;
1075         mcl.noisecycle = 0;
1076
1077         /* change flag */
1078         mcl.flag = MCL_RUN;
1079
1080         est_pos.x = 0;
1081         est_pos.y = 0;
1082         est_pos.angle = 0;
1083
1084         /* generate particles with noises */
1085         mcl.init(&mcl);
1086 }
1087
1088 void SimMcl::moveParts(double dx, double dy, double dangle)
1089 {
1090         int i=0;
1091         mcl_thetas theta;
1092
1093         mcl.cycle++;
1094         /*WDBG(QString("mcl cycle = %1").arg(mcl.cycle));*/
1095
1096         /* FIXME */
1097         /* normally, the [x,y,angle] are values measured by sensors. In this
1098          * example, the values are quantified as the movement without noises*/
1099         for (i=0; i<5; i++) {
1100                 measPos.x += dx;
1101                 measPos.y += dy;
1102                 measPos.angle += dangle;
1103
1104                 measuredPosition[0] = measPos.x;
1105                 measuredPosition[1] = measPos.y;
1106                 measuredPosition[2] = measPos.angle;
1107
1108                 mcl_pos2ang(&measPos, theta, mcl.beacon_cnt, mcl.beacon_color);
1109                 measuredAngles.count = mcl.beacon_cnt;
1110                 for (int i=0; i<mcl.beacon_cnt; i++)
1111                         measuredAngles.val[i] = theta[i];
1112                 QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
1113                                 .arg(measPos.x, 0, 'f', 3)
1114                                 .arg(measPos.y, 0, 'f', 3)
1115                                 .arg(RAD2DEG(measPos.angle), 0, 'f', 2);
1116
1117                 dbg.append(QString("  ["));
1118                 for (int i=0; i<mcl.beacon_cnt-1; i++)
1119                         dbg.append(QString("%1  ")
1120                         .arg(RAD2DEG(theta[i]), 0, 'f', 2));
1121                 dbg.append(QString("%1")
1122                         .arg(RAD2DEG(theta[mcl.beacon_cnt-1]), 0, 'f', 2));
1123                 dbg.append(QString("]"));
1124
1125                 WDBG(dbg);
1126                 countMeasuredAnglesFrequency();
1127                 measuredAnglesWidget->animate();
1128
1129         /* MCL */
1130                 mcl.move(&mcl, dx, dy, dangle);
1131         }
1132         /* /FIXME */
1133         mcl.update(&mcl, mcl.data);
1134         mcl.normalize(&mcl);
1135         mcl.sort(&mcl);
1136         mcl.resample(&mcl);
1137
1138         /* update MCL painting */
1139         emit updateMclSignal();
1140 }
1141
1142 void SimMcl::updateMcl()
1143 {
1144         mcl.cycle++;
1145
1146         /* MCL */
1147         if (updateMclCheckBox->isChecked()) {
1148 //              mcl.move(&mcl, 0.00, 0.00, 0.00);
1149                 mcl.update(&mcl, mcl.data);
1150                 mcl.normalize(&mcl);
1151                 mcl.sort(&mcl);
1152                 mcl.resample(&mcl);
1153         }
1154
1155 //      est_pos = mcl_get_pos(&mcl);
1156
1157         /* update MCL painting */
1158         emit updateMclSignal();
1159 }