robofsm: fix a odometry bug.

diff --git a/src/robofsm/eb2008/fsmloc.c b/src/robofsm/eb2008/fsmloc.c
index 12eb7803ee642f2fe2d53230051bc19f8c641283..8e70b8b7c2a440df4e517f00fae37f7748c1c1ac 100644 (file)
--- a/src/robofsm/eb2008/fsmloc.c
+++ b/src/robofsm/eb2008/fsmloc.c
@@ -47,6 +47,12 @@ FSM_STATE(run)
                        struct mcl_robot_odo *odo = FSM_EVENT_PTR;
                        mcl->predict(mcl, odo);
                        free(odo);
+                       struct mcl_robot_pos *est = robot.mcl->estimated;
+                       ROBOT_LOCK(est_pos);
+                       robot.est_pos.x = est->x;
+                       robot.est_pos.y = est->y;
+                       robot.est_pos.phi = est->angle;
+                       ROBOT_UNLOCK(est_pos);
                        break;
                }
                case EV_LASER_RECEIVED: {

diff --git a/src/robofsm/eb2008/movehelper_eb2008.cc b/src/robofsm/eb2008/movehelper_eb2008.cc
index 5633084dc64b74e6b25659221ed2ac82fdc73b5b..253d032e73571c19f0c2bad3ce4ef41f67e7b153 100644 (file)
--- a/src/robofsm/eb2008/movehelper_eb2008.cc
+++ b/src/robofsm/eb2008/movehelper_eb2008.cc
@@ -23,7 +23,7 @@
 
 #if 1
 struct TrajectoryConstraints trajectoryConstraintsDefault = {
-       maxv: 0.9,              // m/s
+       maxv: 0.3,              // m/s
        maxomega: 1.5,          // rad/s
        maxangacc: 2,           // rad/s^2
        maxacc: 0.3,            // m/s^2

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index 30f598f11994a1fcd97d9bb8f2a8efefc8cc4d3e..9ca6a62d3f7b43346449076e49da3f432e8fd098 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -130,7 +130,7 @@ int robot_init()
        robot.laser.width = PLAYGROUND_WIDTH_M; /* in meters */
        robot.laser.height = PLAYGROUND_WIDTH_M; /* in meters */
        /* the noises */
-       robot.laser.pred_dnoise = 0.01;
+       robot.laser.pred_dnoise = 0.001;
        robot.laser.pred_anoise = DEG2RAD(2);
        robot.laser.aeval_sigma = DEG2RAD(4);
        robot.mcl = mcl_laser_init(&robot.laser, 1000);

diff --git a/src/robofsm/eb2008/test/rectangle.cc b/src/robofsm/eb2008/test/rectangle.cc
index 0af3af83f0d6aff12795db2c05b363f5d398478e..254e5ba6564a4376673053b33af5378847624f42 100644 (file)
--- a/src/robofsm/eb2008/test/rectangle.cc
+++ b/src/robofsm/eb2008/test/rectangle.cc
@@ -42,7 +42,7 @@ void follow_rectangle()
        robot_trajectory_add_point_trans(2, 0.5);
        robot_trajectory_add_point_trans(2, 1.5);
        robot_trajectory_add_point_trans(1, 1.5);
-       robot_trajectory_add_final_point_trans(1, 0.5, TURN(DEG2RAD(90)));
+       robot_trajectory_add_final_point_trans(1, 0.5, NO_TURN());
 }
 
 FSM_STATE(rectangle)
@@ -52,10 +52,10 @@ FSM_STATE(rectangle)
                        follow_rectangle();
                        break;
                case EV_TRAJECTORY_ERROR:
-                       FSM_TRANSITION(turn);
+//                     FSM_TRANSITION(turn);
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(turn);
+//                     FSM_TRANSITION(turn);
                        break;
                default:
                        break;