robofsm: Unified access to estimated position

Because of changes in estimated position, there were many errors in
determining robot's estimated position. At some places, est_pos_uzv was
hardcoded, at some places est_pos_odo etc. Depending on whether
localization worked, the robot behave correctly or not, which was not
good.

This was solved by introducing robot_get_est_pos() which is used at all
places in the program to determine robot's current position. By changing
this function, one can easily change the localization of the whole
robot.

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index bbf9f54650182ea2dd5f7558ac1fb0b89190e29c..09787cf1176f6ce1345e9a4c1c1d41780a90f14d 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -175,25 +175,7 @@ static enum target_status new_trajectory(Trajectory *t)
        }
        // FIXME: This is duplicite code with new_goal. Remove this
        // function and use always new_goal.
-       if (robot.localization_works) {
-               ROBOT_LOCK(est_pos_uzv);
-               pos.x = robot.est_pos_uzv.x;
-               pos.y = robot.est_pos_uzv.y;
-               pos.phi = robot.est_pos_uzv.phi;
-               ROBOT_UNLOCK(est_pos_uzv);
-       } else if (robot.odometry_works) {
-               ROBOT_LOCK(est_pos_odo);
-               pos.x = robot.est_pos_odo.x;
-               pos.y = robot.est_pos_odo.y;
-               pos.phi = robot.est_pos_odo.phi;
-               ROBOT_UNLOCK(est_pos_odo);
-       } else {
-               ROBOT_LOCK(ref_pos);
-               pos.x = robot.ref_pos.x;
-               pos.y = robot.ref_pos.y;
-               pos.phi = robot.ref_pos.phi;
-               ROBOT_UNLOCK(ref_pos);
-       }
+       robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
        pos.v = 0;
        pos.omega = 0;
 
@@ -305,10 +287,12 @@ FSM_STATE(movement)
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        if  (robot.localization_works) {
-                               // TODO lock
-                               robot_set_est_pos_notrans(robot.est_pos_uzv.x,
-                                                         robot.est_pos_uzv.y,
-                                                         robot.est_pos_uzv.phi);
+                               robot_pos_type p;
+                               ROBOT_LOCK(est_pos_uzv);
+                               p = robot.est_pos_uzv;
+                               ROBOT_UNLOCK(est_pos_uzv);
+                               
+                               robot_set_est_pos_notrans(p.x, p.y, p.phi);
                        }
                        break;
                case EV_TRAJECTORY_DONE:

diff --git a/src/robofsm/map_handling.c b/src/robofsm/map_handling.c
index 2f9c8d7bc61ff15de1269aab13d5fbe8a8b24a2a..de6016471da994aa7711e575b30448e1edf6528c 100644 (file)
--- a/src/robofsm/map_handling.c
+++ b/src/robofsm/map_handling.c
@@ -47,9 +47,8 @@ void obstacle_detected_at(double x, double y, bool real_obstacle)
         * #OBS_CSPACE. */
 
        /* Mark "protected" area around the obstacle */
-       ROBOT_LOCK(est_pos_uzv);
-       est_pos = robot.est_pos_uzv;
-       ROBOT_UNLOCK(est_pos_uzv);
+       robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
+
        int obst_size_cell = (int)ceil(OBS_SIZE_M/MAP_CELL_SIZE_M);
        for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
                for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
@@ -122,10 +121,8 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
        struct sharp_pos beam;
        u_int16_t *data;
 
-       ROBOT_LOCK(est_pos_uzv);
-       e = robot.est_pos_uzv;
-       ROBOT_UNLOCK(est_pos_uzv);
-
+       robot_get_est_pos(&e.x, &e.y, &e.phi);
+       
        beam.x = HOKUYO_CENTER_OFFSET_M;
        beam.y = 0;
 

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 5c6fb379fd8cba15147b4bd429de524d3e4d71e6..5994684db17a658e820aab198d075e704fcb38f7 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -316,15 +316,7 @@ static void do_control()
                robot.ref_pos.phi = ref_pos.phi;
                ROBOT_UNLOCK(ref_pos);
 
