Fawkes API  Fawkes Development Version
map_cspace.cpp
1 
2 /***************************************************************************
3  * cspace.cpp: Cached distance map
4  *
5  * Created: Thu May 24 18:46:12 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 
30 #include <queue>
31 #include <math.h>
32 #include <stdlib.h>
33 #include <string.h>
34 #include "map.h"
35 
36 /// @cond EXTERNAL
37 
38 class CellData
39 {
40  public:
41  map_t* map_;
42  unsigned int i_, j_;
43  unsigned int src_i_, src_j_;
44 };
45 
46 class CachedDistanceMap
47 {
48  public:
49  CachedDistanceMap(double scale, double max_dist) :
50  distances_(NULL), scale_(scale), max_dist_(max_dist)
51  {
52  cell_radius_ = max_dist / scale;
53  distances_ = new double *[cell_radius_+2];
54  for(int i=0; i<=cell_radius_+1; i++)
55  {
56  distances_[i] = new double[cell_radius_+2];
57  for(int j=0; j<=cell_radius_+1; j++)
58  {
59  distances_[i][j] = sqrt(i*i + j*j);
60  }
61  }
62  }
63  ~CachedDistanceMap()
64  {
65  if(distances_)
66  {
67  for(int i=0; i<=cell_radius_+1; i++)
68  delete[] distances_[i];
69  delete[] distances_;
70  }
71  }
72  double** distances_;
73  double scale_;
74  double max_dist_;
75  int cell_radius_;
76 };
77 
78 
79 bool operator<(const CellData& a, const CellData& b)
80 {
81  return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
82 }
83 
84 CachedDistanceMap*
85 get_distance_map(double scale, double max_dist)
86 {
87  static CachedDistanceMap* cdm = NULL;
88 
89  if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
90  {
91  if(cdm)
92  delete cdm;
93  cdm = new CachedDistanceMap(scale, max_dist);
94  }
95 
96  return cdm;
97 }
98 
99 void enqueue(map_t* map, unsigned int i, unsigned int j,
100  unsigned int src_i, unsigned int src_j,
101  std::priority_queue<CellData>& Q,
102  CachedDistanceMap* cdm,
103  unsigned char* marked)
104 {
105  if(marked[MAP_INDEX(map, i, j)])
106  return;
107 
108  unsigned int di = abs(i - src_i);
109  unsigned int dj = abs(j - src_j);
110  double distance = cdm->distances_[di][dj];
111 
112  if(distance > cdm->cell_radius_)
113  return;
114 
115  map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
116 
117  CellData cell;
118  cell.map_ = map;
119  cell.i_ = i;
120  cell.j_ = j;
121  cell.src_i_ = src_i;
122  cell.src_j_ = src_j;
123 
124  Q.push(cell);
125 
126  marked[MAP_INDEX(map, i, j)] = 1;
127 }
128 
129 // Update the cspace distance values
130 void map_update_cspace(map_t *map, double max_occ_dist)
131 {
132  unsigned char* marked;
133  std::priority_queue<CellData> Q;
134 
135  marked = new unsigned char[map->size_x*map->size_y];
136  memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
137 
138  map->max_occ_dist = max_occ_dist;
139 
140  CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
141 
142  // Enqueue all the obstacle cells
143  CellData cell;
144  cell.map_ = map;
145  for(int i=0; i<map->size_x; i++)
146  {
147  cell.src_i_ = cell.i_ = i;
148  for(int j=0; j<map->size_y; j++)
149  {
150  if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
151  {
152  map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
153  cell.src_j_ = cell.j_ = j;
154  marked[MAP_INDEX(map, i, j)] = 1;
155  Q.push(cell);
156  }
157  else
158  map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
159  }
160  }
161 
162  while(!Q.empty())
163  {
164  CellData current_cell = Q.top();
165  if(current_cell.i_ > 0)
166  enqueue(map, current_cell.i_-1, current_cell.j_,
167  current_cell.src_i_, current_cell.src_j_,
168  Q, cdm, marked);
169  if(current_cell.j_ > 0)
170  enqueue(map, current_cell.i_, current_cell.j_-1,
171  current_cell.src_i_, current_cell.src_j_,
172  Q, cdm, marked);
173  if((int)current_cell.i_ < map->size_x - 1)
174  enqueue(map, current_cell.i_+1, current_cell.j_,
175  current_cell.src_i_, current_cell.src_j_,
176  Q, cdm, marked);
177  if((int)current_cell.j_ < map->size_y - 1)
178  enqueue(map, current_cell.i_, current_cell.j_+1,
179  current_cell.src_i_, current_cell.src_j_,
180  Q, cdm, marked);
181 
182  Q.pop();
183  }
184 
185  delete[] marked;
186 }
187 
188 #if 0
189 // TODO: replace this with a more efficient implementation. Not crucial,
190 // because we only do it once, at startup.
191 void map_update_cspace(map_t *map, double max_occ_dist)
192 {
193  int i, j;
194  int ni, nj;
195  int s;
196  double d;
197  map_cell_t *cell, *ncell;
198 
199  map->max_occ_dist = max_occ_dist;
200  s = (int) ceil(map->max_occ_dist / map->scale);
201 
202  // Reset the distance values
203  for (j = 0; j < map->size_y; j++)
204  {
205  for (i = 0; i < map->size_x; i++)
206  {
207  cell = map->cells + MAP_INDEX(map, i, j);
208  cell->occ_dist = map->max_occ_dist;
209  }
210  }
211 
212  // Find all the occupied cells and update their neighbours
213  for (j = 0; j < map->size_y; j++)
214  {
215  for (i = 0; i < map->size_x; i++)
216  {
217  cell = map->cells + MAP_INDEX(map, i, j);
218  if (cell->occ_state != +1)
219  continue;
220 
221  cell->occ_dist = 0;
222 
223  // Update adjacent cells
224  for (nj = -s; nj <= +s; nj++)
225  {
226  for (ni = -s; ni <= +s; ni++)
227  {
228  if (!MAP_VALID(map, i + ni, j + nj))
229  continue;
230 
231  ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
232  d = map->scale * sqrt(ni * ni + nj * nj);
233 
234  if (d < ncell->occ_dist)
235  ncell->occ_dist = d;
236  }
237  }
238  }
239  }
240 
241  return;
242 }
243 #endif
244 
245 /// @endcond
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62