robofsm: fix all bad locking stuffs, which could cause wrong race condition.

diff --git a/src/robofsm/eb2008/fsmjoy.c b/src/robofsm/eb2008/fsmjoy.c
index d3046986df26617136c1e6f09eb66c1fd7bf1e2b..be96abe12c84bea1c23830d002d3951957a24133 100644 (file)
--- a/src/robofsm/eb2008/fsmjoy.c
+++ b/src/robofsm/eb2008/fsmjoy.c
@@ -61,7 +61,7 @@ FSM_STATE(joy_scan)
                        break;
                case EV_TIMER:
                        /* button1 - turn off all drives */
-                       if (robot.gorte.joy_data.button1 == 1) {
+                       if (robot.joy_data.button1 == 1) {
                                if(buttons_prev[0]!=1) {
                                        buttons[0] = ~buttons[0];
                                        buttons_prev[0] = 1;
@@ -74,7 +74,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[0] = 0;
 
                        /* button2 - open/close brush left/right  */
-                       if (robot.gorte.joy_data.button2 == 1) {
+                       if (robot.joy_data.button2 == 1) {
                                if(buttons_prev[1]!=1) {
                                        buttons[1] = ~buttons[1];
                                        buttons_prev[1] = 1;
@@ -91,7 +91,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[1] = 0;
 
                        /* button3 - open close back door */
-                       if (robot.gorte.joy_data.button3 == 1) {
+                       if (robot.joy_data.button3 == 1) {
                                if(buttons_prev[2]!=1) {
                                        buttons[2] = ~buttons[2];
                                        buttons_prev[2] = 1;
@@ -105,7 +105,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[2] = 0;
 
                        /* button4 -  */
-                       if (robot.gorte.joy_data.button4 == 1) {
+                       if (robot.joy_data.button4 == 1) {
                                if(buttons_prev[3]!=1) {
                                        buttons[3] = ~buttons[3];
                                        buttons_prev[3] = 1;
@@ -115,7 +115,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[3] = 0;
 
                        /* button5 - */
-                       if (robot.gorte.joy_data.button5 == 1) {
+                       if (robot.joy_data.button5 == 1) {
                                if(buttons_prev[4]!=1) {
                                        buttons[4] = ~buttons[4];
                                        buttons_prev[4] = 1;
@@ -129,7 +129,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[4] = 0;
 
                        /* button6 - turn brush drives off */
-                       if (robot.gorte.joy_data.button6 == 1) {
+                       if (robot.joy_data.button6 == 1) {
                                if(buttons_prev[5]!=1) {
                                        buttons[5] = ~buttons[5];
                                        buttons_prev[5] = 1;
@@ -141,7 +141,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[5] = 0;
 
                        /* button7 */
-                       if (robot.gorte.joy_data.button7 == 1)  {
+                       if (robot.joy_data.button7 == 1)  {
                                if(buttons_prev[6]!=1) {
                                        buttons[6] = ~buttons[6];
                                        buttons_prev[6] = 1;
@@ -151,7 +151,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[6] = 0;
 
                        /* button8 */
-                       if (robot.gorte.joy_data.button8 == 1) {
+                       if (robot.joy_data.button8 == 1) {
                                if(buttons_prev[7]!=1) {
                                        buttons[7] = ~buttons[7];
                                        buttons_prev[7] = 1;
@@ -161,7 +161,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[7] = 0;
 
                        /* button9 */
-                       if (robot.gorte.joy_data.button9 == 1) {
+                       if (robot.joy_data.button9 == 1) {
                                if(buttons_prev[8]!=1) {
                                        buttons[8] = ~buttons[8];
                                        buttons_prev[8] = 1;
@@ -171,7 +171,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[8] = 0;
 
                        /* button10 */
-                       if (robot.gorte.joy_data.button10 == 1) {
+                       if (robot.joy_data.button10 == 1) {
                                if(buttons_prev[9]!=1) {
                                        buttons[9] = ~buttons[9];
                                        buttons_prev[9] = 1;
@@ -181,7 +181,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[9] = 0;
 
                        /* button11 */
-                       if (robot.gorte.joy_data.button11 == 1) {
+                       if (robot.joy_data.button11 == 1) {
                                if(buttons_prev[10]!=1) {
                                        buttons[10] = ~buttons[10];
                                        buttons_prev[10] = 1;
@@ -191,7 +191,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[10] = 0;
 
                        /* button12 */
-                       if (robot.gorte.joy_data.button12 == 1) {
+                       if (robot.joy_data.button12 == 1) {
                                if(buttons_prev[11]!=1) {
                                        buttons[11] = ~buttons[11];
                                        buttons_prev[11] = 1;
@@ -201,7 +201,7 @@ FSM_STATE(joy_scan)
                                buttons_prev[11] = 0;
 
                        /* axis - carousel left/right */
-                       if (robot.gorte.joy_data.axisS1 == -32767) {
+                       if (robot.joy_data.axisS1 == -32767) {
                                if(axis_prev[3] != 1) {
                                        axis[3] = ~axis[3];
                                        axis_prev[3] = 1;
@@ -213,7 +213,7 @@ FSM_STATE(joy_scan)
                        } else
                                axis_prev[3] = 0;
 
-                       if (robot.gorte.joy_data.axisS1 == 32767) {
+                       if (robot.joy_data.axisS1 == 32767) {
                                if(axis_prev[4] != 1) {
                                        axis[4] = ~axis[4];
                                        axis_prev[4] = 1;
@@ -226,17 +226,17 @@ FSM_STATE(joy_scan)
                                axis_prev[4] = 0;
 
                        /* axis - brush in/out */
-                       if (robot.gorte.joy_data.axisS2 == -32767) {
+                       if (robot.joy_data.axisS2 == -32767) {
                                brushes_in();
                        }
 
-                       if (robot.gorte.joy_data.axisS2 == 32767) {
+                       if (robot.joy_data.axisS2 == 32767) {
                                brushes_out();
                        }
 
-                       if (prev_axisR != robot.gorte.joy_data.axisR) {
-                               set_bagr(150 + (-robot.gorte.joy_data.axisR * 50 / 32767));
-                               prev_axisR = robot.gorte.joy_data.axisR;
+                       if (prev_axisR != robot.joy_data.axisR) {
+                               set_bagr(150 + (-robot.joy_data.axisR * 50 / 32767));
+                               prev_axisR = robot.joy_data.axisR;
                        }
 
 

diff --git a/src/robofsm/eb2008/fsmloc.c b/src/robofsm/eb2008/fsmloc.c
index 98da75b08f478648c0a4dd71aabbf5437da81393..a1bd37ba59dd2ba2e69e48236ea11a911ea8495c 100644 (file)
--- a/src/robofsm/eb2008/fsmloc.c
+++ b/src/robofsm/eb2008/fsmloc.c
@@ -102,33 +102,10 @@ FSM_STATE(loc_run)
  */
 FSM_STATE(loc_update)
 {
-       static unsigned int times[LAS_CNT];
-       static int cnt;
        static struct mcl_particle est_pos;
-       int i;
 
        DBG_PRINT_EVENT("loc_update:");
 
-       ROBOT_LOCK(laser_meas);
-       cnt = robot.orte.laser_meas.cnt + 1;
-       times[0] = robot.orte.laser_meas.period;
-       times[1] = robot.orte.laser_meas.measures0;
-       times[2] = robot.orte.laser_meas.measures1;
-       times[3] = robot.orte.laser_meas.measures2;
-       times[4] = robot.orte.laser_meas.measures3;
-       times[5] = robot.orte.laser_meas.measures4;
-       times[6] = robot.orte.laser_meas.measures5;
-       times[7] = robot.orte.laser_meas.measures6;
-       times[8] = robot.orte.laser_meas.measures7;
-       times[9] = robot.orte.laser_meas.measures8;
-       times[10] = robot.orte.laser_meas.measures9;
-       ROBOT_UNLOCK(laser_meas);
-
-       meas_angles.count = cnt;
-       for (i=0; i<cnt; i++) {
-               meas_angles.val[i] = (double)TIME2ANGLE(times[0],times[i+1]);
-       }
-
        mcl.update(&mcl, mcl.data);
        mcl.normalize(&mcl);
        mcl.sort(&mcl);
@@ -136,9 +113,9 @@ FSM_STATE(loc_update)
        est_pos = mcl_get_pos(&mcl);
 
        ROBOT_LOCK(est_pos);
-       robot.gorte.est_pos.x = est_pos.x;
-       robot.gorte.est_pos.y = est_pos.y;
-       robot.gorte.est_pos.phi = est_pos.angle;
+       robot.est_pos.x = est_pos.x;
+       robot.est_pos.y = est_pos.y;
+       robot.est_pos.phi = est_pos.angle;
        ROBOT_UNLOCK(est_pos);
 
        FSM_SIGNAL(MOTION, EV_LOC_UPDATED, NULL);

diff --git a/src/robofsm/eb2008/fsmmove.cc b/src/robofsm/eb2008/fsmmove.cc
index e5936c437de8a3ff2de1aad37678f2b39022cde3..8d3d945035788c5da4e8b605c6dff3d61cc7d527 100644 (file)
--- a/src/robofsm/eb2008/fsmmove.cc
+++ b/src/robofsm/eb2008/fsmmove.cc
@@ -80,9 +80,9 @@ pthread_mutex_t switch_to_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
 // static void odometry(double l, r) {
 //     static double last_l, last_r;
 
-//     robot.act_pos->x = (double)ROBOT_AXIS_TO_BACK_MM/1000;
-//     robot.act_pos->y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2)/1000;
-//     robot.act_pos->phi = 0;
+//     robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000;
+//     robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2)/1000;
+//     robot.act_pos.phi = 0;
 
 //     ROBOT_LOCK(act_pos);
 //     ROBOT_UNLOCK(act_pos);
@@ -194,7 +194,7 @@ static void *trajectory_follower(void *arg)
                        robot_set_act_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
 #endif
                        ROBOT_LOCK(act_pos);
-                       ap = *robot.act_pos;
+                       ap = robot.act_pos;
                        ROBOT_UNLOCK(act_pos);
 
                        double aktPos[] = { ap.x, ap.y, ap.phi };
@@ -243,7 +243,7 @@ static void obstacle_detected_at(double x, double y)
         * #OBS_CSPACE. */
 
        ROBOT_LOCK(act_pos);
-       act_pos = *robot.act_pos;
+       act_pos = robot.act_pos;
        ROBOT_UNLOCK(act_pos);
 
        for (i=(xcell-OBS_AREA); i <= xcell+OBS_AREA; i++) {
@@ -296,18 +296,18 @@ static void * trajctory_recalc(void * arg)
                /** Updates Map. */
                //ShmapUpdateTmpObstacles(OBS_UPDATE_MAP_SPEED);
                /* FIXME: here should be data from opponent sharp sensors */
-               ROBOT_LOCK(sharpsOpponent);
+               ROBOT_LOCK(sharp_opponent);
                val[0] = 2.0;
                val[1] = 2.0;
                val[2] = 2.0;
-               ROBOT_UNLOCK(sharpsOpponent);
+               ROBOT_UNLOCK(sharp_opponent);
                
                const float ang[NUM_SHARPS] = {DEG2RAD(22), DEG2RAD(0), DEG2RAD(-22)};
                for (i=0; i<NUM_SHARPS; i++) {
                        if(val[i] < OBS_OFFSET) {
                                /** Get actual position. */
                                ROBOT_LOCK(act_pos);
-                               p = *robot.act_pos;
+                               p = robot.act_pos;
                                ROBOT_UNLOCK(act_pos);
                                /**
                                * The obstacle cell its calulated as (val*cos(p.phi)) + p.x) and  ((val*sin(p.phi)) + p.y)
@@ -386,11 +386,11 @@ static int new_goal(void)
 
        /// Get actual position.
        ROBOT_LOCK(act_pos);
-       startx = robot.act_pos->x;
-       starty = robot.act_pos->y;
+       startx = robot.act_pos.x;
+       starty = robot.act_pos.y;
        start.x = startx;
        start.y = starty;
-       start.phi = robot.act_pos->phi;
+       start.phi = robot.act_pos.phi;
        tc = robot.goal_trajectory_constraints;
        ROBOT_UNLOCK(act_pos);
        /// Get goal position.
@@ -477,9 +477,9 @@ static int new_trajectory(void)
 
 
        ROBOT_LOCK(act_pos);
-       pos.x = robot.act_pos->x;
-       pos.y = robot.act_pos->y;
-       pos.phi = robot.act_pos->phi;
+       pos.x = robot.act_pos.x;
+       pos.y = robot.act_pos.y;
+       pos.phi = robot.act_pos.phi;
        ROBOT_UNLOCK(act_pos);
 
        if (t->prepare(pos)) {

diff --git a/src/robofsm/eb2008/movehelper_eb2008.cc b/src/robofsm/eb2008/movehelper_eb2008.cc
index b88c0e4ac4065738ec701e82ccda69af44ff2e7d..fe327e6f56238e399210323a4e5f469928de2e41 100644 (file)
--- a/src/robofsm/eb2008/movehelper_eb2008.cc
+++ b/src/robofsm/eb2008/movehelper_eb2008.cc
@@ -84,9 +84,9 @@ void robot_set_act_pos_notrans(double x, double y, double phi)
        if (y<0) y=0;
        if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
        ROBOT_LOCK(act_pos);
-       robot.act_pos->x = x;
-       robot.act_pos->y = y;
-       robot.act_pos->phi = phi;
+       robot.act_pos.x = x;
+       robot.act_pos.y = y;
+       robot.act_pos.phi = phi;
        ROBOT_UNLOCK(act_pos);
 }
 

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index 493520149e3d7e1e2b3e21971584bbe2792d62f8..39dd23bb36a0c22fdc254797b213d0bcef613dce 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -120,6 +120,9 @@ int robot_init()
 
        pthread_mutex_init(&robot.lock, NULL);
        pthread_mutex_init(&robot.lock_act_pos, NULL);
+       pthread_mutex_init(&robot.lock_mcl, NULL);
+       pthread_mutex_init(&robot.lock_meas_angles, NULL);
+       pthread_mutex_init(&robot.lock_joy_data, NULL);
 
        /* FSM initialization */
        for (i=0; i<FSM_CNT; i++)
@@ -167,10 +170,6 @@ int robot_init()
        /* init ORTE domain, create publishers, subscribers, .. */
        robot_init_orte();
 
-       /* just for easier manipulation with this variables */
-       robot.act_pos = &robot.gorte.act_pos;
-       robot.est_pos = &robot.gorte.est_pos;
-       
        return 0;
 }
 /** 

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index 934830b0d5807b5d8d5ba655aa1c9eb2ff4ece07..c1c6c1bca3bd5bee53954567757a279f1f951919 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -63,15 +63,12 @@ enum robot_fsm_id {
 #define __robot_lock_act_pos           lock_act_pos
 #define __robot_lock_est_pos           lock
 #define __robot_lock_des_pos           lock
-#define __robot_lock_mcl               lock_mcl
 #define __robot_lock_goal              lock
-#define __robot_lock_IRsensors         lock
-#define __robot_lock_joy_data          lock
-#define __robot_lock_laser_meas                lock
+#define __robot_lock_mcl               lock_mcl
+#define __robot_lock_joy_data          lock_joy_data
+#define __robot_lock_meas_angles       lock_meas_angles
 #define __robot_lock_new_trajectory    lock
-#define __robot_lock_sharpsOpponent    lock
-#define __robot_lock_sharpsWaste       lock
-#define __robot_lock_waste_cnt         lock
+#define __robot_lock_sharp_opponent    lock
 
 /* robot's main data structure */
 struct robot_eb2008 {
@@ -80,6 +77,8 @@ struct robot_eb2008 {
        pthread_mutex_t lock;
        pthread_mutex_t lock_act_pos;
        pthread_mutex_t lock_mcl;
+       pthread_mutex_t lock_meas_angles;
+       pthread_mutex_t lock_joy_data;
 
        /* competition parameters */
        unsigned char team_color;
@@ -96,9 +95,9 @@ struct robot_eb2008 {
        struct robo_fsm fsm[FSM_CNT];
 
        /* actual position */
-       struct act_pos_type *act_pos;
+       struct act_pos_type act_pos;
        /* estimated position */
-       struct est_pos_type *est_pos;
+       struct est_pos_type est_pos;
 
        /* goal position */
        struct act_pos_type goal;
@@ -107,12 +106,13 @@ struct robot_eb2008 {
        /* move */
        struct act_pos_type move_dpos;
 
+       struct motion_irc_type odometry;
+       struct joy_data_type joy_data;
+
        /* orte */
        struct eb2008_orte_data orte;
        struct generic_orte_data gorte;
 
-       struct motion_irc_type odometry;
-
        /* mcl */
        struct mcl_model mcl;
        struct mcl_angles meas_angles;;

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index 0de2dfbf06b6e11d5585bf46799bd46e35ce0d61..a2996329104388c7584570b6e615815d2f97f680 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -21,6 +21,26 @@
  * PUBLISHER CALLBACKS - GENERIC
  * ---------------------------------------------------------------------- */
 
+void send_act_pos_cb(const ORTESendInfo *info, void *vinstance, 
+                       void *sendCallBackParam)
+{
+       struct act_pos_type *instance = (struct act_pos_type *)vinstance;
+       
+       ROBOT_LOCK(act_pos);
+       *instance = robot.act_pos;
+       ROBOT_UNLOCK(act_pos);
+}
+
+void send_est_pos_cb(const ORTESendInfo *info, void *vinstance, 
+                       void *sendCallBackParam)
+{
+       struct est_pos_type *instance = (struct est_pos_type *)vinstance;
+       
+       ROBOT_LOCK(est_pos);
+       *instance = robot.est_pos;
+       ROBOT_UNLOCK(est_pos);
+}
+
 void send_dummy_cb(const ORTESendInfo *info, void *vinstance, 
                        void *sendCallBackParam)
 {
@@ -63,7 +83,7 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);
 
                        ROBOT_LOCK(act_pos);
-                       aktUhel = robot.act_pos->phi;
+                       aktUhel = robot.act_pos.phi;
                        ROBOT_UNLOCK(act_pos);
                        
                        /* aktualni uhel */
@@ -78,9 +98,9 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                                printf("dx=%f dy=%f dphi=%f\n", dx, dy, deltAlfa);*/
 
                        ROBOT_LOCK(act_pos);
-                       x = robot.act_pos->x + dx;
-                       y = robot.act_pos->y + dy;
-                       phi = robot.act_pos->phi = aktUhel;
+                       x = robot.act_pos.x + dx;
+                       y = robot.act_pos.y + dy;
+                       phi = robot.act_pos.phi = aktUhel;
                        ROBOT_UNLOCK(act_pos);
                        robot_set_act_pos_notrans(x, y, phi);
 
@@ -132,7 +152,9 @@ void rcv_act_pos_cb(const ORTERecvInfo *info, void *vinstance,
 
        switch (info->status) {
                case NEW_DATA:
-                       instance = &(robot.gorte.act_pos);
+                       ROBOT_LOCK(act_pos);
+                       robot.act_pos = *instance;
+                       ROBOT_UNLOCK(act_pos);
                        break;
                case DEADLINE:
                        printf("ORTE deadline occurred - act_pos receive\n");
@@ -148,7 +170,7 @@ void rcv_est_pos_cb(const ORTERecvInfo *info, void *vinstance,
        switch (info->status) {
                case NEW_DATA:
                        ROBOT_LOCK(est_pos);
-                       instance = &(robot.gorte.est_pos);
+                       robot.est_pos = *instance;
                        ROBOT_UNLOCK(est_pos);
                        break;
                case DEADLINE:
@@ -160,13 +182,12 @@ void rcv_est_pos_cb(const ORTERecvInfo *info, void *vinstance,
 void rcv_joy_data_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
-       struct joy_data_type *joy = 
-                       (struct joy_data_type *)vinstance;
-       
+       struct joy_data_type *instance = (struct joy_data_type *)vinstance;
+
        switch (info->status) {
                case NEW_DATA:
                        ROBOT_LOCK(joy_data);
-                       robot.gorte.joy_data = *joy;
+                       robot.joy_data = *instance;
                        ROBOT_UNLOCK(joy_data);
                        break;
                case DEADLINE:
@@ -230,30 +251,31 @@ void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
 void rcv_laser_meas_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
+       struct laser_meas_type *instance = (struct laser_meas_type *)vinstance;
        static unsigned int times[LAS_CNT];
        static struct mcl_particle est_pos;
        static int i;
        
        switch (info->status) {
                case NEW_DATA:
-                       ROBOT_LOCK(laser_meas);
-                       robot.meas_angles.count = robot.orte.laser_meas.cnt + 1;
-                       times[0] = robot.orte.laser_meas.period;
-                       times[1] = robot.orte.laser_meas.measures0;
-                       times[2] = robot.orte.laser_meas.measures1;
-                       times[3] = robot.orte.laser_meas.measures2;
-                       times[4] = robot.orte.laser_meas.measures3;
-                       times[5] = robot.orte.laser_meas.measures4;
-                       times[6] = robot.orte.laser_meas.measures5;
-                       times[7] = robot.orte.laser_meas.measures6;
-                       times[8] = robot.orte.laser_meas.measures7;
-                       times[9] = robot.orte.laser_meas.measures8;
-                       times[10] = robot.orte.laser_meas.measures9;
-                       ROBOT_UNLOCK(laser_meas);
-
+                       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;
+
+                       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_UNLOCK(meas_angles);
 
                        ROBOT_LOCK(mcl);
                        robot.mcl.update(&robot.mcl, robot.mcl.data);
@@ -265,9 +287,9 @@ void rcv_laser_meas_cb(const ORTERecvInfo *info, void *vinstance,
                        est_pos = mcl_get_pos(&robot.mcl);
 
                        ROBOT_LOCK(est_pos);
-                       robot.gorte.est_pos.x = est_pos.x;
-                       robot.gorte.est_pos.y = est_pos.y;
-                       robot.gorte.est_pos.phi = est_pos.angle;
+                       robot.est_pos.x = est_pos.x;
+                       robot.est_pos.y = est_pos.y;
+                       robot.est_pos.phi = est_pos.angle;
                        ROBOT_UNLOCK(est_pos);
 
                        break;
@@ -289,8 +311,8 @@ void robot_init_orte()
        generic_publisher_motion_irc_create(&robot.gorte, NULL, &robot.gorte);
        generic_publisher_motion_speed_create(&robot.gorte, NULL, &robot.gorte);
        generic_publisher_motion_status_create(&robot.gorte, NULL, &robot.gorte);
-       generic_publisher_act_pos_create(&robot.gorte, send_dummy_cb, &robot.gorte);
-       generic_publisher_est_pos_create(&robot.gorte, send_dummy_cb, &robot.gorte);
+       generic_publisher_act_pos_create(&robot.gorte, send_act_pos_cb, &robot.gorte);
+       generic_publisher_est_pos_create(&robot.gorte, send_est_pos_cb, &robot.gorte);
        generic_publisher_joy_data_create(&robot.gorte, NULL, &robot.gorte);
        generic_publisher_pwr_voltage_create(&robot.gorte, NULL, &robot.gorte);
        generic_publisher_pwr_ctrl_create(&robot.gorte, NULL, &robot.gorte);

diff --git a/src/robofsm/test/goto.cc b/src/robofsm/test/goto.cc
index 47148ba0388fe9f421408ef9b7e3682038c3908b..78132fa06b84ff2ff65734b2480c8bfcb3da4ee0 100644 (file)
--- a/src/robofsm/test/goto.cc
+++ b/src/robofsm/test/goto.cc
@@ -18,6 +18,7 @@
 #include <sys/shm.h>
 #include <sys/stat.h>
 #include <math.h>
+#include <robomath.h>
 #include <map.h>
 
 
@@ -27,11 +28,7 @@ FSM_STATE(main_init)
 {
        ShmapInit(1);
        /* Where we are at the begining? */
-       ROBOT_LOCK(act_pos);
-       robot.act_pos->x = 0.5;
-       robot.act_pos->y = 0.5;
-       robot.act_pos->phi = 0;
-       ROBOT_UNLOCK(act_pos);
+       robot_set_act_pos_trans(0.5, 0.5, DEG2RAD(0));
        FSM_TRANSITION(robot_goto_test);
 }
 

diff --git a/src/robofsm/test/line.cc b/src/robofsm/test/line.cc
index dab13e823acb36e29892f07983633fdc143683fe..8d8436e3e50429375ae7aa7f960a4051beb1ee73 100644 (file)
--- a/src/robofsm/test/line.cc
+++ b/src/robofsm/test/line.cc
@@ -20,9 +20,7 @@ FSM_STATE_DECL(wait);
 
 FSM_STATE(main_init) {
        /* Where we are at the begining? */
-       robot.act_pos->x = 1;
-       robot.act_pos->y = 0.5;
-       robot.act_pos->phi = 0;
+       robot_set_act_pos_trans(1, 0.5, DEG2RAD(0));
        FSM_TRANSITION(line);
 }