modules/devices/robot/obstacle_avoidance/obstacle_avoidance.c

   1 /*
   2  *  Copyright Droids Corporation (2007)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: obstacle_avoidance.c,v 1.1.2.8 2009-05-02 10:00:35 zer0 Exp $
  19  *
  20  *  Main code and algorithm: Fabrice DESCLAUX <serpilliere@droids-corp.org>
  21  *  Integration in Aversive: Olivier MATZ <zer0@droids-corp.org>
  22  */
  23
  24 #include <aversive.h>
  25 #include <aversive/error.h>
  26
  27 #include <math.h>
  28 #include <stdio.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31
  32 #include <vect_base.h>
  33 #include <lines.h>
  34 #include <polygon.h>
  35
  36 #include <obstacle_avoidance.h>
  37
  38 #define GET_PT(a) (&(a) - &(oa.points[0]))
  39
  40 static struct obstacle_avoidance oa;
  41
  42 static void __oa_start_end_points(int32_t st_x, int32_t st_y,
  43                                   int32_t en_x, int32_t en_y);
  44
  45 /* reset oa without reseting points coord */
  46 void oa_reset(void)
  47 {
  48         DEBUG(E_OA, "%s()", __FUNCTION__);
  49
  50         memset(oa.valid, 0, sizeof(oa.valid));
  51         memset(oa.pweight, 0, sizeof(oa.pweight));
  52         memset(oa.weight, 0, sizeof(oa.weight));
  53         memset(oa.p, 0, sizeof(oa.p));
  54         memset(oa.pt, 0, sizeof(oa.pt));
  55
  56
  57 }
  58
  59 /** Init the oa structure. Note: In the algorithm, the first polygon
  60  * is a dummy one, and is used to represent the START and END points
  61  * (so it has 2 vertices) */
  62 void oa_init(void)
  63 {
  64         DEBUG(E_OA, "%s()", __FUNCTION__);
  65         memset(&oa, 0, sizeof(oa));
  66
  67         /* set a default start and point, reserve the first poly and
  68          * the first 2 points for it */
  69         oa.polys[0].pts = oa.points;
  70         oa.polys[0].l = 2;
  71         __oa_start_end_points(0, 0, 100, 100);
  72         oa.cur_pt_idx = 2;
  73         oa.cur_poly_idx = 1;
  74 }
  75
  76 /**
  77  * Set the start and destination point. Return 0 on sucess
  78  */
  79 static void __oa_start_end_points(int32_t st_x, int32_t st_y,
  80                                   int32_t en_x, int32_t en_y)
  81 {
  82         /* we always use the first 2 points of the table for start and end */
  83         oa.points[0].x = en_x;
  84         oa.points[0].y = en_y;
  85
  86         /* Each point processed by Dijkstra is marked as valid. If we
  87          * have unreachable points (out of playground or points inside
  88          * polygons) Disjkstra won't mark them as valid. At the end of
  89          * the algorithm, if the destination point is not marked as
  90          * valid, there's no valid path to reach it. */
  91
  92         oa.valid[GET_PT(oa.points[0])] = 0;
  93         /* the real dest is the start point for the algorithm */
  94         oa.pweight[GET_PT(oa.points[0])] = 1;
  95
  96         oa.points[1].x = st_x;
  97         oa.points[1].y = st_y;
  98         oa.valid[GET_PT(oa.points[1])] = 0;
  99         oa.pweight[GET_PT(oa.points[1])] = 0;
 100 }
 101
 102 /**
 103  * Set the start and destination point. Return 0 on sucess
 104  */
 105 void oa_start_end_points(int32_t st_x, int32_t st_y,
 106                          int32_t en_x, int32_t en_y)
 107 {
 108         DEBUG(E_OA, "%s() (%ld,%ld) (%ld,%ld)", __FUNCTION__,
 109               st_x, st_y, en_x, en_y);
 110
 111         __oa_start_end_points(st_x, st_y, en_x, en_y);
 112 }
 113
 114
 115 /**
 116  * Create a new obstacle polygon. Return NULL on error.
 117  */
 118 poly_t *oa_new_poly(uint8_t size)
 119 {
 120         DEBUG(E_OA, "%s(size=%d)", __FUNCTION__, size);
 121
 122         if (oa.cur_pt_idx + size > MAX_PTS)
 123                 return NULL;
 124         if (oa.cur_poly_idx + 1 > MAX_POLY)
 125                 return NULL;
 126
 127         oa.polys[oa.cur_poly_idx].l = size;
 128         oa.polys[oa.cur_poly_idx].pts = &oa.points[oa.cur_pt_idx];
 129         oa.cur_pt_idx += size;
 130
 131         return &oa.polys[oa.cur_poly_idx++];
 132 }
 133
 134 /**
 135  * Add a point to the polygon.
 136  */
 137 void oa_poly_set_point(poly_t *pol,
 138                          int32_t x, int32_t y, uint8_t i)
 139 {
 140         DEBUG(E_OA, "%s() (%ld,%ld)", __FUNCTION__, x, y);
 141
 142         pol->pts[i].x = x;
 143         pol->pts[i].y = y;
 144         oa.valid[GET_PT(pol->pts[i])] = 0;
 145         oa.pweight[GET_PT(pol->pts[i])] = 0;
 146 }
 147
 148 point_t * oa_get_path(void)
 149 {
 150         return oa.u.res;
 151 }
 152
 153 void oa_dump(void)
 154 {
 155         uint8_t i,j;
 156         poly_t *poly;
 157         point_t *pt;
 158
 159         printf_P(PSTR("-- OA dump --\r\n"));
 160         printf_P(PSTR("nb_polys: %d\r\n"), oa.cur_poly_idx);
 161         printf_P(PSTR("nb_pts: %d\r\n"), oa.cur_pt_idx);
 162         for (i=0; i<oa.cur_poly_idx; i++) {
 163                 poly = &oa.polys[i];
 164                 printf_P(PSTR("poly #%d\r\n"), i);
 165                 for (j=0; j<poly->l; j++) {
 166                         pt = &poly->pts[j];
 167                         printf_P(PSTR("  pt #%d (%2.2f,%2.2f)\r\n"), j, pt->x, pt->y);
 168                 }
 169         }
 170 }
 171
 172 /* Iterative Dijkstra algorithm: The valid filed is used to determine if:
 173  *   1: this point has been visited, his weight is correct.
 174  *   2: the point must be visited.
 175  *
 176  * The algorithm does: find a point that must be visited (2) update
 177  * his weight, mark it as (1) and update all his neightbours has 2.
 178  *
 179  * The algorithm ends when no (2) points are found
 180  *
 181  * A point with weight 0 is a point that has not been visited (so
 182  * weight is not affected yet); This explain why first point must have
 183  * a start weight different than 0.
 184  *
 185  * When the algo finds a shorter path to reach a point B from point A,
 186  * it will store in (p, pt) the parent point. This is important to
 187  * remenber and extract the solution path. */
 188 void
 189 dijkstra(uint8_t start_p, uint8_t start)
 190 {
 191         uint8_t i;
 192         int8_t add;
 193         int8_t finish = 0;
 194         /* weight == 0 means not visited */
 195         /* weight == 1 for start */
 196
 197         /* find all point linked to start */
 198
 199         oa.valid[GET_PT(oa.polys[start_p].pts[start])] = 2;
 200
 201         while (!finish){
 202                 finish = 1;
 203
 204                 for (start_p = 0;start_p<MAX_POLY;start_p++) {
 205                         for (start = 0;start<oa.polys[start_p].l;start++) {
 206                                 if (oa.valid[GET_PT(oa.polys[start_p].pts[start])] != 2)
 207                                         continue;
 208                                 add = -2;
 209
 210                                 /* For all points that must be
 211                                  * visited, we look for rays that
 212                                  * start or stop at this point.  As
 213                                  * ray array is (poly_num, point_num)
 214                                  * we wtep 2 by 2. */
 215                                 for (i=0 ; i<oa.ray_n ; i+=2) {
 216
 217                                         /* If index is even in the
 218                                          * aray, we have a start point
 219                                          * ray, so connected point is
 220                                          * i+2;
 221                                          *
 222                                          * If index is odd, we are in stop
 223                                          * point and ray start point is at
 224                                          * i-2 pos */
 225                                         add = -add;
 226
 227                                         if (start_p != oa.u.rays[i] || start != oa.u.rays[i+1])
 228                                                 continue;
 229
 230                                         if ((oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] != 0) &&
 231                                             (oa.pweight[GET_PT(oa.polys[start_p].pts[start])]+oa.weight[i/4] >=
 232                                              oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])]))
 233                                                 continue;
 234
 235                                         oa.p[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] = start_p;
 236                                         oa.pt[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] = start;
 237                                         oa.valid[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])]=2;
 238                                         oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] =
 239                                                 oa.pweight[GET_PT(oa.polys[start_p].pts[start])]+oa.weight[i/4];
 240
 241                                         oa.valid[GET_PT(oa.polys[start_p].pts[start])] = 1;
 242                                         finish = 0;
 243                                         DEBUG(E_OA, "%s() (%ld,%ld p=%ld) %ld (%ld,%ld p=%ld)\r\n", __FUNCTION__,
 244                                               oa.polys[start_p].pts[start].x, oa.polys[start_p].pts[start].y,
 245                                               oa.pweight[GET_PT(oa.polys[start_p].pts[start])],oa.weight[i/4],
 246
 247                                               oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]].x,
 248                                               oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]].y,
 249                                               oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])]
 250                                               );
 251                                 }
 252                         }
 253                 }
 254         }
 255 }
 256
 257
 258 /* display the path */
 259 int8_t
 260 get_path(poly_t *polys, uint8_t *rays)
 261 {
 262         uint8_t p, pt, p1, pt1, i;
 263
 264         p=0;
 265         pt=1;
 266         i=0;
 267
 268         /* forget the first point */
 269
 270         while (!(p==0 && pt==0)) {
 271
 272                 if (i>=MAX_CHKPOINTS)
 273                         return -1;
 274
 275                 if (oa.valid[GET_PT(polys[p].pts[pt])]==0) {
 276                         DEBUG(E_OA, "invalid path!");
 277                         return -1;
 278                 }
 279
 280                 p1 = oa.p[GET_PT(polys[p].pts[pt])];
 281                 pt1 =  oa.pt[GET_PT(polys[p].pts[pt])];
 282                 p = p1; pt = pt1;
 283                 oa.u.res[i].x = polys[p].pts[pt].x;
 284                 oa.u.res[i].y = polys[p].pts[pt].y;
 285                 DEBUG(E_OA, "result[%d]: %d, %d", i, oa.u.res[i].x, oa.u.res[i].y);
 286                 i++;
 287         }
 288
 289         return i;
 290 }
 291
 292 int8_t
 293 oa_process(void)
 294 {
 295         uint8_t ret;
 296         uint8_t i;
 297
 298         /* First we compute the visibility graph */
 299         ret = calc_rays(oa.polys, oa.cur_poly_idx, oa.u.rays);
 300         DEBUG(E_OA, "nb_rays = %d", ret);
 301
 302         DEBUG(E_OA, "Ray list");
 303         for (i=0;i<ret;i+=4) {
 304                 DEBUG(E_OA, "%d,%d -> %d,%d", oa.u.rays[i], oa.u.rays[i+1],
 305                       oa.u.rays[i+2], oa.u.rays[i+3]);
 306         }
 307
 308         /* Then we affect the rays lengths to their weights */
 309         calc_rays_weight(oa.polys, oa.cur_poly_idx,
 310                          oa.u.rays, ret, oa.weight);
 311
 312         DEBUG(E_OA, "Ray weights");
 313         for (i=0;i<ret;i+=4) {
 314                 DEBUG(E_OA, "%d,%d -> %d,%d (%d)",
 315                        (int)oa.polys[oa.u.rays[i]].pts[oa.u.rays[i+1]].x,
 316                        (int)oa.polys[oa.u.rays[i]].pts[oa.u.rays[i+1]].y,
 317                        (int)oa.polys[oa.u.rays[i+2]].pts[oa.u.rays[i+3]].x,
 318                        (int)oa.polys[oa.u.rays[i+2]].pts[oa.u.rays[i+3]].y,
 319                        oa.weight[i/4]);
 320         }
 321
 322         /* We aplly dijkstra on the visibility graph from the start
 323          * point (point 0 of the polygon 0) */
 324         oa.ray_n = ret;
 325         DEBUG(E_OA, "dijkstra ray_n = %d", ret);
 326         dijkstra(0, 0);
 327
 328         /* As dijkstra sets the parent points in the resulting graph,
 329          * we can backtrack the solution path. */
 330         return get_path(oa.polys, oa.u.rays);
 331 }