robofsm: Implemented mcl->set_estimated

diff --git a/src/robofsm/eb2008/fsmloc.c b/src/robofsm/eb2008/fsmloc.c
index 3472f240a07caf35692d4b0afe57d5409003f7e7..fde542f6e132618d30b4833ad9b354cd586ee789 100644 (file)
--- a/src/robofsm/eb2008/fsmloc.c
+++ b/src/robofsm/eb2008/fsmloc.c
@@ -30,15 +30,6 @@ FSM_STATE_DECL(ignore_laser);
  */
 FSM_STATE(loc_init)
 {
-       /* MCL initialization */
-       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_anoise = DEG2RAD(2);
-       robot.laser.aeval_sigma = DEG2RAD(4);
-
-       robot.mcl = mcl_laser_init(&robot.laser, 1000);
        FSM_TRANSITION(run);
 }
 
@@ -75,8 +66,15 @@ FSM_STATE(run)
                        robot.mcl->resample(robot.mcl);
                        break;
                }
+               case EV_SET_ESTIMATED: {
+                       struct mcl_robot_pos *pos = FSM_EVENT_PTR;
+                       mcl->set_estimated(mcl, pos);
+                       free(pos);
+                       break;
+               }
                case EV_ENTRY:
                case EV_EXIT:
+                       break;
                case EV_RETURN:
                case EV_TIMER:
                        DBG_PRINT_EVENT("unhandled event");

diff --git a/src/robofsm/eb2008/roboevent_eb2008.py b/src/robofsm/eb2008/roboevent_eb2008.py
index 21b2d247ea3536dce15886e21699e8e39085198b..631ddbb3d7faa8e89ef0ca92764e8912a95d6486 100644 (file)
--- a/src/robofsm/eb2008/roboevent_eb2008.py
+++ b/src/robofsm/eb2008/roboevent_eb2008.py
@@ -17,6 +17,7 @@ events = {
     "loc" : {
        "EV_ODO_RECEIVED" : "Execute prediction step based on odometry data",
        "EV_LASER_RECEIVED" : "Execute update step based on laser measurement",
+       "EV_SET_ESTIMATED" : "Sets the estimated position to the value from event parameter",
        "EV_RESET" : "Initialize MCL to uniform distribution on the whole playground",
     },
     "motion" : {

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index 686d62ed42f73b23b6147f00ccdb3608f7b668da..70420357b7f467a717b4ddfbeb7144d6f907780d 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -121,6 +121,19 @@ int robot_init()
        robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
        robot.orte.drives.bagr = BAGR_DRIVE_OFF;
        
+       /* MCL initialization */
+       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_anoise = DEG2RAD(2);
+       robot.laser.aeval_sigma = DEG2RAD(4);
+       robot.mcl = mcl_laser_init(&robot.laser, 1000);
+
+       robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;
+       robot.fsm[FSM_ID_DISP].state = &fsm_state_disp_init;
+       robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
+
        /* init ORTE domain, create publishers, subscribers, .. */
        robot_init_orte();