robomon2: update estimated position fields.

diff --git a/src/laser-nav/mcl/mcl.h b/src/laser-nav/mcl/mcl.h
index 544a40c153319c565a39516a6ecdb9958a79086b..54d3e28c0fda2f5cfb32da31fdb4a6868e376ac4 100644 (file)
--- a/src/laser-nav/mcl/mcl.h
+++ b/src/laser-nav/mcl/mcl.h
@@ -78,9 +78,9 @@ void mcl_normalize(struct mcl_model *mcl);
 void mcl_partsort(struct mcl_model *mcl);
 void mcl_resample(struct mcl_model *mcl);
 void mcl_init (struct mcl_model *mcl);
+inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl);
 #ifdef __cplusplus
 }
 #endif 
-inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl);
 
 #endif /* __mcl__ */

diff --git a/src/robomon/src2/SimMcl.cpp b/src/robomon/src2/SimMcl.cpp
index 14bcd9071ae3b043d3d7fc1f17a5f3fe89e108c3..8638f25671e765764e0947c80ae66ee83c0d9e40 100644 (file)
--- a/src/robomon/src2/SimMcl.cpp
+++ b/src/robomon/src2/SimMcl.cpp
@@ -186,7 +186,8 @@ void SimMcl::createMiscGroupBox()
        QVBoxLayout *vlayout = new QVBoxLayout();
        QGridLayout *layout = new QGridLayout();
 
-       resetMclPushButton = new QPushButton(tr("Reset MCL"));
+       resetMclPushButton = new QPushButton(tr("&Reset MCL"));
+       resetMclPushButton->setShortcut(tr("r"));
        layout->addWidget(resetMclPushButton, 0, 0);
 
        vlayout->addLayout(layout);
@@ -346,13 +347,22 @@ void SimMcl::resetMcl()
 
 void SimMcl::updateMcl()
 {
-       if (paintObjectsRadioButton->isChecked()) {
+       if (paintElementsRadioButton->isChecked()) {
+               glWidget->animate();
+       } else {
                for (int i=0; i<mcl.count; i++) {
                        robots[i].updateRobot();
                }
-       } else {
-               glWidget->animate();
        }
+
+       /* update estimated position fields */
+       struct mcl_particle part = mcl_get_pos(&mcl);
+
+       estPosX->setText(QString("%1").arg(part.x, 0, 'f', 3));
+       estPosY->setText(QString("%1").arg(part.y, 0, 'f', 3));
+       estPosPhi->setText(QString("%1 (%2 rad)")
+                       .arg(part.angle, 0, 'f', 0)
+                       .arg(DEG2RAD(part.angle), 0, 'f', 1));
 }
 
 /**********************************************************************