robofsm: Fixes for obstacle avoidance

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 89c8f8a923003f30af0f78bf64273a1b62c29710..0fdb9d552f15560e838db72e80bf63ab181758ad 100644 (file)
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -120,12 +120,17 @@ int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double
        int i, x, y;
        int xcontig, ycontig, xstart, ystart, xgoal, ygoal;
         struct map *map = ShmapIsMapInit();
+       bool valid;
 
         if (!map) return -1;
        // Transform real data in cell data
-        ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart);
+        ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart, &valid);
+       if (!valid)
+               return -1;
         start = &graph[ystart][xstart];
-        ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal);
+        ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
+       if (!valid)
+               return -1;
         goal = &graph[ygoal][xgoal];
        
        /**@{*/

diff --git a/src/pathplan/map.c b/src/pathplan/map.c
index 29f2c6d8e2558d918d4cb00b9e1d1eb0df1df2d1..90abc591bd7d5883c6897a5e0ffa103705db4a35 100644 (file)
--- a/src/pathplan/map.c
+++ b/src/pathplan/map.c
@@ -18,7 +18,6 @@
 #include <sys/stat.h>
 #include <math.h>
 
-
 struct map* map = NULL;
 int shmap_id;
 /** @addtogroup maplib */
@@ -131,8 +130,12 @@ struct map_cell ShmapNoCell;
 struct map_cell *ShmapCellAtPoint(double x_m, double y_m)
 {
         int i, j;
-        ShmapPoint2Cell(x_m, y_m, &i, &j);
-        return &(map->cells[j][i]);
+       bool valid;
+        ShmapPoint2Cell(x_m, y_m, &i, &j, &valid);
+       if (valid)
+               return &(map->cells[j][i]);
+       else
+               return NULL;
 }
 
 /**
@@ -158,7 +161,8 @@ int ShmapIsFreeCell(int x, int y)
 int ShmapIsFreePoint(double x_m, double y_m)
 {
        int i,j;
-        ShmapPoint2Cell(x_m, y_m, &i, &j);
+       bool valid;
+        ShmapPoint2Cell(x_m, y_m, &i, &j, &valid);
         return ShmapIsFreeCell(i, j);
 
 }
@@ -175,8 +179,13 @@ int ShmapIsFreePoint(double x_m, double y_m)
 int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
 {
        int i,j, i1, i2, j1, j2;
-        ShmapPoint2Cell(x1, y1, &i1, &j1);
-        ShmapPoint2Cell(x2, y2, &i2, &j2);
+       bool valid;
+        ShmapPoint2Cell(x1, y1, &i1, &j1, &valid);
+       if (!valid)
+               return 0;
+        ShmapPoint2Cell(x2, y2, &i2, &j2, &valid);
+       if (!valid)
+               return 0;
         if (i1 > i2) {
                 i = i2;
                 i2 = i1;
@@ -213,9 +222,16 @@ int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_f
  * @param x    Coodonte of X 
 */
 
