robofsm: Fixed obstacle detection

diff --git a/src/robofsm/eb2007/robot_eb2007.h b/src/robofsm/eb2007/robot_eb2007.h
index a0fe5c58dcd4574fa71a033153ed69e27c7ce68a..e744e3de52ae88d8fb37937ecebdfd4dc7cb093c 100644 (file)
--- a/src/robofsm/eb2007/robot_eb2007.h
+++ b/src/robofsm/eb2007/robot_eb2007.h
@@ -170,7 +170,7 @@ FSM_STATE_DECL(joy_init);
 
 /*Thread priorities*/
 #define TRAJ_FOLLOWER_PRIO 255 /* As high as possible */
-#define UPDATE_MAP_PRIO 10     /* Priority of the thread for updating map with detected obstacles. */
-#define TRAJ_RECALC_PRIO 0     /* As low as possible */
+#define OBST_FORGETING_PRIOTR 10       /* Priority of the thread for forgeting detected obstacles. */
+#define TRAJ_RECALC_PRIO 0
 
 #endif /* ROBOT_EB2007_H */

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index a113a3cd71708d6b6c1f4f40292c49a8191ba1da..b9924b5fe756785130c8f14367b02f27e9b5ff3a 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -268,14 +268,15 @@ FSM_STATE(init)
 
 FSM_STATE(wait_for_start)
 {
-       pthread_t thid;
-
        switch (FSM_EVENT) {
 #ifdef WAIT_FOR_START
-               case EV_START:
+               case EV_START: {
+                       pthread_t thid;
+
                        /* start competition timer */
                        pthread_create(&thid, NULL, wait_for_end, NULL);
                        pthread_create(&thid, NULL, wait_to_deposition, NULL);
+               }
 #else
                case EV_ENTRY:
 #endif

diff --git a/src/robofsm/eb2008/fsmmove.cc b/src/robofsm/eb2008/fsmmove.cc
index 1a5bf5f92a16a1e4f861ec7565d06c4464a6fa4a..63f9a6178c74efdb18e47acf8e24a516a6cf1fa0 100644 (file)
--- a/src/robofsm/eb2008/fsmmove.cc
+++ b/src/robofsm/eb2008/fsmmove.cc
@@ -46,7 +46,6 @@ const struct balet_params k = {
 
 // Global varibles
 static pthread_t thr_trajctory_follower;
-static pthread_t thr_trajctory_recalc;
 static struct timeval tv_start; /**< Absolute time, when trajectory started. */
 
 /** Stores the actually followed trajectory object */
@@ -382,32 +381,29 @@ FSM_STATE(init)
                        perror("move_init: pthread_create");
                        exit(1);
                }
-               // Thread of trajectory recalc
-               pthread_attr_init (&tattr);
-               pthread_attr_getschedparam (&tattr, &param);
-               param.sched_priority = TRAJ_RECALC_PRIO;
-               ret = pthread_attr_setschedparam (&tattr, &param);
-               if(ret) {
-                       perror("move_init: pthread_attr_setschedparam(recalc)");
-                       exit(1);
-               }
-               //pthread_create(&thr_trajctory_recalc, NULL, trajctory_recalc, NULL);
-               
-               // Thread update Map
-               pthread_attr_init (&tattr);
-               pthread_attr_getschedparam (&tattr, &param);
-               param.sched_priority = UPDATE_MAP_PRIO;
-               ret = pthread_attr_setschedparam (&tattr, &param);
-               if(ret) {
-                       perror("pthread_attr_setschedparam");
-                       exit(1);
-               }
-               //pthread_create(&thr_update_map, NULL, thread_update_map, NULL);
-               
+
                FSM_TRANSITION(wait_for_command);
        }
 }
 