-               if (robot.odometry_works) {
-                       ROBOT_LOCK(est_pos_odo);
-                       est_pos.x = robot.est_pos_odo.x;
-                       est_pos.y = robot.est_pos_odo.y;
-                       est_pos.phi = robot.est_pos_odo.phi;
-                       ROBOT_UNLOCK(est_pos_odo);
-               } else {
-                       est_pos = ref_pos;
-               }
+               robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
 
 #ifdef MOTION_PRINT_REF
                static double last_t;
@@ -543,25 +535,7 @@ void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time)
                // Robot doesn't move, so return current position
                pthread_mutex_unlock(&actual_trajectory_lock);
 
-               if (robot.localization_works) {
-                       ROBOT_LOCK(est_pos_uzv);
-                       pos.x = robot.est_pos_uzv.x;
-                       pos.y = robot.est_pos_uzv.y;
-                       pos.phi = robot.est_pos_uzv.phi;
-                       ROBOT_UNLOCK(est_pos_uzv);
-               } else if (robot.odometry_works) {
-                       ROBOT_LOCK(est_pos_odo);
-                       pos.x = robot.est_pos_odo.x;
-                       pos.y = robot.est_pos_odo.y;
-                       pos.phi = robot.est_pos_odo.phi;
-                       ROBOT_UNLOCK(est_pos_odo);
-               } else {
-                       ROBOT_LOCK(ref_pos);
-                       pos.x = robot.ref_pos.x;
-                       pos.y = robot.ref_pos.y;
-                       pos.phi = robot.ref_pos.phi;
-                       ROBOT_UNLOCK(ref_pos);
-               }
+               robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
                pos.v = 0;
                pos.omega = 0;
        }

diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc
index 62468cc9bdeb8725fbce13cdce372c5fa22af50b..20658a55e08d4775c52f0f0cf39bc0b9555b007c 100644 (file)
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -263,12 +263,8 @@ void robot_moveto_notrans(double x, double y,
 void robot_move_by(double distance, struct move_target_heading heading, struct TrajectoryConstraints *tc)
 {
        double x, y, phi;
-       
-       ROBOT_LOCK(est_pos_uzv);
-       x = robot.est_pos_uzv.x;
-       y = robot.est_pos_uzv.y ;
-       phi = robot.est_pos_uzv.phi;
-       ROBOT_UNLOCK(est_pos_uzv);
+
+       robot_get_est_pos(&x, &y, &phi);
 
        x += distance*cos(phi);
        y += distance*sin(phi);

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 225744044b9a871145c1fe31f74e0ff515ba0b55..e196f4203c46e303661f3fb4bf7c1954263c627f 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -275,3 +275,26 @@ void robot_destroy()
        ShmapFree();
        DBG("robofsm: stop.\n");
 }
+
+void robot_get_est_pos(double *x, double *y, double *phi)
+{
+       if (robot.localization_works) {
+               ROBOT_LOCK(est_pos_uzv);
+               *x = robot.est_pos_uzv.x;
+               *y = robot.est_pos_uzv.y;
+               *phi = robot.est_pos_uzv.phi;
+               ROBOT_UNLOCK(est_pos_uzv);
+       } else if (robot.odometry_works) {
+               ROBOT_LOCK(est_pos_odo);
+               *x = robot.est_pos_odo.x;
+               *y = robot.est_pos_odo.y;
+               *phi = robot.est_pos_odo.phi;
+               ROBOT_UNLOCK(est_pos_odo);
+       } else {
+               ROBOT_LOCK(ref_pos);
+               *x = robot.ref_pos.x;
+               *y = robot.ref_pos.y;
+               *phi = robot.ref_pos.phi;
+               ROBOT_UNLOCK(ref_pos);
+       }
+}

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index b8c96ba8968803d89df837961cba0d7d8d31e7e2..c72727e60593fc1e533cba4921b6a860321503f1 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -209,6 +209,8 @@ int robot_start() __attribute__ ((warn_unused_result));
 void robot_exit();
 void robot_destroy();
 
+void robot_get_est_pos(double *x, double *y, double *phi);
+
 /* Hack to easily disable display if it is not configured */
 void serial_comm(int status);
 
@@ -238,5 +240,4 @@ static inline void act_lift(unsigned short int pos)
        ROBOT_UNLOCK(lift_cmd);
 }
 
-
 #endif /* ROBOT_H */