-void ShmapPoint2Cell(double x, double y, int *ix, int *iy){
-        if (ix) *ix = (int)floor(x/MAP_CELL_SIZE_M);
-        if (iy) *iy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
+void ShmapPoint2Cell(double x, double y, int *ix, int *iy, bool *valid){
+       int xx, yy;
+        xx = (int)floor(x/MAP_CELL_SIZE_M);
+        yy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
+       
+       if (valid != NULL) {
+               *valid = ShmapIsCellInMap(xx, yy);
+       }
+        if (ix) *ix = xx;
+        if (iy) *iy = yy;
 }
 
 /**

diff --git a/src/pathplan/map.h b/src/pathplan/map.h
index 38e0cbc5fda65c210120807d774567abb0c41d85..c924450c69826ffc1f408c33af321da124850404 100644 (file)
--- a/src/pathplan/map.h
+++ b/src/pathplan/map.h
@@ -1,5 +1,8 @@
 #ifndef __MAP_H
 #define __MAP_H
+
+#include <stdbool.h>
+
 /**
  * @defgroup maplib    Library to manage the map
  * This libary implements a discrete map use wich can be use in robotics.
@@ -213,7 +216,7 @@ void ShmapFree(void);
 void ShmapDt(void);
 struct map * ShmapIsMapInit(void);
 
-void ShmapPoint2Cell(double x, double y, int *cx, int *cy);
+void ShmapPoint2Cell(double x, double y, int *cx, int *cy, bool *valid);
 
 int ShmapCell2Point(int ix, int iy, double *x, double *y);
 
@@ -237,7 +240,8 @@ static inline int ShmapIsCellInMap(int x, int y){
  */
 static inline int ShampIsPointInMap(double x_m, double y_m){
         int x, y;
-        ShmapPoint2Cell(x_m, y_m, &x, &y);
+       bool valid;
+        ShmapPoint2Cell(x_m, y_m, &x, &y, &valid);
        return ShmapIsCellInMap(x, y);
 }
 

diff --git a/src/robodim/eb2008/robodim_eb2008.h b/src/robodim/eb2008/robodim_eb2008.h
index 1081e7f28eb48125ddeec32057b65192f6a0f96c..604d842a7c22209871d4ec915a887172174d4679 100644 (file)
--- a/src/robodim/eb2008/robodim_eb2008.h
+++ b/src/robodim/eb2008/robodim_eb2008.h
@@ -18,15 +18,15 @@
  * ROBOT DIMENSIONS 
  *                                   S0
  *       S4 +--------------------------+ S2
- *        ^ |   |     |                |---|
+ *        ^ |   |     |                |--,
  *     ^  | |   -------    ABE         |   |
  *   RR|  | |      :<--------------------->|
- *     v  | |      :                   |   |
- *       W| |      + Center  (L)       |   |
- *        | |  AB  :       AF          |   |
+ *     v  | |      :                   |--' 
+ *       W| |      + Center  (L)       |    
+ *        | |  AB  :       AF          |--,
  *        | |<---->:<----------------->|   |
  *        | |   -------                |   |
- *        v |   |     |                |---|
+ *        v |   |     |                |--' 
  *       S5 +--------------------------+ S3
  *                 <-->              S1
  *                  WR
@@ -46,8 +46,8 @@
 #define ROBOT_AXIS_TO_BACK_M (ROBOT_AXIS_TO_BACK_MM/1000.0)
 #define ROBOT_AXIS_TO_FRONT_MM 183 /* AF */
 #define ROBOT_AXIS_TO_FRONT_M (ROBOT_AXIS_TO_FRONT_MM/1000.0)
-#define ROBOT_AXIS_TO_BELT_MM 280 /* ABE */ /* FIXME: no belt in the Eurobot 2008 */
-#define ROBOT_AXIS_TO_BELT_M (ROBOT_AXIS_TO_BELT_MM/1000.0)
+#define ROBOT_AXIS_TO_BRUSH_MM 280 /* ABE */
+#define ROBOT_AXIS_TO_BRUSH_M (ROBOT_AXIS_TO_BRUSH_MM/1000.0)
 
 
 /**

diff --git a/src/robofsm/eb2008/fsmmove.cc b/src/robofsm/eb2008/fsmmove.cc
index ed48905426107e3c04effe907aed3d6dca397ee5..4ab02632f1d0da217937fb9e6ee46f644827e180 100644 (file)
--- a/src/robofsm/eb2008/fsmmove.cc
+++ b/src/robofsm/eb2008/fsmmove.cc
@@ -105,19 +105,26 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
        Pos future_pos;
        struct map *map = robot.map;
        int xcell, ycell;
+       double x, y;
+       bool valid;
 
-       traj->getRefPos(current_time+0.5, future_pos);
+       traj->getRefPos(current_time+1/*s*/, future_pos);
 
-       ShmapPoint2Cell(future_pos.x, future_pos.y, &xcell, &ycell);
-       printf("Checking cell %2d %2d", xcell, ycell);
+       /* Ignore obstacles when turning */
+       if (fabs(future_pos.v) < 0.01)
+               return;
+
+       x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
+       y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
+       
+       ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
+       if (!valid)
+               return;
        if (map->cells[ycell][xcell].detected_obstacle > 0) {
-               printf(" obstacle");
                if (sem_trywait(&recalculation_not_running) == 0) {
-                       printf(" - starting recalc");
                        sem_post(&start_recalculation);
                }
        }
-       printf("\n");
 }
 
 static void do_control()
@@ -376,10 +383,8 @@ static enum target_status new_target(struct move_target *target)
 /* This recalculation must only be triggered from mvoement state. */
 void *thread_trajectory_realculation(void *arg)
 {
-       printf("Recalculation thread ready");
        while (1) {
                sem_wait(&start_recalculation);
-               printf("Recalculation started");
                /* TODO: Different start for planning than esitmated position */
                enum target_status ret = new_goal(&current_target);     
                switch (ret) {                                          

diff --git a/src/robofsm/eb2008/map_handling.c b/src/robofsm/eb2008/map_handling.c
index 7f147684c6474077e70dcf1b7c73ffada2dc16d1..5c9be54a544d295848f13b499303f331508ea92d 100644 (file)
--- a/src/robofsm/eb2008/map_handling.c
+++ b/src/robofsm/eb2008/map_handling.c
@@ -19,12 +19,13 @@ void obstacle_detected_at(double x, double y)
        struct est_pos_type est_pos;
        struct map *map = robot.map;
        double xx, yy;
+       bool valid;
 
        if (!map)
                return;
 
-       ShmapPoint2Cell(x, y, &xcell, &ycell);
-       if (!ShmapIsCellInMap(xcell, ycell))
+       ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
+       if (!valid)
                return;
 
        /* The obstacle was detected here */