+#define FIND_NEW_WAY(target)                                   \
+do {                                                           \
+       enum target_status ret = new_goal(target);              \
+       switch (ret) {                                          \
+               case TARGET_OK:                                 \
+                       FSM_TRANSITION(movement);               \
+                       break;                                  \
+               case TARGET_ERROR:                              \
+                       printf("Target error\n");               \
+                       FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); \
+                       break;                                  \
+               case TARGET_NOT_REACHABLE:                      \
+                       FSM_TRANSITION(wait_and_try_again);     \
+                       break;                                  \
+       }                                                       \
+} while(0)
+                                 
+
 FSM_STATE(wait_for_command)
 {
        enum target_status ret;
@@ -448,6 +444,9 @@ FSM_STATE(movement)
                        FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
                        FSM_TRANSITION(wait_for_command);
                        break;
+               case EV_OBSTACLE_ON_ROAD:
+                       FIND_NEW_WAY(&current_target);
+                       break;
                case EV_TRAJECTORY_LOST:
                        FSM_TRANSITION(reset_mcl);
                        break;
@@ -487,6 +486,9 @@ FSM_STATE(close_to_target)
                case EV_MOVE_STOP:
                        FSM_TRANSITION(wait_for_command);
                        break;
+               case EV_OBSTACLE_ON_ROAD:
+                       /* ignore it */
+                       break;
                case EV_EXIT:
                        stop();
                        break;
@@ -506,8 +508,7 @@ FSM_STATE(reset_mcl)
                        FSM_SIGNAL(LOC, EV_RESET, NULL);
                        break;
                case EV_FOUND_AFTER_RESET:
-                       FSM_TRANSITION(movement);
-                       printf("TODO: start motion again\n");
+                       FIND_NEW_WAY(&current_target);
                        break;
                case EV_MOVE_STOP:
                        FSM_TRANSITION(wait_for_command);
@@ -520,6 +521,7 @@ FSM_STATE(reset_mcl)
                case EV_TRAJECTORY_LOST:
                case EV_RETURN:
                case EV_TIMER:
+               case EV_OBSTACLE_ON_ROAD:
                        DBG_PRINT_EVENT("unhandled event");
        }
 }
@@ -527,26 +529,14 @@ FSM_STATE(reset_mcl)
 
 FSM_STATE(wait_and_try_again)
 {
-       enum target_status ret;
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        stop();
                        FSM_TIMER(1000);
                        break;
                case EV_TIMER:
-                       ret = new_goal(&current_target);
-                       switch (ret) {
-                               case TARGET_OK:
-                                       FSM_TRANSITION(movement);
-                                       break;
-                               case TARGET_ERROR:
-                                       printf("Target error\n");
-                                       FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
-                                       break;
-                               case TARGET_NOT_REACHABLE:
-                                       FSM_TRANSITION(wait_and_try_again);
-                                       break;
-                       }
+                       FIND_NEW_WAY(&current_target);
+                       break;
                case EV_MOVE_STOP:
                        FSM_TRANSITION(wait_for_command);
                        break;
@@ -556,7 +546,10 @@ FSM_STATE(wait_and_try_again)
                case EV_TRAJECTORY_LOST:
                case EV_RETURN:
                case EV_FOUND_AFTER_RESET:
-               case EV_EXIT:
+               case EV_OBSTACLE_ON_ROAD:
                        DBG_PRINT_EVENT("unhandled event");
+                       break;
+               case EV_EXIT:
+                       break;
        }
 }

diff --git a/src/robofsm/eb2008/map_handling.c b/src/robofsm/eb2008/map_handling.c
index bc016dd1c21c0c785099b09c54706a89b9b1fb0c..da34a7c74cfca473130f19d4010c93f5c7b595d2 100644 (file)
--- a/src/robofsm/eb2008/map_handling.c
+++ b/src/robofsm/eb2008/map_handling.c
@@ -141,7 +141,7 @@ static void gettimeofday_ts(struct timespec *ts)
 /**
  * A thread updating the map
  */
