+++ /dev/null
-/*
- * SimMcl.cpp 07/10/31
- *
- * A simulation for testing the Monte Carlo Localization algorithm.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
- * License: GNU GPL v.2
- */
-
-#include <QtGui>
-#include <QKeyEvent>
-#include <QCloseEvent>
-#include <QEvent>
-#include <QGLWidget>
-
-#include <math.h>
-#include <robomath.h>
-#include <robodim.h>
-#include <mcl.h>
-#include <laser-nav.h>
-#include <roboorte.h>
-
-#include "PlaygroundScene.h"
-#include "MiscGui.h"
-#include "SimMcl.h"
-#include "GlWidget.h"
-#include "MclPainter.h"
-#include "AnglesHistogramPainter.h"
-
-SimMcl::SimMcl(QWidget *parent)
- : QWidget(parent)
-{
- QFont font;
- font.setPointSize(8);
- setFont(font);
-
- debugWindowEnabled = false;
- hasCurrentWidget = false;
- glWidgetInitialized = false;
- widgetInitialized = false;
- mclPainterInitialized = false;
- viewObjectsInitialized = false;
- objectsCountChanged = false;
- hasOpenGL = QGLFormat::hasOpenGL();
-
- createLeftLayout();
- createRightLayout();
-
- createOrte();
- createActions();
- initPln();
- initPlnData();
- initMcl();
-
- QGridLayout *mainLayout = new QGridLayout;
- mainLayout->addLayout(leftLayout, 0, 0);
- mainLayout->addLayout(rightLayout, 0, 1);
- setLayout(mainLayout);
-
- setFocusPolicy(Qt::StrongFocus);
- /*grabKeyboard();*/
- WDBG("Youuuhouuuu!!");
-
- /* some default selection */
- openglCheckBox->setChecked(OPENGL_DEFAULT);
-// paintObjectsRadioButton->setChecked(true);
- paintElementsRadioButton->setChecked(true);
- mclAnglesRadioButton->setChecked(true);
- mclBlueBeaconsRadioButton->setChecked(true);
-// mclPositionsRadioButton->setChecked(true);
- aliasingCheckBox->setChecked(ALIASING_DEFAULT);
-
- /* FIXME: initialization position of the measurement, used for testing */
- measPos.x = PLAYGROUND_WIDTH_M / 2.0;
- measPos.y = PLAYGROUND_HEIGHT_M / 2.0;
- measPos.angle = 0;
-}
-
-/**********************************************************************
- * GUI
- **********************************************************************/
-void SimMcl::createLeftLayout()
-{
- leftLayout = new QVBoxLayout();
-
- /* create a debug window and enable it */
- createDebugGroupBox();
- debugWindowEnabled = true;
- createPlaygroundGroupBox();
-
- leftLayout->addWidget(playgroundGroupBox);
- leftLayout->addWidget(debugGroupBox);
-}
-
-void SimMcl::createRightLayout()
-{
- rightLayout = new QVBoxLayout();
- QVBoxLayout *layout = new QVBoxLayout();
- QGridLayout *layout1 = new QGridLayout();
-
- createPositionGroupBox();
- createGraphicGroupBox();
- createMclGroupBox();
- createMiscGroupBox();
-
- layout1->addWidget(positionGroupBox, 0, 0);
- layout1->addWidget(graphicGroupBox, 0, 1);
-
- layout->addLayout(layout1);
- layout->addWidget(mclGroupBox);
- layout->addWidget(miscGroupBox);
-
- rightLayout->addLayout(layout);
-}
-
-void SimMcl::createPlaygroundGroupBox()
-{
- playgroundGroupBox = new QGroupBox(tr("Playground"));
- playgroundLayout = new QHBoxLayout();
-
- playgroundScene =
- new PlaygroundScene(PlaygroundScene::PLAYGROUND_EUROBOT2007);
- playgroundScene->setItemIndexMethod(QGraphicsScene::NoIndex);
- playgroundSceneView = new QGraphicsView(playgroundScene);
- playgroundSceneView->setMinimumWidth((int)(playgroundScene->width())+15);
- playgroundSceneView->setMinimumHeight((int)(playgroundScene->height())+15);
- playgroundSceneView->setRenderHint(QPainter::Antialiasing);
- playgroundSceneView->setBackgroundBrush(QColor(230, 200, 167));
-
- playgroundGroupBox->setLayout(playgroundLayout);
-}
-
-void SimMcl::createDebugGroupBox()
-{
- debugGroupBox = new QGroupBox(tr("Debug window"));
- QHBoxLayout *layout = new QHBoxLayout();
-
- debugWindow = new QTextEdit();
- debugWindow->setReadOnly(true);
-
- layout->addWidget(debugWindow);
- debugGroupBox->setLayout(layout);
-}
-
-void SimMcl::createPositionGroupBox()
-{
- positionGroupBox = new QGroupBox(tr("Position state"));
- QGridLayout *layout = new QGridLayout();
-
- actPosX = new QLineEdit();
- actPosY = new QLineEdit();
- actPosPhi = new QLineEdit();
-
- estPosX = new QLineEdit();
- estPosY = new QLineEdit();
- estPosPhi = new QLineEdit();
-
- actPosX->setReadOnly(true);
- actPosY->setReadOnly(true);
- actPosPhi->setReadOnly(true);
-
- estPosX->setReadOnly(true);
- estPosY->setReadOnly(true);
- estPosPhi->setReadOnly(true);
-
- layout->addWidget(MiscGui::createLabel("X"), 1, 0);
- layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
- layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
-
- layout->addWidget(MiscGui::createLabel("Actual", Qt::AlignLeft), 0, 1);
- layout->addWidget(actPosX, 1, 1);
- layout->addWidget(actPosY, 2, 1);
- layout->addWidget(actPosPhi, 3, 1);
-
- layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 0, 2);
- layout->addWidget(estPosX, 1, 2);
- layout->addWidget(estPosY, 2, 2);
- layout->addWidget(estPosPhi, 3, 2);
-
- positionGroupBox->setLayout(layout);
-}
-
-void SimMcl::createGraphicGroupBox()
-{
- graphicGroupBox = new QGroupBox(tr("Graphic settings"));
- QVBoxLayout *layout = new QVBoxLayout();
-
- paintObjectsRadioButton = new QRadioButton(tr("&Object style"));
- paintElementsRadioButton = new QRadioButton(tr("&Element style"));
- openglCheckBox = new QCheckBox(tr("Open&GL"));
- aliasingCheckBox = new QCheckBox(tr("A&liasing"));
-
- paintElementsRadioButton->setShortcut(tr("e"));
- paintObjectsRadioButton->setShortcut(tr("o"));
- openglCheckBox->setShortcut(tr("g"));
- aliasingCheckBox->setShortcut(tr("l"));
-
- layout->addWidget(paintElementsRadioButton);
- layout->addWidget(paintObjectsRadioButton);
- layout->addWidget(openglCheckBox);
- layout->addWidget(aliasingCheckBox);
-
- graphicGroupBox->setLayout(layout);
-}
-
-void SimMcl::createMclGroupBox()
-{
- mclGroupBox = new QGroupBox(tr("MCL"));
- mclParametersGroupBox = new QGroupBox(tr("Parameters"));
- mclStyleGroupBox = new QGroupBox(tr("Style"));
- mclBeaconsGroupBox = new QGroupBox(tr("Beacons"));
-
- QVBoxLayout *vlayout = new QVBoxLayout();
- QHBoxLayout *hlayout = new QHBoxLayout();
- QHBoxLayout *hlayout1 = new QHBoxLayout();
- QGridLayout *layout = new QGridLayout();
- QVBoxLayout *layout1 = new QVBoxLayout();
- QVBoxLayout *layout2 = new QVBoxLayout();
- QVBoxLayout *layout3 = new QVBoxLayout();
-
- mclWidthLineEdit = new QLineEdit();
- mclHeightLineEdit = new QLineEdit();
- mclXOffLineEdit = new QLineEdit();
- mclYOffLineEdit = new QLineEdit();
- mclGenDnoiseLineEdit = new QLineEdit();
- mclGenAnoiseLineEdit = new QLineEdit();
- mclMovDnoiseLineEdit = new QLineEdit();
- mclMovAnoiseLineEdit = new QLineEdit();
- mclWMinLineEdit = new QLineEdit();
- mclWMaxLineEdit = new QLineEdit();
- mclEvalSigmaLineEdit = new QLineEdit();
- mclAEvalSigmaLineEdit = new QLineEdit();
- mclMaxavdistLineEdit = new QLineEdit();
- mclMaxnoisecycleLineEdit = new QLineEdit();
- mclCountLineEdit = new QLineEdit();
- mclBeaconCount = new QLineEdit();
-
- layout->addWidget(MiscGui::createLabel("width"), 0, 0);
- layout->addWidget(MiscGui::createLabel("height"), 1, 0);
- layout->addWidget(MiscGui::createLabel("x offset"), 2, 0);
- layout->addWidget(MiscGui::createLabel("y offset"), 3, 0);
- layout->addWidget(MiscGui::createLabel("gen_dnoise"), 4, 0);
- layout->addWidget(MiscGui::createLabel("gen_anoise"), 5, 0);
- layout->addWidget(MiscGui::createLabel("mov_dnoise"), 6, 0);
- layout->addWidget(MiscGui::createLabel("mov_anoise"), 7, 0);
-
- layout->addWidget(mclWidthLineEdit, 0, 1);
- layout->addWidget(mclHeightLineEdit, 1, 1);
- layout->addWidget(mclXOffLineEdit, 2, 1);
- layout->addWidget(mclYOffLineEdit, 3, 1);
- layout->addWidget(mclGenDnoiseLineEdit, 4, 1);
- layout->addWidget(mclGenAnoiseLineEdit, 5, 1);
- layout->addWidget(mclMovDnoiseLineEdit, 6, 1);
- layout->addWidget(mclMovAnoiseLineEdit, 7, 1);
-
- layout->addWidget(MiscGui::createLabel("count"), 0, 2);
- layout->addWidget(MiscGui::createLabel("w_min"), 1, 2);
- layout->addWidget(MiscGui::createLabel("w_max"), 2, 2);
- layout->addWidget(MiscGui::createLabel("eval sigma"), 3, 2);
- layout->addWidget(MiscGui::createLabel("aeval sigma"), 4, 2);
- layout->addWidget(MiscGui::createLabel("maxavdist"), 5, 2);
- layout->addWidget(MiscGui::createLabel("maxnoisecycle"), 6, 2);
- layout->addWidget(MiscGui::createLabel("beacon_cnt"), 7, 2);
-
- layout->addWidget(mclCountLineEdit, 0, 3);
- layout->addWidget(mclWMinLineEdit, 1, 3);
- layout->addWidget(mclWMaxLineEdit, 2, 3);
- layout->addWidget(mclEvalSigmaLineEdit, 3, 3);
- layout->addWidget(mclAEvalSigmaLineEdit, 4, 3);
- layout->addWidget(mclMaxavdistLineEdit, 5, 3);
- layout->addWidget(mclMaxnoisecycleLineEdit, 6, 3);
- layout->addWidget(mclBeaconCount, 7, 3);
-
- mclWidthLineEdit->setMinimumWidth(50);
- mclCountLineEdit->setMinimumWidth(50);
-
- mclWidthLineEdit->setText("3.000");
- mclHeightLineEdit->setText("2.100");
- mclXOffLineEdit->setText("0.000");
- mclYOffLineEdit->setText("0.000");
- mclGenDnoiseLineEdit->setText("0.99");
- mclGenAnoiseLineEdit->setText("360");
- mclMovDnoiseLineEdit->setText("0.002");
- mclMovAnoiseLineEdit->setText("2");
- mclBeaconCount->setText("4");
-
- mclCountLineEdit->setText("1000");
- mclWMinLineEdit->setText("0.25");
- mclWMaxLineEdit->setText("2.0");
- mclEvalSigmaLineEdit->setText("160");
- mclAEvalSigmaLineEdit->setText("10");
- mclMaxavdistLineEdit->setText("0.15");
- mclMaxnoisecycleLineEdit->setText("10");
-
- resetMclPushButton = new QPushButton(tr("&Reset MCL"));
- resetMclPushButton->setShortcut(tr("r"));
- resetHistogramsPushButton = new QPushButton(tr("Reset &histograms"));
- resetHistogramsPushButton->setShortcut(tr("h"));
- generateMeasurementPushButton = new QPushButton(tr("Generate &measurement"));
- generateMeasurementPushButton->setShortcut(tr("m"));
- mclPositionsRadioButton = new QRadioButton(tr("&Position"));
- mclPositionsRadioButton->setShortcut(tr("p"));
- mclAnglesRadioButton = new QRadioButton(tr("&Angle"));
- mclAnglesRadioButton->setShortcut(tr("a"));
- mclBlueBeaconsRadioButton = new QRadioButton(tr("&Blue"));
- mclBlueBeaconsRadioButton->setShortcut(tr("b"));
- mclRedBeaconsRadioButton = new QRadioButton(tr("Re&d"));
- mclRedBeaconsRadioButton->setShortcut(tr("d"));
- resetHistogramsPeriodicallyCheckBox =
- new QCheckBox(tr("Re&set histograms periodically"));
- updateMclCheckBox =
- new QCheckBox(tr("Update MCL"));
-
- resetHistogramsPeriodicallyCheckBox->setShortcut(tr("s"));
- updateMclCheckBox->setShortcut(tr("u"));
-
- layout1->addWidget(mclPositionsRadioButton);
- layout1->addWidget(mclAnglesRadioButton);
-
- layout3->addWidget(mclBlueBeaconsRadioButton);
- layout3->addWidget(mclRedBeaconsRadioButton);
-
- displayCountSlider = new QSlider(Qt::Horizontal);
- displayCountSlider->setTracking(false);
- displayCountSlider->setTickPosition(QSlider::TicksBelow);
- displayCountSlider->setMinimum(0);
- displayCountSlider->setMaximum(mclCountLineEdit->text().toInt());
- displayCountSlider->setValue(mclCountLineEdit->text().toInt());
- displayCount = displayCountSlider->value();
-
- mclParametersGroupBox->setLayout(layout);
- mclStyleGroupBox->setLayout(layout1);
- mclBeaconsGroupBox->setLayout(layout3);
-
- hlayout1->addWidget(mclStyleGroupBox);
- hlayout1->addWidget(mclBeaconsGroupBox);
-
- layout2->addLayout(hlayout1);
- layout2->addWidget(resetMclPushButton);
- layout2->addWidget(resetHistogramsPushButton);
- layout2->addWidget(generateMeasurementPushButton);
- layout2->addWidget(resetHistogramsPeriodicallyCheckBox);
- layout2->addWidget(updateMclCheckBox);
- layout2->addStretch(1);
-
- hlayout->addWidget(mclParametersGroupBox);
- hlayout->addLayout(layout2);
-
- vlayout->addLayout(hlayout);
- vlayout->addWidget(MiscGui::createLabel("Number of particles with "
- "highest weight to be displayed"));
- vlayout->addWidget(displayCountSlider);
-
- mclGroupBox->setLayout(vlayout);
-}
-
-void SimMcl::createMiscGroupBox()
-{
- miscGroupBox = new QGroupBox(tr("Miscellaneous"));
- QVBoxLayout *vlayout = new QVBoxLayout();
-
- measuredAnglesHistogram =
- new AnglesHistogramPainter(measuredAnglesFreq, ANGLE_FREQ_COUNT);
- estimatedAnglesHistogram =
- new AnglesHistogramPainter(estimatedAnglesFreq, ANGLE_FREQ_COUNT);
- measuredAnglesWidget = new Widget(measuredAnglesHistogram, this);
- estimatedAnglesWidget = new Widget(estimatedAnglesHistogram, this);
-
- vlayout->addWidget(measuredAnglesWidget);
- vlayout->addWidget(estimatedAnglesWidget);
- vlayout->addStretch(1);
-
- miscGroupBox->setLayout(vlayout);
-}
-
-void SimMcl::setWidgetCurrent(QWidget *widget)
-{
- if (hasCurrentWidget) {
- currentWidget->hide();
- playgroundLayout->removeWidget(currentWidget);
- }
-
- currentWidget = widget;
- playgroundLayout->addWidget(widget);
- widget->show();
- hasCurrentWidget = true;
-}
-
-/**********************************************************************
- * GUI actions
- **********************************************************************/
-void SimMcl::createActions()
-{
- connect(openglCheckBox, SIGNAL(stateChanged(int)),
- this, SLOT(enableOpenGl(int)));
- connect(aliasingCheckBox, SIGNAL(stateChanged(int)),
- this, SLOT(setAliasing(int)));
- connect(paintElementsRadioButton, SIGNAL(toggled(bool)),
- this, SLOT(usePaintElements(bool)));
- connect(paintObjectsRadioButton, SIGNAL(toggled(bool)),
- this, SLOT(usePaintObjects(bool)));
- connect(resetMclPushButton, SIGNAL(clicked()), this, SLOT(resetMcl()));
- connect(resetHistogramsPushButton, SIGNAL(clicked()),
- this, SLOT(resetHistograms()));
- connect(resetHistogramsPeriodicallyCheckBox, SIGNAL(stateChanged(int)),
- this, SLOT(resetHistogramsPeriodically(int)));
- connect(generateMeasurementPushButton, SIGNAL(clicked()),
- this, SLOT(generateMeasurement()));
- connect(this, SIGNAL(updateMclSignal()), this, SLOT(updateMclPainter()));
- connect(mclCountLineEdit, SIGNAL(textChanged(QString)),
- this, SLOT(mclCountChanged(QString)));
- connect(displayCountSlider, SIGNAL(valueChanged(int)),
- this, SLOT(setDisplayCount(int)));
- connect(mclPositionsRadioButton, SIGNAL(toggled(bool)),
- this, SLOT(usePositionOriented(bool)));
- connect(mclAnglesRadioButton, SIGNAL(toggled(bool)),
- this, SLOT(useAngleOriented(bool)));
- connect(mclBlueBeaconsRadioButton, SIGNAL(toggled(bool)),
- this, SLOT(useBlueBeacons(bool)));
- connect(mclRedBeaconsRadioButton, SIGNAL(toggled(bool)),
- this, SLOT(useRedBeacons(bool)));
-}
-
-bool SimMcl::enableOpenGl(int state)
-{
- bool rv;
-
- if (state == Qt::Unchecked) {
- WDBG("OpenGL support disabled.");
- rv = false;
- }
-
- if (state == Qt::Checked) {
- if (!hasOpenGL) {
- WDBG("This system does not support OpenGL objects. "
- "Drawing too many samples may take a while.");
- openglCheckBox->setCheckState(Qt::Unchecked);
- return false;
- }
- WDBG("This system supports OpenGL. OpenGL support enabled.");
- rv = true;
- }
-
- if (paintElementsRadioButton->isChecked())
- usePaintElements(true);
-
- return rv;
-}
-
-void SimMcl::setAliasing(int state)
-{
- if (state == Qt::Checked) {
- if (widgetInitialized)
- widget->setAliasing(true);
- measuredAnglesWidget->setAliasing(true);
- estimatedAnglesWidget->setAliasing(true);
- playgroundSceneView->setRenderHint(QPainter::Antialiasing);
- } else {
- if (widgetInitialized)
- widget->setAliasing(false);
- measuredAnglesWidget->setAliasing(false);
- estimatedAnglesWidget->setAliasing(false);
- playgroundSceneView->setRenderHint(QPainter::Antialiasing, false);
- }
- updateMclPainter();
-}
-
-void SimMcl::initPaintElements()
-{
- struct mcl_robot_pos estimated = { 1.0, 1.0, 0};
- mclPainter = new MclPainter(mcl, &displayCount);
- mclPainter->estimated = estimated;
- mclPainter->setSize(610, 425);
- mclPainterInitialized = true;
- widgetInitialized = false;
- glWidgetInitialized = false;
-}
-
-void SimMcl::usePaintElements(bool state)
-{
- /* not checked */
- if (!state) {
- return;
- }
-
- /* initialize the painter */
- if (!mclPainterInitialized)
- initPaintElements();
-
- /* initialize widgets if not initialized */
- if (openglCheckBox->isChecked() && hasOpenGL &&
- !glWidgetInitialized) {
- glWidget = new GLWidget(mclPainter, this);
- glWidgetInitialized = true;
- } else if (!widgetInitialized) {
- widget = new Widget(mclPainter, this);
- widgetInitialized = true;
- }
-
- /* use OpenGL if required */
- if (openglCheckBox->isChecked()) {
- setWidgetCurrent(glWidget);
- } else {
- setWidgetCurrent(widget);
- }
-
- updateMclPainter();
-
- WDBG("Particles are represented as graphic elements (faster).");
-}
-
-void SimMcl::initPaintObjects()
-{
- struct mcl_laser *l = mcl_to_laser(mcl);
- struct mcl_laser_state *parts = l->parts;
-
- robots = new SmallRobot[mcl->count];
-
- for (int i=0; i<mcl->count; i++) {
- robots[i].setZValue(10);
- robots[i].mclPart = (struct mcl_laser_state *)&parts[i];
- robots[i].widgetSize = QSize((int)playgroundScene->width(),
- (int)playgroundScene->height());
- robots[i].playgroundSize =
- QSizeF(PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M);
- robots[i].init();
- robots[i].setParent(this);
- playgroundScene->addItem(&robots[i]);
- robotsList.append(&robots[i]);
- }
- viewObjectsInitialized = true;
-}
-
-void SimMcl::usePaintObjects(bool state)
-{
- /* not checked */
- if (!state) {
- return;
- }
-
- if (!viewObjectsInitialized)
- initPaintObjects();
-
- /* use OpenGL if required */
- if (openglCheckBox->isChecked()) {
- playgroundSceneView->setViewport(
- new QGLWidget(QGLFormat(QGL::SampleBuffers)));
- } else {
- playgroundSceneView->setViewport(0);
- }
-
- setWidgetCurrent(playgroundSceneView);
-
- updateMclPainter();
-
- WDBG("Particles are represented as objects. You can move over "
- "particles to see more details. Drag and drop is enabled.");
-}
-
-void SimMcl::removePaintObjects()
-{
- /* deallocate memory and remove object from the scene */
- while(!robotsList.empty()) {
- QGraphicsItem *item = robotsList.takeFirst();
- playgroundScene->removeItem(item);
- }
-}
-
-void SimMcl::resetMcl()
-{
- /* if the object count changed, reinitialize the objects */
- if (objectsCountChanged) {
- removePaintObjects();
- viewObjectsInitialized = false;
- mclPainterInitialized = false;
- }
-
- /* mcl initialization */
- initMcl();
-
- /* number of displayed particles */
- displayCountSlider->setMaximum(mclCountLineEdit->text().toInt());
- if (mclCountLineEdit->text().toInt() < displayCountSlider->value() ||
- objectsCountChanged)
- displayCountSlider->setValue(mclCountLineEdit->text().toInt());
- displayCount = displayCountSlider->value();
-
- /* display depends on the selected style */
- if (paintElementsRadioButton->isChecked())
- usePaintElements(true);
- else
- usePaintObjects(true);
-
- objectsCountChanged = false;
-
- emit updateMclSignal();
-}
-
-void SimMcl::resetHistograms()
-{
- resetHistogram(measuredAnglesFreq);
- resetHistogram(estimatedAnglesFreq);
- countMeasuredAnglesFrequency();
- measuredAnglesWidget->animate();
- countEstimatedAnglesFrequency();
- estimatedAnglesWidget->animate();
-}
-
-void SimMcl::resetHistogramsPeriodically(int state)
-{
-}
-
-void SimMcl::updateMclPainter()
-{
- updatePln();
- if (paintElementsRadioButton->isChecked()) {
- if (openglCheckBox->isChecked()) {
- glWidget->animate();
- } else {
- widget->animate();
- }
- } else {
- int count = mcl->count-displayCountSlider->value();
- for (int i=0; i<mcl->count; i++) {
- if (i < count) {
- robots[i].hide();
- } else {
- robots[i].show();
- robots[i].updateRobot();
- }
- }
- }
-
- /* count angles frequency and display histograms */
- countEstimatedAnglesFrequency();
- estimatedAnglesWidget->animate();
-
- /* update estimated position fields */
- struct mcl_robot_pos estimated = *(struct mcl_robot_pos *)(mcl->estimated);
- mclPainter->estimated = estimated;
-
- estPosX->setText(QString("%1").arg(estimated.x, 0, 'f', 3));
- estPosY->setText(QString("%1").arg(estimated.y, 0, 'f', 3));
- estPosPhi->setText(QString("%1 (%2 rad)")
- .arg(RAD2DEG(estimated.angle), 0, 'f', 0)
- .arg(estimated.angle, 0, 'f', 2));
-}
-
-void SimMcl::mclCountChanged(QString text)
-{
- objectsCountChanged = true;
-}
-
-void SimMcl::setDisplayCount(int value)
-{
- WDBG(QString("Number of particles to be displayed: %1").arg(value));
- displayCount = displayCountSlider->value();
- if (!objectsCountChanged)
- emit updateMclSignal();
-}
-
-void SimMcl::robotMoved()
-{
- updatePln();
-}
-
-void SimMcl::usePositionOriented(bool state)
-{
- if (!state)
- return;
-
- /* FIXME */
-// mcl.update = mcl_update;
-// mcl.data = measuredPosition;
-}
-
-void SimMcl::useAngleOriented(bool state)
-{
- if (!state)
- return;
-
- /* FIXME */
-// mcl.update = mcl_update2;
-// mcl.data = &measuredAngles;
-}
-
-void SimMcl::useBlueBeacons(bool state)
-{
- if (!state)
- return;
-
- mcl_laser.beacon_color = BEACON_BLUE;
-}
-
-void SimMcl::useRedBeacons(bool state)
-{
- if (!state)
- return;
-
- mcl_laser.beacon_color = BEACON_RED;
-}
-
-void SimMcl::generateMeasurement()
-{
- mcl_thetas theta;
- measPos.x = (double)((qrand()&0xffff)/65535.0*(double)mcl_laser.width);
- measPos.y = (double)((qrand()&0xffff)/65535.0*(double)mcl_laser.height);
- measPos.angle = DEG2RAD((double)((qrand()&0xffff)/65535.0*360));
- /* convert generated position to angles between reflectors */
- mcl_pos2ang(&measPos, theta, BEACON_CNT, mcl_laser.beacon_color);
- measuredAngles.count = BEACON_CNT;
- for (int i=0; i<BEACON_CNT; i++)
- measuredAngles.val[i] = theta[i];
-
- QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
- .arg(measPos.x, 0, 'f', 3)
- .arg(measPos.y, 0, 'f', 3)
- .arg(RAD2DEG(measPos.angle), 0, 'f', 2);
-
- dbg.append(QString(" ["));
- for (int i=0; i<BEACON_CNT-1; i++)
- dbg.append(QString("%1 ")
- .arg(RAD2DEG(theta[i]), 0, 'f', 2));
- dbg.append(QString("%1")
- .arg(RAD2DEG(theta[BEACON_CNT-1]), 0, 'f', 2));
- dbg.append(QString("]"));
-
- WDBG(dbg);
-
- resetHistogram(measuredAnglesFreq);
- countMeasuredAnglesFrequency();
- measuredAnglesWidget->animate();
-}
-/**********************************************************************
- * EVENTS
- **********************************************************************/
-bool SimMcl::event(QEvent *event)
-{
- switch (event->type()) {
- case QEVENT(QEV_LASER):
- emit laserDataReceivedSignal();
- countMeasuredAnglesFrequency();
- measuredAnglesWidget->animate();
- break;
- case QEVENT(QEV_MOTION_IRC):
- emit motionIrcReceivedSignal();
- break;
- default:
- if (event->type() == QEvent::Close)
- closeEvent((QCloseEvent *)event);
- else if (event->type() == QEvent::KeyPress)
- keyPressEvent((QKeyEvent *)event);
- else if (event->type() == QEvent::KeyRelease)
- keyReleaseEvent((QKeyEvent *)event);
- else if (event->type() == QEvent::FocusIn)
- grabKeyboard();
- else if (event->type() == QEvent::FocusOut)
- releaseKeyboard();
- else {
- event->ignore();
- return false;
- }
- break;
- }
- event->accept();
- return true;
-}
-
-void SimMcl::keyPressEvent(QKeyEvent *event)
-{
- if (event->isAutoRepeat()) {
- event->ignore();
- return;
- }
-
- switch (event->key()) {
- case Qt::Key_Up:
- moveParts(0.0, 0.002, 0.0);
- break;
- case Qt::Key_Down:
- moveParts(0.0, -0.002, 0.0);
- break;
- case Qt::Key_Left:
- moveParts(-0.002, 0.0, 0.0);
- break;
- case Qt::Key_Right:
- moveParts(0.002, 0.0, 0.0);
- break;
- /*case Qt::Key_L:
- emit laserDataReceivedSignal();
- countMeasuredAnglesFrequency();
- measuredAnglesWidget->animate();
- break;*/
- default:
- event->ignore();
- break;
- }
- event->accept();
-}
-
-void SimMcl::keyReleaseEvent(QKeyEvent *event)
-{
- if (event->isAutoRepeat()) {
- event->ignore();
- return;
- }
-
- switch (event->key()) {
- case Qt::Key_Up:
- case Qt::Key_Down:
- case Qt::Key_Left:
- case Qt::Key_Right:
- /*WDBG("arrow key released");*/
- break;
- default:
- event->ignore();
- break;
- }
- event->accept();
-}
-
-void SimMcl::closeEvent(QCloseEvent *event)
-{
- robottype_roboorte_destroy(&orte);
-}
-
-/**********************************************************************
- * ORTE
- **********************************************************************/
-void SimMcl::createOrte()
-{
- int rv;
-
- orte.strength = 12;
-
- rv = robottype_roboorte_init(&orte);
- if (!rv) {
- printf("SimMcl: Unable to initialize ORTE\n");
- }
-
- robottype_subscriber_motion_irc_create(&orte,
- rcv_motion_irc_cb, this);
- robottype_subscriber_laser_data_create(&orte,
- receiveLaserCallBack, this);
-
- /* set actions to do when we receive data from orte */
- connect(this, SIGNAL(laserDataReceivedSignal()),
- this, SLOT(laserReceived()));
- connect(this, SIGNAL(motionIrcReceivedSignal()),
- this, SLOT(updateOdometry()));
-}
-
-void SimMcl::laserReceived()
-{
- unsigned int times[LAS_CNT];
- int cnt;
-
- /*WDBG("ORTE received: laser");*/
-
- measuredAngles.count = 1;
- measuredAngles.val[0] = TIME2ANGLE(orte.laser_data.period,
- orte.laser_data.measure);
-
- /*QString dbg = QString("theta=%1")
- .arg(RAD2DEG(measuredAngles.val[0]), 0, 'f', 3);
- WDBG(dbg);*/
-
- countMeasuredAnglesFrequency();
- measuredAnglesWidget->animate();
-
- updateMcl();
-}
-
-void SimMcl::updateOdometry()
-{
- static struct motion_irc_type curr_irc;
- static struct motion_irc_type prev_irc;
- static int firstRun = 1;
- /* spocitat prevodovy pomer */
- double n = (double)(28.0 / 1.0);
- /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
- double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
-
- double aktk0 = 0;
- double aktk1 = 0;
- double deltaU = 0;
- double deltAlfa = 0;
-
-// WDBG("ORTE received: motion_irc");
-
- curr_irc = orte.motion_irc;
-
- if(firstRun) {
- prev_irc = curr_irc;
- firstRun = 0;
- }
-
- aktk0 = ((prev_irc.left - curr_irc.left) >> 8) * c;
- aktk1 = ((curr_irc.right - prev_irc.right) >> 8) * c;
- prev_irc = curr_irc;
-
- deltaU = (aktk0 + aktk1) / 2;
- deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
-
- struct mcl_robot_odo odo = {deltaU, 0, deltAlfa};
-
- mcl->predict(mcl, &odo);
-// if (fabs(deltaU) > 0 || fabs(deltAlfa) > 0) {
-// QString dbg = QString("du=%1 dphi=%3 ")
-// .arg(deltaU, 0, 'f', 3)
-// .arg(deltAlfa, 0, 'f', 2);
-// WDBG(dbg);
-//
-// }
-}
-
-/**********************************************************************
- * PLN - Passive Laser Navigation
- **********************************************************************/
-void SimMcl::initPln()
-{
- /* set reference points. Used to calculate angles, position.. */
- pln_set_points();
-}
-
-void SimMcl::updatePln()
-{
-}
-
-void SimMcl::initPlnData()
-{
- measuredAngles.count = 0;
- for (int i=0; i<10; i++)
- measuredAngles.val[i] = 0;
-
- resetHistogram(measuredAnglesFreq);
- resetHistogram(estimatedAnglesFreq);
-}
-
-void SimMcl::resetHistogram(struct angles_freq *data)
-{
- for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
- data[i].angle = (int)i;
- data[i].frequency = 0;
- }
-}
-
-void SimMcl::countMeasuredAnglesFrequency()
-{
- double a;
-
- if (resetHistogramsPeriodicallyCheckBox->isChecked())
- resetHistogram(measuredAnglesFreq);
-
- for (int i=0; i<measuredAngles.count; i++) {
- /* normalize angles */
- a = fabs(fmod(RAD2DEG(measuredAngles.val[i]), 360));
-
- for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
- if ((measuredAnglesFreq[i].angle) == (int)a)
- measuredAnglesFreq[i].frequency++;
- }
- }
-
- angles_freq_sort(measuredAnglesFreq, ANGLE_FREQ_COUNT);
-}
-
-void SimMcl::countEstimatedAnglesFrequency()
-{
- struct mcl_laser *l = mcl_to_laser(mcl);
- struct mcl_laser_state *parts = l->parts;
- double a;
- mcl_thetas theta;
- int _angles[ANGLE_FREQ_COUNT];
-
- if (resetHistogramsPeriodicallyCheckBox->isChecked())
- resetHistogram(estimatedAnglesFreq);
-
- for (int i=0; i<ANGLE_FREQ_COUNT; i++)
- _angles[i] = 0;
-
- for (int i=0; i<displayCountSlider->value(); i++) {
- mcl_pos2ang(&parts[i], theta, BEACON_CNT, mcl_laser.beacon_color);
- for (int e=0; e<BEACON_CNT; e++) {
- /* normalize angles */
- a = fabs(fmod(RAD2DEG(theta[e]), 360));
- _angles[(int)a]++;
- }
- }
-
- for (int i=0; i<ANGLE_FREQ_COUNT; i++) {
- estimatedAnglesFreq[i].angle = (double)i;
- estimatedAnglesFreq[i].frequency = _angles[i];
- }
-
- angles_freq_sort(estimatedAnglesFreq, ANGLE_FREQ_COUNT);
-}
-
-/**********************************************************************
- * MCL
- **********************************************************************/
-void SimMcl::initMcl()
-{
- /* MCL laser */
- mcl_laser.width = PLAYGROUND_WIDTH_M;
- mcl_laser.height = PLAYGROUND_HEIGHT_M;
- mcl_laser.pred_dnoise = mclMovDnoiseLineEdit->text().toFloat();
- mcl_laser.pred_anoise = DEG2RAD(mclMovAnoiseLineEdit->text().toFloat());
- mcl_laser.aeval_sigma = DEG2RAD(mclAEvalSigmaLineEdit->text().toFloat());
-
- if (mclRedBeaconsRadioButton->isChecked()) {
- mcl_laser.beacon_color = BEACON_RED;
- } else {
- mcl_laser.beacon_color = BEACON_BLUE;
- }
-
- mcl = mcl_laser_init(&mcl_laser, mclCountLineEdit->text().toInt());
-
- /* amount of particles */
-// if (objectsCountChanged || !viewObjectsInitialized
-// || !mclPainterInitialized) {
-// mcl.parts = (struct mcl_particle *)
-// malloc(sizeof(struct mcl_particle)*mcl.count);
-// }
-
- /* style of particles evaluation */
- if (mclPositionsRadioButton->isChecked()) {
- /* position evaluation oriented update */
-// mcl.data = measuredPosition;
- } else {
- /* angles evalution oriented update */
-// mcl.data = &measuredAngles;
- }
-}
-
-void SimMcl::moveParts(double dx, double dy, double dangle)
-{
- int j=0;
- mcl_thetas theta;
- struct mcl_robot_odo odo = { dx, dy, dangle };
-
- /*WDBG(QString("mcl cycle = %1").arg(mcl.cycle));*/
-
- /* FIXME: not working yet */
- /* FIXME */
- /* normally, the [x,y,angle] are values measured by sensors. In this
- * example, the values are quantified as the movement without noises*/
- for (j=0; j<5; j++) {
- measPos.x += dx;
- measPos.y += dy;
- measPos.angle += dangle;
-
- measuredPosition[0] = measPos.x;
- measuredPosition[1] = measPos.y;
- measuredPosition[2] = measPos.angle;
-
- mcl_pos2ang(&measPos, theta, BEACON_CNT, mcl_laser.beacon_color);
- measuredAngles.count = BEACON_CNT;
- for (int i=0; i<BEACON_CNT; i++)
- measuredAngles.val[i] = theta[i];
- QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
- .arg(measPos.x, 0, 'f', 3)
- .arg(measPos.y, 0, 'f', 3)
- .arg(RAD2DEG(measPos.angle), 0, 'f', 2);
-
- dbg.append(QString(" ["));
- for (int i=0; i<BEACON_CNT-1; i++)
- dbg.append(QString("%1 ")
- .arg(RAD2DEG(theta[i]), 0, 'f', 2));
- dbg.append(QString("%1")
- .arg(RAD2DEG(theta[BEACON_CNT-1]), 0, 'f', 2));
- dbg.append(QString("]"));
-
- WDBG(dbg);
- countMeasuredAnglesFrequency();
- measuredAnglesWidget->animate();
-
- /* MCL */
- mcl->predict(mcl, &odo);
-
- }
-
- /* /FIXME */
- mcl->update(mcl, &measuredAngles);
- mcl->resample(mcl);
-
- /* update MCL painting */
- emit updateMclSignal();
-}
-
-void SimMcl::updateMcl()
-{
- /* MCL */
- if (updateMclCheckBox->isChecked()) {
-// mcl->predict(mcl, 0.00, 0.00, 0.00);
- mcl->update(mcl, &measuredAngles);
- mcl->resample(mcl);
- }
-
- /* update MCL painting */
- emit updateMclSignal();
-}
projects / eurobot / public.git / commitdiff