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


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:19