robomon2: update MCL visualization to use the new MCL API.

diff --git a/src/robomon/src2/MclPainter.cpp b/src/robomon/src2/MclPainter.cpp
index da0336e97d39c088a911c48bd5dbef78345faaf9..13e47ed4488c28e93e459d8dac3a814e4eb5ff79 100644 (file)
--- a/src/robomon/src2/MclPainter.cpp
+++ b/src/robomon/src2/MclPainter.cpp
@@ -69,6 +69,32 @@ void MclPainter::paint(QPainter *painter, QPaintEvent *event)
                        -(qreal)(((PLAYGROUND_HEIGHT_M-parts[i].y) 
                                / PLAYGROUND_HEIGHT_M * height)+5));
        }
+
+       /* draw a circle */
+       painter->translate(
+               (qreal)(estimated.x / PLAYGROUND_WIDTH_M * width), 
+               (qreal)((PLAYGROUND_HEIGHT_M-estimated.y) 
+                       / PLAYGROUND_HEIGHT_M * height));
+
+       painter->setBrush(QColor(40, 40, 255));
+       painter->drawEllipse(0, 0, 20, 20);
+       painter->translate(
+               -(qreal)(estimated.x / PLAYGROUND_WIDTH_M * width), 
+               -(qreal)((PLAYGROUND_HEIGHT_M-estimated.y) 
+                       / PLAYGROUND_HEIGHT_M * height));
+
+       /* draw a line representing the robot's head */
+       painter->translate(
+               (qreal)((estimated.x / PLAYGROUND_WIDTH_M * width)+10), 
+               (qreal)(((PLAYGROUND_HEIGHT_M-estimated.y) 
+                       / PLAYGROUND_HEIGHT_M * height)+10));
+       painter->rotate(RAD2DEG(-estimated.angle));
+       painter->drawLine(0, 0, 80, 0);
+       painter->rotate(RAD2DEG(estimated.angle));
+       painter->translate(
+               -(qreal)((estimated.x / PLAYGROUND_WIDTH_M * width)+10), 
+               -(qreal)(((PLAYGROUND_HEIGHT_M-estimated.y) 
+                       / PLAYGROUND_HEIGHT_M * height)+10));
 }
 
 void MclPainter::setSize(int width, int height)

diff --git a/src/robomon/src2/MclPainter.h b/src/robomon/src2/MclPainter.h
index 4197a368364041e60fe9e5c507c8b0bac0f8aae7..fe0adcd55006eefc688d92d1c42f710f723a0623 100644 (file)
--- a/src/robomon/src2/MclPainter.h
+++ b/src/robomon/src2/MclPainter.h
@@ -23,6 +23,7 @@ class MclPainter : public Painter
 public:
        MclPainter(struct mcl_model *mcl, int *displayCount);
        ~MclPainter();
+       struct mcl_robot_pos estimated;
 
 public:
        void paint(QPainter *painter, QPaintEvent *event);

diff --git a/src/robomon/src2/SimMcl.cpp b/src/robomon/src2/SimMcl.cpp
index 760394ba25140d9f9a1547d7f8d930ab141660d8..01194c801851df79ff680acf365864c34b467362 100644 (file)
--- a/src/robomon/src2/SimMcl.cpp
+++ b/src/robomon/src2/SimMcl.cpp
@@ -469,7 +469,9 @@ void SimMcl::setAliasing(int state)
 
 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;
@@ -636,13 +638,14 @@ void SimMcl::updateMclPainter()
        estimatedAnglesWidget->animate();
 
        /* update estimated position fields */
-       struct mcl_laser_state *estimated = (struct mcl_laser_state*)mcl->estimated;
+       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));
+       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));
+                       .arg(RAD2DEG(estimated.angle), 0, 'f', 0)
+                       .arg(estimated.angle, 0, 'f', 2));
 }
 
 void SimMcl::mclCountChanged(QString text)
@@ -901,14 +904,14 @@ void SimMcl::updateOdometry()
 
        struct mcl_robot_odo odo = {deltaU, 0, deltAlfa};
 
-       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);
-
-               mcl->predict(mcl, &odo);
-       }
+       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);
+// 
+//     }
 }
 
 /**********************************************************************
@@ -1001,9 +1004,9 @@ void SimMcl::initMcl()
        /* MCL laser */
        mcl_laser.width = PLAYGROUND_WIDTH_M;
        mcl_laser.height = PLAYGROUND_HEIGHT_M;
-       mcl_laser.pred_dnoise = 0.003;
-       mcl_laser.pred_anoise = 0.03;
-       mcl_laser.aeval_sigma = 0.90;
+       mcl_laser.pred_dnoise = 0.03;
+       mcl_laser.pred_anoise = DEG2RAD(5);
+       mcl_laser.aeval_sigma = DEG2RAD(10);
 
        if (mclRedBeaconsRadioButton->isChecked()) {
                mcl_laser.beacon_color = BEACON_RED;
@@ -1032,56 +1035,57 @@ void SimMcl::initMcl()
 
 void SimMcl::moveParts(double dx, double dy, double dangle)
 {
-//     int i=0;
-//     mcl_thetas theta;
-// 
-//     mcl.cycle++;
-//     /*WDBG(QString("mcl cycle = %1").arg(mcl.cycle));*/
-// 
-//     /* FIXME */
-//     /* normally, the [x,y,angle] are values measured by sensors. In this 
-//      * example, the values are quantified as the movement without noises*/
-//     for (i=0; i<5; i++) {
-//             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, mcl.beacon_cnt, mcl.beacon_color);
-//             measuredAngles.count = mcl.beacon_cnt;
-//             for (int i=0; i<mcl.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<mcl.beacon_cnt-1; i++)
-//                     dbg.append(QString("%1  ")
-//                     .arg(RAD2DEG(theta[i]), 0, 'f', 2));
-//             dbg.append(QString("%1")
-//                     .arg(RAD2DEG(theta[mcl.beacon_cnt-1]), 0, 'f', 2));
-//             dbg.append(QString("]"));
-// 
-//             WDBG(dbg);
-//             countMeasuredAnglesFrequency();
-//             measuredAnglesWidget->animate();
-// 
-//     /* MCL */
-//             mcl.move(&mcl, dx, dy, dangle);
-//     }
-//     /* /FIXME */
-//     mcl.update(&mcl, mcl.data);
-//     mcl.normalize(&mcl);
-//     mcl.sort(&mcl);
-//     mcl.resample(&mcl);
-// 
-//     /* update MCL painting */
-//     emit updateMclSignal();
+       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()