robofsm: MotionControl change functions to accept new interface

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index cbe94bb1c921831300034eeb5cbbbbfd9ce8eb20..e785426491dad5ab72201e2084d9082f248c5dce 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -34,6 +34,8 @@
 #include "robot_config.h"
 #include <robomath.h>
 #include <ul_log.h>
+#include "guard.hpp"
+#include "events.h"
 
 UL_LOG_CUST(ulogd_motion_control); /* Log domain name = ulogd + name of the file */
 
@@ -110,7 +112,7 @@ static void delete_actual_trajectory()
        old = actual_trajectory;
        actual_trajectory = NULL;
        pthread_mutex_unlock(&actual_trajectory_lock);
-       robot_send_speed(0,0);
+       robot.move_helper.send_speed(0,0);
        if (old) delete(old);
 }
 
@@ -123,16 +125,14 @@ static void notify_fsm(bool done, double error)
        if (error > MAX_POS_ERROR_M) {
                if (!lost_sent) {
                        lost_sent = true;
-                       FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST, NULL);
+                       robot.sched.queue_event(robot.MOTION, new evTrajectoryLost());
                }
        } else {
                lost_sent = false;
                if (done) {
-                       if (error < CLOSE_TO_TARGET_M) {
-                               FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE, NULL);
-                       } else if (!done_sent) {
+                       if (error < CLOSE_TO_TARGET_M || !done_sent) {
+                               robot.sched.queue_event(robot.MOTION, new evTrajectoryDone());
                                done_sent = true;
-                               FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE, NULL);
                        }
                } else {
                        done_sent = false;
@@ -166,7 +166,7 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
                        continue;
                if (map->cells[ycell][xcell].detected_obstacle > 0) {
                        if (sem_trywait(&recalculation_not_running) == 0) {
-                               FSM_SIGNAL(MOTION, EV_OBSTACLE, NULL);
+                               robot.sched.queue_event(robot.MOTION, new evObstacle());
                                break;
                        }
                }
@@ -221,13 +221,14 @@ static void do_control()
                if (ref_pos.omega > actual_trajectory->constr.maxomega)
                        DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, actual_trajectory->constr.maxomega);
 
-               ROBOT_LOCK(ref_pos);
-               robot.ref_pos.x = ref_pos.x;
-               robot.ref_pos.y = ref_pos.y;
-               robot.ref_pos.phi = ref_pos.phi;
-               ROBOT_UNLOCK(ref_pos);
+               {
+                       Guard g(robot.lock_ref_pos);
+                       robot.ref_pos.x = ref_pos.x;
+                       robot.ref_pos.y = ref_pos.y;
+                       robot.ref_pos.phi = ref_pos.phi;
+               }
 
-               robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
+               robot.get_est_pos(est_pos.x, est_pos.y, est_pos.phi);
 
 #ifdef MOTION_PRINT_REF
                static double last_t;
@@ -250,7 +251,7 @@ static void do_control()
 
 
        // Apply controller output
-       robot_send_speed(speedl, speedr);
+       robot.move_helper.send_speed(speedl, speedr);
        pthread_mutex_unlock(&actual_trajectory_lock);
 }
 
@@ -429,7 +430,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);
 
-               robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
+               robot.get_est_pos(pos.x, pos.y, pos.phi);
                pos.v = 0;
                pos.omega = 0;
        }

diff --git a/src/robofsm/motion-control.h b/src/robofsm/motion-control.h
index 0f029e9c27888106a3d75346007142f394f7c511..ee6536b958b675545881c896373e4d6e7091cdd5 100644 (file)
--- a/src/robofsm/motion-control.h
+++ b/src/robofsm/motion-control.h
@@ -9,9 +9,6 @@
 #ifndef MOTION_CONTROL_H
 #define MOTION_CONTROL_H
 
-#if !defined(FSM_MOTION) && !defined(MOTION_CONTROL) && !defined(MOTION_CONTROL_INIT_ONLY)
-#error This file should only be included from fsmmove.cc
-#endif
 
 #ifdef __cplusplus
 extern "C" {