-static void * thread_obstacle_forgeting(void * arg)
+void * thread_obstacle_forgeting(void * arg)
 {
        struct timespec ts;
        sem_t timer;

diff --git a/src/robofsm/eb2008/map_handling.h b/src/robofsm/eb2008/map_handling.h
index 1e9dcf84fb29621f203d1337c97e570b9fea9e07..d545a070f0e8a6b12a4e8cfd97068b94dd8e3f73 100644 (file)
--- a/src/robofsm/eb2008/map_handling.h
+++ b/src/robofsm/eb2008/map_handling.h
@@ -3,6 +3,7 @@
 
 #include <robodim_eb2008.h>
 
+void * thread_obstacle_forgeting(void * arg);
 void update_map(struct sharps_type *s);
 
 #endif

diff --git a/src/robofsm/eb2008/roboevent_eb2008.py b/src/robofsm/eb2008/roboevent_eb2008.py
index 65cd590b0a42e9ef5789a5f0394cea7a825c7ebb..f8894f7e283deddf11205895c8d2d71d2f8d5d22 100644 (file)
--- a/src/robofsm/eb2008/roboevent_eb2008.py
+++ b/src/robofsm/eb2008/roboevent_eb2008.py
@@ -25,6 +25,7 @@ events = {
         "EV_TRAJECTORY_DONE" : "Reference position is at the end of the previously submitted trajectory",
        "EV_TRAJECTORY_DONE_AND_CLOSE" : "::EV_TRAJECTORY_DONE + distance of actual position to the target is less than XXX",
        "EV_FOUND_AFTER_RESET" : "MCL estimated position after reset",
+       "EV_OBSTACLE_ON_ROAD" : "An obstacle is detected on the current trajectory",
     },
     "joy" : {
     },

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index a5aa11442560a6f790b2678f924dee19cbd1b995..4b3457f551f8cd7d827474b07d659d76fe88937f 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -8,14 +8,19 @@
  * License: GNU GPL v.2
  */
 
+#include <map.h>
+#include <movehelper_eb2008.h>
+#include <pthread.h>
+#include <robomath.h>
 #include <robot_eb2008.h>
+#include <robot_orte.h>
+#include <servos_eb2008.h>
 #include <signal.h>
+#include <sys/time.h>
+#include <time.h>
 #include <unistd.h>
-#include <servos_eb2008.h>
-#include <movehelper_eb2008.h>
-#include <robot_orte.h>
-#include <robomath.h>
-#include <map.h>
+#include "map_handling.h"
+
 
 #define THREAD_PRIO_MOTION     30
 #define THREAD_PRIO_MAIN       25
@@ -174,6 +179,26 @@ int robot_start()
                }
        }
 
+       pthread_attr_t tattr;
+       struct sched_param param;
+       pthread_t thr_obstacle_forgeting;
+       int ret;
+
+       // Trajectory follower thread
+       pthread_attr_init (&tattr);
+       pthread_attr_getschedparam (&tattr, &param);
+       param.sched_priority = OBST_FORGETING_PRIO;
+       ret = pthread_attr_setschedparam (&tattr, &param);
+       if(ret) {
+               perror("robot_start: pthread_attr_setschedparam()");
+               exit(1);
+       }
+       ret = pthread_create(&thr_obstacle_forgeting, &tattr, thread_obstacle_forgeting, NULL);
+       if(ret) {
+               perror("robot_start: pthread_create");
+               exit(1);
+       }
+       
        return 0;
 }
 

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index a57e6dbd5aadf7e2261408bc8ad8970666650930..37fa04fa30b8525c0c35d45d9d17cc966a5f2f09 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -157,11 +157,6 @@ struct robot_eb2008 {
        /* estimated position */
        struct est_pos_type est_pos;
 
-       /* goal position */ /* FIXME: Move away */
-       struct ref_pos_type goal; 
-       struct TrajectoryConstraints goal_trajectory_constraints;
-       struct final_heading goal_final_heading;
-
        struct motion_irc_type odometry;
        struct joy_data_type joy_data;
        struct drives_type drives;
@@ -210,7 +205,6 @@ FSM_STATE_FULL_DECL(disp, init);
 
 /*Thread priorities*/
 #define TRAJ_FOLLOWER_PRIO 255 /* As high as possible */
-#define UPDATE_MAP_PRIO 10     /* Priority of the thread for updating map with detected obstacles. */
-#define TRAJ_RECALC_PRIO 0     /* As low as possible */
+#define OBST_FORGETING_PRIO 10         /* Priority of the thread for forgeting detected obstacles. */
 
 #endif /* ROBOT_EB2008_H */