types: act_pos renamed to ref_pos

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 41798fe932e7dfb5550a0adba1749e45b65310a6..f83281b116bcbab4ccf10353af98bbf8fae278a6 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -1,3 +1,3 @@
 # -*- makefile -*-
 
-SUBDIRS = eb2007 eb2008 test utils
+SUBDIRS = eb2008 test utils

diff --git a/src/robofsm/eb2007/fsmmove.cc b/src/robofsm/eb2007/fsmmove.cc
index 08eeca0fc77f3a3e64f4ad100fda18a4b468557e..80facddf2abbfd4deb0ec4b0013e86d83a3eccd7 100644 (file)
--- a/src/robofsm/eb2007/fsmmove.cc
+++ b/src/robofsm/eb2007/fsmmove.cc
@@ -190,17 +190,17 @@ static void *trajectory_follower(void *arg)
                        // to reference position.
                        robot_set_act_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
 #endif
-                       ROBOT_LOCK(act_pos);
-                       act_pos.x = robot.act_pos.x;
-                       act_pos.y = robot.act_pos.y;
-                       act_pos.phi = robot.act_pos.phi;
-                       ROBOT_UNLOCK(act_pos);
+                       ROBOT_LOCK(ref_pos);
+                       ref_pos.x = robot.ref_pos.x;
+                       ref_pos.y = robot.ref_pos.y;
+                       ref_pos.phi = robot.ref_pos.phi;
+                       ROBOT_UNLOCK(ref_pos);
 
                        DBG("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f\r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega);
                        fflush(stdout);
                        // Call the controller
                        double error;
-                       error = balet(ref_pos, act_pos, k, balet_out);
+                       error = balet(ref_pos, ref_pos, k, balet_out);
                        speeds[0] = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
                        speeds[1] = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
                        notify_fsm(done, error);
@@ -219,7 +219,7 @@ static void *trajectory_follower(void *arg)
 static void obstacle_detected_at(double x, double y)
 {
        int i,j, xcell, ycell;
-       struct act_pos_type act_pos;
+       struct ref_pos_type ref_pos;
        struct map *map = ShmapIsMapInit();
        double xx, yy;
        bool obs_already_seen;
@@ -240,15 +240,15 @@ static void obstacle_detected_at(double x, double y)
         * cell between them, the path will be recalculated. @see
         * #OBS_CSPACE. */
 
-       ROBOT_LOCK(act_pos);
-       act_pos = robot.act_pos;
-       ROBOT_UNLOCK(act_pos);
+       ROBOT_LOCK(ref_pos);
+       ref_pos = robot.ref_pos;
+       ROBOT_UNLOCK(ref_pos);
 
        for (i=(xcell-OBS_AREA); i <= xcell+OBS_AREA; i++) {
                for (j=(ycell- OBS_AREA); j <=ycell + OBS_AREA; j++) {
                        if (!ShmapIsCellInMap(i, j)) continue;
                        ShmapCell2Point(i, j, &xx, &yy);
-                       if (distance(xx, yy, act_pos.x, act_pos.y) > 0.2) {
+                       if (distance(xx, yy, ref_pos.x, ref_pos.y) > 0.2) {
                                if (i==xcell && j==ycell) 
                                        map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
                                map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
@@ -287,7 +287,7 @@ static void * trajctory_recalc(void * arg)
 #define NUM_SHARPS 3
        double val[NUM_SHARPS], x, y;
        //Pos p;
-       struct act_pos_type p;
+       struct ref_pos_type p;
        int i;
        
        while (1) {
@@ -303,9 +303,9 @@ static void * trajctory_recalc(void * arg)
                for (i=0; i<NUM_SHARPS; i++) {
                        if(val[i] < OBS_OFFSET) {
                                /** Get actual position. */
-                               ROBOT_LOCK(act_pos);
-                               p = robot.act_pos;
-                               ROBOT_UNLOCK(act_pos);
+                               ROBOT_LOCK(ref_pos);
+                               p = robot.ref_pos;
+                               ROBOT_UNLOCK(ref_pos);
                                /**
                                * The obstacle cell its calulated as (val*cos(p.phi)) + p.x) and  ((val*sin(p.phi)) + p.y)
                                * where p is actual position and val is the value of the sensors in milimiters. Then ShmapPoint2Cell_X() and
@@ -371,7 +371,7 @@ static void stop()
  */
 static int new_goal(void)
 {
-       struct act_pos_type goal;
+       struct ref_pos_type goal;
        double startx, starty, angle;
        PathPoint * path = NULL;
        PathPoint * tmp_point = NULL;
@@ -382,14 +382,14 @@ static int new_goal(void)
        //tc.maxacc *= 2.5;
 
        /// Get actual position.
-       ROBOT_LOCK(act_pos);
-       startx = robot.act_pos.x;
-       starty = robot.act_pos.y;
+       ROBOT_LOCK(ref_pos);
+       startx = robot.ref_pos.x;
+       starty = robot.ref_pos.y;
        start.x = startx;
        start.y = starty;
-       start.phi = robot.act_pos.phi;
+       start.phi = robot.ref_pos.phi;
        tc = robot.goal_trajectory_constraints;
-       ROBOT_UNLOCK(act_pos);
+       ROBOT_UNLOCK(ref_pos);
        /// Get goal position.
        ROBOT_LOCK(goal);
        goal = robot.goal;
@@ -473,11 +473,11 @@ static int new_trajectory(void)
        ROBOT_UNLOCK(new_trajectory);
 
 
-       ROBOT_LOCK(act_pos);
-       pos.x = robot.act_pos.x;
-       pos.y = robot.act_pos.y;
-       pos.phi = robot.act_pos.phi;
-       ROBOT_UNLOCK(act_pos);
+       ROBOT_LOCK(est_pos);
+       pos.x = robot.est_pos.x;
+       pos.y = robot.est_pos.y;
+       pos.phi = robot.est_pos.phi;
+       ROBOT_UNLOCK(est_pos);
 
        if (t->prepare(pos)) {
                go(t);

diff --git a/src/robofsm/eb2007/movehelper_eb2007.cc b/src/robofsm/eb2007/movehelper_eb2007.cc
index 7e52faf29d27aedea08eaa211891ab0fa1084cce..25b6a3a3110dcfa7dbee4acff7463c9e2fce6154 100644 (file)
--- a/src/robofsm/eb2007/movehelper_eb2007.cc
+++ b/src/robofsm/eb2007/movehelper_eb2007.cc
@@ -83,11 +83,11 @@ void robot_set_act_pos_notrans(double x, double y, double phi)
        if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
        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_UNLOCK(act_pos);
+       ROBOT_LOCK(est_pos);
+       robot.est_pos.x = x;
+       robot.est_pos.y = y;
+       robot.est_pos.phi = phi;
+       ROBOT_UNLOCK(est_pos);
 }
 
 /** 

diff --git a/src/robofsm/eb2007/roboorte.c b/src/robofsm/eb2007/roboorte.c
index 897bef1377a791fa884922c9291e520710b48b38..42c95fb905372ca6f33ff493439b068b90f610bd 100644 (file)
--- a/src/robofsm/eb2007/roboorte.c
+++ b/src/robofsm/eb2007/roboorte.c
@@ -175,18 +175,19 @@ static void recvOdoCallBack(const ORTERecvInfo *info,void *vinstance, void *recv
                        deltaU = (dk0 + dk1) / 2;
 
                        deltAlfa = (dk1 - dk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);    // dk1 - dk0
-                       ROBOT_LOCK(act_pos);
-                       aktUhel = robot.act_pos.phi;
-                       ROBOT_UNLOCK(act_pos);
+                       /* FIXME */
+/*                     ROBOT_LOCK(act_pos); */
+/*                     aktUhel = robot.act_pos.phi; */
+/*                     ROBOT_UNLOCK(act_pos); */
                        
                        aktUhel += deltAlfa;            // aktualni uhel
 
                        double x, y, phi;
-                       ROBOT_LOCK(act_pos);
-                       x = robot.act_pos.x + deltaU * cos(aktUhel);
-                       y = robot.act_pos.y + deltaU * sin(aktUhel);
-                       phi = robot.act_pos.phi = aktUhel;
-                       ROBOT_UNLOCK(act_pos);
+/*                     ROBOT_LOCK(act_pos); */
+/*                     x = robot.act_pos.x + deltaU * cos(aktUhel); */
+/*                     y = robot.act_pos.y + deltaU * sin(aktUhel); */
+/*                     phi = robot.act_pos.phi = aktUhel; */
+/*                     ROBOT_UNLOCK(act_pos); */
                        robot_set_act_pos_notrans(x, y, phi);
                        break;
                case DEADLINE:

diff --git a/src/robofsm/eb2007/robot_eb2007.h b/src/robofsm/eb2007/robot_eb2007.h
index 9f886cb79fc111c1fdedfa4f73b811de6c335cb8..a0fe5c58dcd4574fa71a033153ed69e27c7ce68a 100644 (file)
--- a/src/robofsm/eb2007/robot_eb2007.h
+++ b/src/robofsm/eb2007/robot_eb2007.h
@@ -70,7 +70,7 @@ struct bottle_sens {
 
 /* Mapping of robot structure fields to locks */
 //#define __robot_lock_ lock   /* ROBOT_LOCK() */
-#define __robot_lock_act_pos           lock_act_pos
+#define __robot_lock_ref_pos           lock_ref_pos
 #define __robot_lock_est_pos           lock
 #define __robot_lock_des_pos           lock
 #define __robot_lock_goal              lock
@@ -88,7 +88,7 @@ struct robot_eb2007 {
        long long cnt;
        unsigned char   mode;   
        pthread_mutex_t lock;
-       pthread_mutex_t lock_act_pos;
+       pthread_mutex_t lock_ref_pos;
 
        /** Temporary storage for new trajectory. After the trajectory creation is
         * finished, this trajectory is submitted to fsmmove. */
@@ -103,16 +103,16 @@ struct robot_eb2007 {
        /* mcl model */
        struct mcl_model mcl;
        /* actual position */
-       struct act_pos_type act_pos;
+       struct ref_pos_type ref_pos;
 
        /* estimated position */
        struct est_pos_type est_pos;
        /* goal position */
-       struct act_pos_type goal;
+       struct ref_pos_type goal;
        struct TrajectoryConstraints goal_trajectory_constraints;
        struct final_heading goal_final_heading;
        /* move */
-       struct act_pos_type move_dpos;
+       struct ref_pos_type move_dpos;
 
        /* orte */
        struct laserData_type laser_recv;

diff --git a/src/robofsm/eb2008/fsmdisplay.cc b/src/robofsm/eb2008/fsmdisplay.cc
index bfc83734f3758a33857c2e1212d6a1d00f8824de..84e467ab6b8fb59d27e80d4619b0ea342c06d4e1 100644 (file)
--- a/src/robofsm/eb2008/fsmdisplay.cc
+++ b/src/robofsm/eb2008/fsmdisplay.cc
@@ -185,7 +185,7 @@ FSM_STATE(status)
                        break;
                case EV_TIMER:
                        uoled_send_voltage(&robot.gorte.pwr_voltage);
-                       uoled_send_position(&robot.gorte.act_pos);
+                       uoled_send_position(&robot.gorte.est_pos);
                        ROBOT_LOCK(disp);
                        if(msg_waiting) {
                                memcpy(buff, msg, 10);

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index 3ac050d978d8167dd73796db22cede1c2a5cb758..1f0c07263f21ae7c49ba61cf16c7be658ce0741c 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -40,9 +40,6 @@
 /* competition time in seconds */
 
 
-struct act_pos_type des_pos;
-
-
 enum {
        LEFT = 0,
        RIGHT,
@@ -178,16 +175,15 @@ void *wait_to_deposition(void *arg)
  * @param act  actual position
  * @param pos  countered position
  */
-void get_new_pos(struct act_pos_type *act, struct act_pos_type *pos, 
+void get_relative_pos(struct est_pos_type *est, struct ref_pos_type *ref, 
                        double l, double phi)
 {
-       pos->x = act->x + l*cos(act->phi + phi);
-       pos->y = act->y + l*sin(act->phi + phi);
-//     pos->phi = act->phi + phi;
-       pos->phi = phi;
+       ref->x = est->x + l*cos(est->phi + phi);
+       ref->y = est->y + l*sin(est->phi + phi);
+       ref->phi = est->phi + phi;
 }
 
-void robot_goto_point(struct act_pos_type des_pos)
+void robot_goto_point(struct ref_pos_type des_pos)
 {
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 
@@ -196,7 +192,7 @@ void robot_goto_point(struct act_pos_type des_pos)
        robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
 }
 
-void robot_go_backward_to_point(struct act_pos_type des_pos)
+void robot_go_backward_to_point(struct ref_pos_type des_pos)
 {
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 

diff --git a/src/robofsm/eb2008/fsmmove.cc b/src/robofsm/eb2008/fsmmove.cc
index cb23745e2dc33d10e06291bae441afb74f90c226..7db62b57131b40541c112a1306f45c36987ef773 100644 (file)
--- a/src/robofsm/eb2008/fsmmove.cc
+++ b/src/robofsm/eb2008/fsmmove.cc
@@ -4,6 +4,7 @@
  * @author     Michal Sojka, Jose Maria Martin Laguna
  *
  */
+#define DEBUG
 #define FSM_MOTION
 #include <sys/time.h>
 #include <time.h>
@@ -191,11 +192,11 @@ static void *trajectory_follower(void *arg)
                        // to reference position.
                        robot_set_act_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
 #endif
-                       ROBOT_LOCK(act_pos);
-                       act_pos.x = robot.act_pos.x;
-                       act_pos.y = robot.act_pos.y;
-                       act_pos.phi = robot.act_pos.phi;
-                       ROBOT_UNLOCK(act_pos);
+                       ROBOT_LOCK(est_pos);
+                       act_pos.x = robot.est_pos.x;
+                       act_pos.y = robot.est_pos.y;
+                       act_pos.phi = robot.est_pos.phi;
+                       ROBOT_UNLOCK(est_pos);
 
                        DBG("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f\r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega);
                        fflush(stdout);
@@ -220,11 +221,14 @@ static void *trajectory_follower(void *arg)
 static void obstacle_detected_at(double x, double y)
 {
        int i,j, xcell, ycell;
-       struct act_pos_type act_pos;
-       struct map *map = ShmapIsMapInit();
+       struct est_pos_type est_pos;
+       struct map *map = robot.map;
        double xx, yy;
        bool obs_already_seen;
 
+       if (!map)
+               return;
+
        ShmapPoint2Cell(x, y, &xcell, &ycell);
        if (!ShmapIsCellInMap(xcell, ycell))
                return;
@@ -241,15 +245,15 @@ static void obstacle_detected_at(double x, double y)
         * cell between them, the path will be recalculated. @see
         * #OBS_CSPACE. */
 
-       ROBOT_LOCK(act_pos);
-       act_pos = robot.act_pos;
-       ROBOT_UNLOCK(act_pos);
+       ROBOT_LOCK(est_pos);
+       est_pos = robot.est_pos;
+       ROBOT_UNLOCK(est_pos);
 
        for (i=(xcell-OBS_AREA); i <= xcell+OBS_AREA; i++) {
                for (j=(ycell- OBS_AREA); j <=ycell + OBS_AREA; j++) {
                        if (!ShmapIsCellInMap(i, j)) continue;
                        ShmapCell2Point(i, j, &xx, &yy);
-                       if (distance(xx, yy, act_pos.x, act_pos.y) > 0.2) {
+                       if (distance(xx, yy, est_pos.x, est_pos.y) > 0.2) {
                                if (i==xcell && j==ycell) 
                                        map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
                                map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
@@ -288,7 +292,7 @@ static void * trajctory_recalc(void * arg)
 #define NUM_SHARPS 3
        double val[NUM_SHARPS], x, y;
        //Pos p;
-       struct act_pos_type p;
+       struct est_pos_type p;
        int i;
        
        while (1) {
@@ -304,9 +308,9 @@ static void * trajctory_recalc(void * arg)
                for (i=0; i<NUM_SHARPS; i++) {
                        if(val[i] < OBS_OFFSET) {
                                /** Get actual position. */
-                               ROBOT_LOCK(act_pos);
-                               p = robot.act_pos;
-                               ROBOT_UNLOCK(act_pos);
+                               ROBOT_LOCK(est_pos);
+                               p = robot.est_pos;
+                               ROBOT_UNLOCK(est_pos);
                                /**
                                * The obstacle cell its calulated as (val*cos(p.phi)) + p.x) and  ((val*sin(p.phi)) + p.y)
                                * where p is actual position and val is the value of the sensors in milimiters. Then ShmapPoint2Cell_X() and
@@ -372,7 +376,7 @@ static void stop()
  */
 static int new_goal(void)
 {
-       struct act_pos_type goal;
+       struct ref_pos_type goal;
        double startx, starty, angle;
        PathPoint * path = NULL;
        PathPoint * tmp_point = NULL;
@@ -383,14 +387,14 @@ static int new_goal(void)
        //tc.maxacc *= 2.5;
 
        /// Get actual position.
-       ROBOT_LOCK(act_pos);
-       startx = robot.act_pos.x;
-       starty = robot.act_pos.y;
+       ROBOT_LOCK(est_pos);
+       startx = robot.est_pos.x;
+       starty = robot.est_pos.y;
        start.x = startx;
        start.y = starty;
-       start.phi = robot.act_pos.phi;
+       start.phi = robot.est_pos.phi;
        tc = robot.goal_trajectory_constraints;
-       ROBOT_UNLOCK(act_pos);
+       ROBOT_UNLOCK(est_pos);
        /// Get goal position.
        ROBOT_LOCK(goal);
        goal = robot.goal;
@@ -474,11 +478,11 @@ static int new_trajectory(void)
        ROBOT_UNLOCK(new_trajectory);
 
 
-       ROBOT_LOCK(act_pos);
-       pos.x = robot.act_pos.x;
-       pos.y = robot.act_pos.y;
-       pos.phi = robot.act_pos.phi;
-       ROBOT_UNLOCK(act_pos);
+       ROBOT_LOCK(est_pos);
+       pos.x = robot.est_pos.x;
+       pos.y = robot.est_pos.y;
+       pos.phi = robot.est_pos.phi;
+       ROBOT_UNLOCK(est_pos);
 
        if (t->prepare(pos)) {
                go(t);
@@ -530,7 +534,7 @@ FSM_STATE(move_init)
                        perror("move_init: pthread_attr_setschedparam(recalc)");
                        exit(1);
                }
-               pthread_create(&thr_trajctory_recalc, NULL, trajctory_recalc, NULL);
+               //pthread_create(&thr_trajctory_recalc, NULL, trajctory_recalc, NULL);
                
                // Thread update Map
                pthread_attr_init (&tattr);
@@ -541,7 +545,7 @@ FSM_STATE(move_init)
                        perror("pthread_attr_setschedparam");
                        exit(1);
                }
-               pthread_create(&thr_update_map, NULL, thread_update_map, NULL);
+               //pthread_create(&thr_update_map, NULL, thread_update_map, NULL);
                
                FSM_TRANSITION(wait_for_command);
        }

diff --git a/src/robofsm/eb2008/movehelper_eb2008.cc b/src/robofsm/eb2008/movehelper_eb2008.cc
index fe327e6f56238e399210323a4e5f469928de2e41..83ca5a0976d64265bbe4eeedb6d50779f20043cf 100644 (file)
--- a/src/robofsm/eb2008/movehelper_eb2008.cc
+++ b/src/robofsm/eb2008/movehelper_eb2008.cc
@@ -83,11 +83,12 @@ void robot_set_act_pos_notrans(double x, double y, double phi)
        if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
        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_UNLOCK(act_pos);
+       ROBOT_LOCK(ref_pos);
+       robot.ref_pos.x = x;
+       robot.ref_pos.y = y;
+       robot.ref_pos.phi = phi;
+       ROBOT_UNLOCK(ref_pos);
+       // FIXME: Initialize MCL estimate
 }
 
 /** 

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index 843a8fc7efad242899f8a7db74581e75b72945f2..686d62ed42f73b23b6147f00ccdb3608f7b668da 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -15,6 +15,7 @@
 #include <movehelper_eb2008.h>
 #include <robot_orte.h>
 #include <robomath.h>
+#include <map.h>
 
 #define THREAD_PRIO_MOTION     30
 #define THREAD_PRIO_MAIN       25
@@ -73,8 +74,8 @@ int robot_init()
        robot.beacon_color = BEACON_BLUE;
 
        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_ref_pos, NULL);
+       pthread_mutex_init(&robot.lock_est_pos, NULL);
        pthread_mutex_init(&robot.lock_meas_angles, NULL);
        pthread_mutex_init(&robot.lock_joy_data, NULL);
        pthread_mutex_init(&robot.lock_disp, NULL);
@@ -97,6 +98,8 @@ int robot_init()
                printf("We are BLUE!\n");
        }
 
+       robot.map = ShmapInit(1);
+
        signal(SIGINT, int_handler);
        block_signals();
 

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index 464464116e70278eb651b909e8780c01a0d8bac3..32d3a6e49e6e40c570c248b585a1764f4f72cae5 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -60,11 +60,10 @@ enum robot_fsm_id {
 
 /* Mapping of robot structure fields to locks */
 //#define __robot_lock_ lock   /* ROBOT_LOCK() */
-#define __robot_lock_act_pos           lock_act_pos
-#define __robot_lock_est_pos           lock
+#define __robot_lock_ref_pos           lock_ref_pos
+#define __robot_lock_est_pos           lock_est_pos
 #define __robot_lock_des_pos           lock
 #define __robot_lock_goal              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
@@ -76,8 +75,8 @@ struct robot_eb2008 {
        long long cnt;
        unsigned char   mode;   
        pthread_mutex_t lock;
-       pthread_mutex_t lock_act_pos;
-       pthread_mutex_t lock_mcl;
+       pthread_mutex_t lock_ref_pos;
+       pthread_mutex_t lock_est_pos;
        pthread_mutex_t lock_meas_angles;
        pthread_mutex_t lock_joy_data;
        pthread_mutex_t lock_disp;
@@ -97,17 +96,14 @@ struct robot_eb2008 {
        struct robo_fsm fsm[FSM_CNT];
 
        /* actual position */
-/*  FIXME: What is the difference between act_pos and est_pos? Probably should be renamed to ref_pos. -- MS */
-       struct act_pos_type act_pos;
+       struct ref_pos_type ref_pos;
        /* estimated position */
        struct est_pos_type est_pos;
 
-       /* goal position */
-       struct act_pos_type goal;
+       /* goal position */ /* FIXME: Move away */
+       struct ref_pos_type goal; 
        struct TrajectoryConstraints goal_trajectory_constraints;
        struct final_heading goal_final_heading;
-       /* move */
-       struct act_pos_type move_dpos;
 
        struct motion_irc_type odometry;
        struct joy_data_type joy_data;
@@ -122,6 +118,8 @@ struct robot_eb2008 {
        /* mcl */
        struct mcl_model *mcl;  /* Pointer to the generic MCL model */
        struct mcl_laser laser; /* MCL implementation for laser */
+
+       struct map *map;        /* Map for pathplanning (no locking) */
 }; /* robot_eb2008 */
 
 extern struct robot_eb2008 robot;

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index 8369b67d4ee6217ab7090fc3dde45d3a0be410d4..d6f9ad65f6165e60d1854a0879ff0ba7ad4fa3b9 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -22,14 +22,14 @@
  * PUBLISHER CALLBACKS - GENERIC
  * ---------------------------------------------------------------------- */
 
-void send_act_pos_cb(const ORTESendInfo *info, void *vinstance, 
+void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance, 
                        void *sendCallBackParam)
 {
-       struct act_pos_type *instance = (struct act_pos_type *)vinstance;
+       struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
        
-       ROBOT_LOCK(act_pos);
-       *instance = robot.act_pos;
-       ROBOT_UNLOCK(act_pos);
+       ROBOT_LOCK(ref_pos);
+       *instance = robot.ref_pos;
+       ROBOT_UNLOCK(ref_pos);
 }
 
 void send_est_pos_cb(const ORTESendInfo *info, void *vinstance, 
@@ -82,11 +82,11 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        deltaU = (aktk0 + aktk1) / 2;
                        deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
 
-                       struct mcl_robot_odo odo = {deltaU, 0, deltAlfa};
-                       ROBOT_LOCK(mcl);
-                       robot.mcl->predict(robot.mcl, &odo);
-                       ROBOT_UNLOCK(mcl);
-
+                       struct mcl_robot_odo *odo = malloc(sizeof(struct mcl_robot_odo));
+                       odo->dx = deltaU;
+                       odo->dy = 0;
+                       odo->dangle = deltAlfa;
+                       FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo);
                        break;
                case DEADLINE:
                        printf("ORTE deadline occurred - motion_irc receive\n");
@@ -118,40 +118,6 @@ void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
-void rcv_act_pos_cb(const ORTERecvInfo *info, void *vinstance,
-                       void *recvCallBackParam)
-{
-       struct act_pos_type *instance = (struct act_pos_type *)vinstance;
-
-       switch (info->status) {
-               case NEW_DATA:
-                       ROBOT_LOCK(act_pos);
-                       robot.act_pos = *instance;
-                       ROBOT_UNLOCK(act_pos);
-                       break;
-               case DEADLINE:
-                       printf("ORTE deadline occurred - act_pos receive\n");
-                       break;
-       }
-}
-
-void rcv_est_pos_cb(const ORTERecvInfo *info, void *vinstance,
-                       void *recvCallBackParam)
-{
-       struct est_pos_type *instance = (struct est_pos_type *)vinstance;
-       
-       switch (info->status) {
-               case NEW_DATA:
-                       ROBOT_LOCK(est_pos);
-                       robot.est_pos = *instance;
-                       ROBOT_UNLOCK(est_pos);
-                       break;
-               case DEADLINE:
-                       printf("ORTE deadline occurred - est_pos receive\n");
-                       break;
-       }
-}
-
 void rcv_joy_data_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
@@ -263,7 +229,7 @@ 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_act_pos_cb, &robot.gorte);
+       generic_publisher_ref_pos_create(&robot.gorte, send_ref_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);
@@ -276,8 +242,6 @@ void robot_init_orte()
        generic_subscriber_motion_irc_create(&robot.gorte, rcv_motion_irc_cb, &robot.gorte);
        generic_subscriber_motion_speed_create(&robot.gorte, rcv_motion_speed_cb, &robot.gorte);
        generic_subscriber_motion_status_create(&robot.gorte, rcv_motion_status_cb, &robot.gorte);
-       generic_subscriber_act_pos_create(&robot.gorte, rcv_act_pos_cb, &robot.gorte);
-       generic_subscriber_est_pos_create(&robot.gorte, rcv_est_pos_cb, &robot.gorte);
        generic_subscriber_joy_data_create(&robot.gorte, rcv_joy_data_cb, &robot.gorte);
        generic_subscriber_pwr_voltage_create(&robot.gorte, rcv_pwr_voltage_cb, &robot.gorte);
        generic_subscriber_pwr_ctrl_create(&robot.gorte, rcv_pwr_ctrl_cb, &robot.gorte);

diff --git a/src/types/robottype.c b/src/types/robottype.c
index 5c464102a94f76863ae934179c110cea312c5b9e..c9f4a1955636acfa209ab323110d9a63e38480d3 100644 (file)
--- a/src/types/robottype.c
+++ b/src/types/robottype.c
@@ -110,24 +110,24 @@ motion_status_type_register(ORTEDomain *d) {
 }
 
 /****************************************************************/
-/* struct - act_pos                                             */
+/* struct - ref_pos                                             */
 /****************************************************************/
 
-void act_pos_serialize(CDR_Codec *cdrCodec,act_pos *object) {
+void ref_pos_serialize(CDR_Codec *cdrCodec,ref_pos *object) {
   CORBA_double_serialize(cdrCodec,&(object->x));
   CORBA_double_serialize(cdrCodec,&(object->y));
   CORBA_double_serialize(cdrCodec,&(object->phi));
 }
 
 void
-act_pos_deserialize(CDR_Codec *cdrCodec,act_pos *object) {
+ref_pos_deserialize(CDR_Codec *cdrCodec,ref_pos *object) {
   CORBA_double_deserialize(cdrCodec,&(object->x));
   CORBA_double_deserialize(cdrCodec,&(object->y));
   CORBA_double_deserialize(cdrCodec,&(object->phi));
 }
 
 int
-act_pos_get_max_size(ORTEGetMaxSizeParam *gms) {
+ref_pos_get_max_size(ORTEGetMaxSizeParam *gms) {
   CORBA_double_get_max_size(gms);
   CORBA_double_get_max_size(gms);
   CORBA_double_get_max_size(gms);
@@ -135,14 +135,14 @@ act_pos_get_max_size(ORTEGetMaxSizeParam *gms) {
 }
 
 Boolean
-act_pos_type_register(ORTEDomain *d) {
+ref_pos_type_register(ORTEDomain *d) {
   Boolean ret;
 
   ret=ORTETypeRegisterAdd(d,
-                          "act_pos",
-                          (ORTETypeSerialize)act_pos_serialize,
-                          (ORTETypeDeserialize)act_pos_deserialize,
-                          act_pos_get_max_size,
+                          "ref_pos",
+                          (ORTETypeSerialize)ref_pos_serialize,
+                          (ORTETypeDeserialize)ref_pos_deserialize,
+                          ref_pos_get_max_size,
                           0);
   return ret;
 }
@@ -155,7 +155,6 @@ void est_pos_serialize(CDR_Codec *cdrCodec,est_pos *object) {
   CORBA_double_serialize(cdrCodec,&(object->x));
   CORBA_double_serialize(cdrCodec,&(object->y));
   CORBA_double_serialize(cdrCodec,&(object->phi));
-  CORBA_double_serialize(cdrCodec,&(object->probability));
 }
 
 void
@@ -163,7 +162,6 @@ est_pos_deserialize(CDR_Codec *cdrCodec,est_pos *object) {
   CORBA_double_deserialize(cdrCodec,&(object->x));
   CORBA_double_deserialize(cdrCodec,&(object->y));
   CORBA_double_deserialize(cdrCodec,&(object->phi));
-  CORBA_double_deserialize(cdrCodec,&(object->probability));
 }
 
 int
@@ -171,7 +169,6 @@ est_pos_get_max_size(ORTEGetMaxSizeParam *gms) {
   CORBA_double_get_max_size(gms);
   CORBA_double_get_max_size(gms);
   CORBA_double_get_max_size(gms);
-  CORBA_double_get_max_size(gms);
   return gms->csize;
 }
 

diff --git a/src/types/robottype.h b/src/types/robottype.h
index 0519a0f61148af3330fd79ac9bb9ae7a16fe3bd8..074bb1203c64738339a0725256313206c4b73c8a 100644 (file)
--- a/src/types/robottype.h
+++ b/src/types/robottype.h
@@ -41,10 +41,10 @@ CORBA_unsigned_short err_right;
 };
 
 #endif
-#if !defined(_act_pos_defined)
-#define _act_pos_defined 1
-typedef struct act_pos_type act_pos;
-struct act_pos_type {
+#if !defined(_ref_pos_defined)
+#define _ref_pos_defined 1
+typedef struct ref_pos_type ref_pos;
+struct ref_pos_type {
 CORBA_double x;
 CORBA_double y;
 CORBA_double phi;
@@ -58,7 +58,6 @@ struct est_pos_type {
 CORBA_double x;
 CORBA_double y;
 CORBA_double phi;
-CORBA_double probability;
 };
 
 #endif
@@ -148,10 +147,10 @@ void motion_status_deserialize(CDR_Codec *cdrCodec,motion_status *object);
 int motion_status_get_max_size(ORTEGetMaxSizeParam *gms);
 Boolean motion_status_type_register(ORTEDomain *d);
 
-void act_pos_serialize(CDR_Codec *cdrCodec,act_pos *object);
-void act_pos_deserialize(CDR_Codec *cdrCodec,act_pos *object);
-int act_pos_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean act_pos_type_register(ORTEDomain *d);
+void ref_pos_serialize(CDR_Codec *cdrCodec,ref_pos *object);
+void ref_pos_deserialize(CDR_Codec *cdrCodec,ref_pos *object);
+int ref_pos_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean ref_pos_type_register(ORTEDomain *d);
 
 void est_pos_serialize(CDR_Codec *cdrCodec,est_pos *object);
 void est_pos_deserialize(CDR_Codec *cdrCodec,est_pos *object);

diff --git a/src/types/robottype.idl b/src/types/robottype.idl
index b06df6061b181799afb40556efe494c5b2a4a158..61eb8e1d61aa9697c4bd01599f071a467adb07f5 100644 (file)
--- a/src/types/robottype.idl
+++ b/src/types/robottype.idl
@@ -13,7 +13,7 @@ struct motion_status {
        unsigned short err_right;
 };
 
-struct act_pos {
+struct ref_pos {
        double x;
        double y;
        double phi;
@@ -23,7 +23,6 @@ struct est_pos {
        double x;
        double y;
        double phi;
-       double probability;
 };
 
 struct joy_data {

diff --git a/src/ulcdd/oledlib.cc b/src/ulcdd/oledlib.cc
index 0582b8479ddceb979a747b66ca9e937a879464c0..921ccb498c2bfee340acb01374ea1d19c4188c07 100644 (file)
--- a/src/ulcdd/oledlib.cc
+++ b/src/ulcdd/oledlib.cc
@@ -61,7 +61,7 @@ int oled_send_voltage(uint8_t *buff, int buff_size, struct pwr_voltage_type *vol
        return 0;
 }
 
-int oled_send_position(uint8_t *buff, int buff_size, struct act_pos_type *pos)
+int oled_send_position(uint8_t *buff, int buff_size, struct est_pos_type *pos)
 {
        if(buff_size < POSITION_MSG_SIZE)
                return -1;

diff --git a/src/ulcdd/oledlib.h b/src/ulcdd/oledlib.h
index 48cbd95a8d4786454bc300ec67be5ab477b8862f..5142a4ed9f21e504e7050b3c1d6b7d9941a283c7 100644 (file)
--- a/src/ulcdd/oledlib.h
+++ b/src/ulcdd/oledlib.h
@@ -64,7 +64,7 @@ extern "C" {
 int oled_switch_mode(uint8_t *buff, int buff_size, uint8_t mode, uint8_t status);
 int oled_send_voltage(uint8_t *buff, int buff_size, struct pwr_voltage_type *volt);
 int oled_set_color(uint8_t *buff, int buff_size, uint8_t color);
-int oled_send_position(uint8_t *buff, int buff_size, struct act_pos_type *pos);
+int oled_send_position(uint8_t *buff, int buff_size, struct est_pos_type *pos);
 int oled_send_fsm_state(uint8_t *buff, int buff_size, const char *name, int len);
 #ifdef __cplusplus
 }

diff --git a/src/ulcdd/uoled.c b/src/ulcdd/uoled.c
index 4bb4259e89e86cac85823c07eeb4686e73589605..9a79dd01cbc8c6490babfbcdc34ebc6bf73619ff 100644 (file)
--- a/src/ulcdd/uoled.c
+++ b/src/ulcdd/uoled.c
@@ -92,7 +92,7 @@ int uoled_set_color(uint8_t color)
        return 0;
 }
 
-int uoled_send_position(struct act_pos_type *pos) 
+int uoled_send_position(struct est_pos_type *pos) 
 {
        int ret;
        uint8_t msg[POSITION_MSG_SIZE];

diff --git a/src/ulcdd/uoled.h b/src/ulcdd/uoled.h
index 8dfb72d54eee385adeaa45cdc58565f635d97e77..389c60bbad048c93e29b96341324dd7a3a82618c 100644 (file)
--- a/src/ulcdd/uoled.h
+++ b/src/ulcdd/uoled.h
@@ -11,7 +11,7 @@ struct sercom_data* uoled_init(void(*sighandler)(int));
 int uoled_send_voltage(struct pwr_voltage_type *volt);
 int uoled_switch_mode_rep(uint8_t mode, uint8_t status); 
 int uoled_send_state(const char *name, int len);
-int uoled_send_position(struct act_pos_type *pos); 
+int uoled_send_position(struct est_pos_type *pos); 
 int uoled_set_color(uint8_t color);
 #ifdef __cplusplus
 }