robofsm and robomon has been updated to update MCL with one measured

angles. Other modification.

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index 39dd23bb36a0c22fdc254797b213d0bcef613dce..a5b080d4505cb2b36e193ac7cd5011acbf80ef94 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -59,8 +59,10 @@ static void int_handler(int sig)
 void robot_loc_init()
 {
        /* MCL initialization */
-       robot.mcl.width = PLAYGROUND_WIDTH_M;   /* in meters */
-       robot.mcl.height = PLAYGROUND_HEIGHT_M; /* in meters */
+       robot.mcl.xoff = 0.5;
+       robot.mcl.xoff = 0.2;
+       robot.mcl.width = 1.0;  /* in meters */
+       robot.mcl.height = 0.5; /* in meters */
        /* the noises */
        robot.mcl.gen_dnoise = 0.99;
        robot.mcl.gen_anoise = 360;

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index a2996329104388c7584570b6e615815d2f97f680..c670f4c8dfb6bbdb34bfaaeb2fb8304363bf1f09 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -15,6 +15,7 @@
 #include <robot_eb2008.h>
 #include <movehelper_eb2008.h>
 #include <math.h>
+#include <robomath.h>
 #include <laser-nav.h>
 
 /* ---------------------------------------------------------------------- 
@@ -108,7 +109,7 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        if (!robot.use_loc)
                                break;
 
-                       if (dx > 0.0001 || dy > 0.0001 || deltAlfa > 0.0001) {
+                       if (fabs(dx) > 0.001 || fabs(dy) > 0.001 || fabs(deltAlfa) > 0.001) {
                                ROBOT_LOCK(mcl);
                                robot.mcl.move(&robot.mcl, dx, dy, deltAlfa);
                                ROBOT_UNLOCK(mcl);
@@ -248,35 +249,27 @@ void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
-void rcv_laser_meas_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
-       struct laser_meas_type *instance = (struct laser_meas_type *)vinstance;
-       static unsigned int times[LAS_CNT];
+       struct laser_data_type *instance = (struct laser_data_type *)vinstance;
        static struct mcl_particle est_pos;
-       static int i;
        
        switch (info->status) {
                case NEW_DATA:
-                       times[0] = instance->period;
-                       times[1] = instance->measures0;
-                       times[2] = instance->measures1;
-                       times[3] = instance->measures2;
-                       times[4] = instance->measures3;
-                       times[5] = instance->measures4;
-                       times[6] = instance->measures5;
-                       times[7] = instance->measures6;
-                       times[8] = instance->measures7;
-                       times[9] = instance->measures8;
-                       times[10] = instance->measures9;
+                       if (!robot.use_loc)
+                               break;
 
                        ROBOT_LOCK(meas_angles);
-                       robot.meas_angles.count = instance->cnt + 1;
-                       for (i=0; i<robot.meas_angles.count; i++)
-                               robot.meas_angles.val[i] = 
-                                       (double)TIME2ANGLE(times[0],times[i+1]);
+                       robot.meas_angles.count = 1;
+                       robot.meas_angles.val[0] = 
+                                       (double)TIME2ANGLE(instance->period,
+                                                       instance->measure);
                        ROBOT_UNLOCK(meas_angles);
 
+//                     printf("a=%f\n", RAD2DEG(robot.meas_angles.val[0]));
+                       break;
+
                        ROBOT_LOCK(mcl);
                        robot.mcl.update(&robot.mcl, robot.mcl.data);
                        robot.mcl.normalize(&robot.mcl);
@@ -286,6 +279,7 @@ void rcv_laser_meas_cb(const ORTERecvInfo *info, void *vinstance,
 
                        est_pos = mcl_get_pos(&robot.mcl);
 
+                       printf("est_pos=[%f,%f,%f]\n", est_pos.x, est_pos.y, est_pos.angle);
                        ROBOT_LOCK(est_pos);
                        robot.est_pos.x = est_pos.x;
                        robot.est_pos.y = est_pos.y;
@@ -333,6 +327,6 @@ void robot_init_orte()
        /* create eb2008 subscribers */
        eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
        eb2008_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
-       eb2008_subscriber_laser_meas_create(&robot.orte, rcv_laser_meas_cb, &robot.orte);
+       eb2008_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
 }
 

diff --git a/src/robomon/src2/SimMcl.cpp b/src/robomon/src2/SimMcl.cpp
index a5c7e6e005c2d77cba434bb58f0e80d55bea343a..b31286a1950f60e6e1c393dd8c4ec044f374c247 100644 (file)
--- a/src/robomon/src2/SimMcl.cpp
+++ b/src/robomon/src2/SimMcl.cpp
@@ -830,7 +830,7 @@ void SimMcl::createOrte()
 
        generic_subscriber_motion_irc_create(&orte_generic, 
                                rcv_motion_irc_cb, this);
