Transform to C++: Sending events

Change methods FSM_SIGNAL to new interface.

diff --git a/src/boostFSM/movehelper.cc b/src/boostFSM/movehelper.cc
index 0e38ccdc33e56f430aa5419f8bb1f847c3bb0865..88d2902d10346fd50070b36722393b364f694b7c 100644 (file)
--- a/src/boostFSM/movehelper.cc
+++ b/src/boostFSM/movehelper.cc
@@ -23,6 +23,7 @@
 #include <string.h>
 #include <ul_log.h>
 #include "guard.hpp"
+#include "events.h"
 
 UL_LOG_CUST(ulogd_movehelper); /* Log domain name = ulogd + name of the file */
 
@@ -101,7 +102,7 @@ MoveHelper& MoveHelper::add_point_notrans(double x_m, double y_m)
 MoveHelper& MoveHelper::add_final_point_notrans(double x_m, double y_m, move_target_heading heading)
 {
        move_target *target;
-       
+       printf("%s\n", __FUNCTION__);
        if (t) {
                if (heading.operation == TOP_ARRIVE_FROM) {
                        double ax, ay;
@@ -117,7 +118,8 @@ MoveHelper& MoveHelper::add_final_point_notrans(double x_m, double y_m, move_tar
                target->heading = heading;
                target->trajectory = t;
                target->use_planning = false;
-               /*TODO*/FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
+//             /*TODO*/FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
+               robot.sched.queue_event(robot.MOTION, new EV_NEW_TARGET(target));
                t = NULL; 
        } else {
                ul_logerr("%s: Error - No trajectory\n", __FUNCTION__);
@@ -150,7 +152,8 @@ bool MoveHelper::get_arrive_from_point(double target_x_m, double target_y_m, mov
  */
 void MoveHelper::stop()
 {
-       FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
+       robot.sched.queue_event(robot.MOTION, new EV_MOVE_STOP());
+       //FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
 }
 
 
@@ -195,8 +198,9 @@ void MoveHelper::goto_notrans(double x, double y, move_target_heading heading, T
        target->use_planning = planning;
        if (tc) target->tc = *tc;
        else target->tc = trajectoryConstraintsDefault;
-       
-       /*TODO*/FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
+
+       robot.sched.queue_event(robot.MOTION, new EV_NEW_TARGET(target));
+//     /*TODO*/FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 }
 
 /** 
@@ -211,7 +215,7 @@ void MoveHelper::move_by(double distance, move_target_heading heading, Trajector
        double x, y, phi;
 
        
-       robot.get_est_pos(&x, &y, &phi);
+       robot.get_est_pos(x, y, phi);
 
        x += distance*cos(phi);
        y += distance*sin(phi);
@@ -227,7 +231,8 @@ void MoveHelper::move_by(double distance, move_target_heading heading, Trajector
        target->y = y;
        target->heading = heading;
        target->trajectory = t;
-       /*TODO*/FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
+       robot.sched.queue_event(robot.MOTION, new EV_NEW_TARGET(target));
+//     /*TODO*/FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
        delete t;
 }
 

diff --git a/src/boostFSM/robot_orte.cc b/src/boostFSM/robot_orte.cc
index 74b59a9c9d77086b8d851cf1bca2955a59431b7d..605d1fa4100d9d5a0a5c95898c594990697be447 100644 (file)
--- a/src/boostFSM/robot_orte.cc
+++ b/src/boostFSM/robot_orte.cc
@@ -30,6 +30,7 @@
 #include <ul_log.h>
 #include "guard.hpp"
 #include "robot.h"
+#include "events.h"
 
 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
 
@@ -78,7 +79,7 @@ static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
 {
        struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
 
-       robot.get_est_pos(&instance->x, &instance->y, &instance->phi);
+       robot.get_est_pos(instance->x, instance->y, instance->phi);
 }
 
 void send_dummy_cb(const ORTESendInfo *info, void *vinstance, 
@@ -288,7 +289,8 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                                        robot.status[COMPONENT_START] = STATUS_OK;
                                        /* Competition starts when plugged out */
                                        if (instance->start_condition) {
-                                               FSM_SIGNAL(MAIN, EV_START, NULL);
+                                               robot.sched.queue_event(robot.MAIN, new EV_START());
+                                               //FSM_SIGNAL(MAIN, EV_START, NULL);
                                                robot.start_state = COMPETITION_STARTED;
                                                ul_logmsg("STARTED\n");
                                        }
@@ -389,7 +391,9 @@ void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
                             instance->response.right != last_response_right &&
                            instance->response.left == act.jaw_left_get_last_request() &&
                            instance->response.left == act.jaw_right_get_last_request())
-                               FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
+                               robot.sched.queue_event(robot.MAIN, new EV_JAWS_DONE());
+                                               
+//                       FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
 
                        last_response_left = instance->response.left;
                        last_response_right = instance->response.right;
@@ -416,7 +420,9 @@ void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
 
                         if (instance->response != last_response &&
                             instance->response == act.lift_get_last_request())
-                                FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
+                         robot.sched.queue_event(robot.MAIN, new EV_LIFT_DONE());
+                                               
+//                                 FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
                         last_response = instance->response;
                         break;
                 case DEADLINE:
@@ -437,7 +443,9 @@ void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
 
                        if (!last_strategy && instance->strategy) {
                                        /* strategy switching */
-                                       FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
+                                       robot.sched.queue_event(robot.MAIN, new EV_SWITCH_STRATEGY());
+                                               
+//                                     FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
                        }
                        last_strategy = instance->strategy;
                        break;
@@ -454,10 +462,14 @@ void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
        switch (info->status) {
                case NEW_DATA:
                        if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
-                               FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+robot.sched.queue_event(robot.MOTION, new EV_OBSTACLE_BEHIND());
+                                               
+                         //                            FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 
                        if (instance->bumper_left || instance->bumper_right) {
-                               FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
+      robot.sched.queue_event(robot.MOTION, new EV_OBSTACLE_SIDE());
+                                               
+                         //                            FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
                        }
                        break;
                case DEADLINE: