robofsm: Add bumpers subscriber and fix bumpers handling.

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 2438170826ef03ffddf845518642cbcb9eb659d4..bc952c4f0053fcd1f6a8ebf303b8f200e50d31ba 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -442,17 +442,15 @@ void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
                            void *recvCallBackParam)
 {
         struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
-        static bool last_left, last_right;
         switch (info->status) {
                 case NEW_DATA:
-                        if (instance->bumper_rear)
+                        if (!instance->bumper_rear) {
                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
-                        if ((!last_left && instance->bumper_left) ||
-                            (!last_right && instance->bumper_right)) {
+                        }
+                        if ((!instance->bumper_left) ||
+                            (!instance->bumper_right)) {
                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
                         }
-                        last_right = instance->bumper_right;
-                        last_left = instance->bumper_left;
                         break;
                 case DEADLINE:
                         break;
@@ -516,6 +514,7 @@ int robot_init_orte()
        robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
        robottype_subscriber_crane_status_create(&robot.orte, rcv_crane_status_cb, &robot.orte);
        robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
+        robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
        robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 
        return rv;