-       eb2008_subscriber_laser_meas_create(&orte_eb2008, 
+       eb2008_subscriber_laser_data_create(&orte_eb2008, 
                                receiveLaserCallBack, this);
 
        /* set actions to do when we receive data from orte */
@@ -845,32 +845,15 @@ void SimMcl::laserReceived()
        unsigned int times[LAS_CNT];
        int cnt;
 
-//     WDBG("ORTE received: laser");
-
-       cnt = orte_eb2008.laser_meas.cnt;
-       times[0] = orte_eb2008.laser_meas.period;
-       times[1] = orte_eb2008.laser_meas.measures0;
-       times[2] = orte_eb2008.laser_meas.measures1;
-       times[3] = orte_eb2008.laser_meas.measures2;
-       times[4] = orte_eb2008.laser_meas.measures3;
-       times[5] = orte_eb2008.laser_meas.measures4;
-       times[6] = orte_eb2008.laser_meas.measures5;
-       times[7] = orte_eb2008.laser_meas.measures6;
-       times[8] = orte_eb2008.laser_meas.measures7;
-       times[9] = orte_eb2008.laser_meas.measures8;
-       times[10] = orte_eb2008.laser_meas.measures9;
-
-       /* avoid segfault when receiving some error */
-       if (cnt > (LAS_CNT-1)) {
-               WDBG(QString("Detected a bad number of received laser data: %1")
-                       .arg(cnt));
-               cnt = LAS_CNT - 1;
-       }
+       /*WDBG("ORTE received: laser");*/
 
-       measuredAngles.count = cnt;
-       for (int i=0; i<cnt; i++) {
-               measuredAngles.val[i] = (double)TIME2ANGLE(times[0],times[i+1]);
-       }
+       measuredAngles.count = 1;
+       measuredAngles.val[0] = TIME2ANGLE(orte_eb2008.laser_data.period, 
+                               orte_eb2008.laser_data.measure);
+
+       /*QString dbg = QString("theta=%1")
+                       .arg(RAD2DEG(measuredAngles.val[0]), 0, 'f', 3);
+       WDBG(dbg);*/
 
        countMeasuredAnglesFrequency();
        measuredAnglesWidget->animate();
@@ -880,6 +863,7 @@ void SimMcl::laserReceived()
 
 void SimMcl::updateOdometry()
 {
+       static struct motion_irc_type curr_irc;
        static struct motion_irc_type prev_irc;
        static int firstRun = 1;
        /* spocitat prevodovy pomer */
@@ -898,14 +882,16 @@ void SimMcl::updateOdometry()
 
 //     WDBG("ORTE received: motion_irc");
 
+       curr_irc = orte_generic.motion_irc;
+
        if(firstRun) {
-               prev_irc = orte_generic.motion_irc;
+               prev_irc = curr_irc;
                firstRun = 0;
        }
        
-       aktk0 = (prev_irc.left - orte_generic.motion_irc.left) >> 8;
-       aktk1 = (orte_generic.motion_irc.right - prev_irc.right) >> 8;                  
-       prev_irc = orte_generic.motion_irc;
+       aktk0 = (prev_irc.left - curr_irc.left) >> 8;
+       aktk1 = (curr_irc.right - prev_irc.right) >> 8;                 
+       prev_irc = curr_irc;
        dk0 = aktk0;
        dk1 = aktk1;    
 
@@ -918,17 +904,19 @@ void SimMcl::updateOdometry()
 
        /* dk1 - dk0 */
        deltAlfa = (dk1 - dk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);
+
+       est_pos.angle += deltAlfa;
        
        double dx, dy;
 
-       dx = deltaU;
-       dy = deltaU;
+       dx = deltaU * cos(est_pos.angle);
+       dy = deltaU * sin(est_pos.angle);
 
-       if (dx != 0 || dy != 0 || deltAlfa != 0) {
+       if (fabs(dx) > 0.001 || fabs(dy) > 0.001 || fabs(deltAlfa) > 0.001) {
                QString dbg = QString("dx=%1 dy=%2 dphi=%3 ")
                                .arg(dx, 0, 'f', 3)
                                .arg(dy, 0, 'f', 3)
-                               .arg(RAD2DEG(deltAlfa), 0, 'f', 2);
+                               .arg(deltAlfa, 0, 'f', 2);
                WDBG(dbg);
 
                mcl.move(&mcl, dx, dy, deltAlfa);
@@ -1077,6 +1065,10 @@ void SimMcl::initMcl()
        /* change flag */
        mcl.flag = MCL_RUN;
 
+       est_pos.x = 0;
+       est_pos.y = 0;
+       est_pos.angle = 0;
+
        /* generate particles with noises */
        mcl.init(&mcl);
 }
@@ -1146,6 +1138,8 @@ void SimMcl::updateMcl()
        mcl.sort(&mcl);
        mcl.resample(&mcl);
 
+       est_pos = mcl_get_pos(&mcl);
+
        /* update MCL painting */
        emit updateMclSignal();
 }

diff --git a/src/robomon/src2/SimMcl.h b/src/robomon/src2/SimMcl.h
index 6059d84b5485b81ce153bf131e5974487185838f..93038f97f2cab35de4b15973b1bfa7520d6c40aa 100644 (file)
--- a/src/robomon/src2/SimMcl.h
+++ b/src/robomon/src2/SimMcl.h
@@ -221,6 +221,7 @@ private:
        bool mclInitialized;
        struct mcl_model mcl;
        struct mcl_particle measPos;
+       struct mcl_particle est_pos;
        double measuredPosition[3];
 };