map_cspace.cpp
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 
00022 #include <queue>
00023 #include <math.h>
00024 #include <stdlib.h>
00025 #include <string.h>
00026 #include "amcl/map/map.h"
00027 
00028 class CellData
00029 {
00030   public:
00031     map_t* map_;
00032     unsigned int i_, j_;
00033     unsigned int src_i_, src_j_;
00034 };
00035 
00036 class CachedDistanceMap
00037 {
00038   public:
00039     CachedDistanceMap(double scale, double max_dist) : 
00040       distances_(NULL), scale_(scale), max_dist_(max_dist) 
00041     {
00042       cell_radius_ = max_dist / scale;
00043       distances_ = new double *[cell_radius_+2];
00044       for(int i=0; i<=cell_radius_+1; i++)
00045       {
00046         distances_[i] = new double[cell_radius_+2];
00047         for(int j=0; j<=cell_radius_+1; j++)
00048         {
00049           distances_[i][j] = sqrt(i*i + j*j);
00050         }
00051       }
00052     }
00053     ~CachedDistanceMap()
00054     {
00055       if(distances_)
00056       {
00057         for(int i=0; i<=cell_radius_+1; i++)
00058           delete[] distances_[i];
00059         delete[] distances_;
00060       }
00061     }
00062     double** distances_;
00063     double scale_;
00064     double max_dist_;
00065     int cell_radius_;
00066 };
00067 
00068 
00069 bool operator<(const CellData& a, const CellData& b)
00070 {
00071   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;
00072 }
00073 
00074 CachedDistanceMap*
00075 get_distance_map(double scale, double max_dist)
00076 {
00077   static CachedDistanceMap* cdm = NULL;
00078 
00079   if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
00080   {
00081     if(cdm)
00082       delete cdm;
00083     cdm = new CachedDistanceMap(scale, max_dist);
00084   }
00085 
00086   return cdm;
00087 }
00088 
00089 void enqueue(map_t* map, unsigned int i, unsigned int j, 
00090              unsigned int src_i, unsigned int src_j,
00091              std::priority_queue<CellData>& Q,
00092              CachedDistanceMap* cdm,
00093              unsigned char* marked)
00094 {
00095   if(marked[MAP_INDEX(map, i, j)])
00096     return;
00097 
00098   unsigned int di = abs(i - src_i);
00099   unsigned int dj = abs(j - src_j);
00100   double distance = cdm->distances_[di][dj];
00101 
00102   if(distance > cdm->cell_radius_)
00103     return;
00104 
00105   map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
00106 
00107   CellData cell;
00108   cell.map_ = map;
00109   cell.i_ = i;
00110   cell.j_ = j;
00111   cell.src_i_ = src_i;
00112   cell.src_j_ = src_j;
00113 
00114   Q.push(cell);
00115 
00116   marked[MAP_INDEX(map, i, j)] = 1;
00117 }
00118 
00119 // Update the cspace distance values
00120 void map_update_cspace(map_t *map, double max_occ_dist)
00121 {
00122   unsigned char* marked;
00123   std::priority_queue<CellData> Q;
00124 
00125   marked = new unsigned char[map->size_x*map->size_y];
00126   memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
00127 
00128   map->max_occ_dist = max_occ_dist;
00129 
00130   CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
00131 
00132   // Enqueue all the obstacle cells
00133   CellData cell;
00134   cell.map_ = map;
00135   for(int i=0; i<map->size_x; i++)
00136   {
00137     cell.src_i_ = cell.i_ = i;
00138     for(int j=0; j<map->size_y; j++)
00139     {
00140       if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
00141       {
00142         map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
00143         cell.src_j_ = cell.j_ = j;
00144         marked[MAP_INDEX(map, i, j)] = 1;
00145         Q.push(cell);
00146       }
00147       else
00148         map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
00149     }
00150   }
00151 
00152   while(!Q.empty())
00153   {
00154     CellData current_cell = Q.top();
00155     if(current_cell.i_ > 0)
00156       enqueue(map, current_cell.i_-1, current_cell.j_, 
00157               current_cell.src_i_, current_cell.src_j_,
00158               Q, cdm, marked);
00159     if(current_cell.j_ > 0)
00160       enqueue(map, current_cell.i_, current_cell.j_-1, 
00161               current_cell.src_i_, current_cell.src_j_,
00162               Q, cdm, marked);
00163     if((int)current_cell.i_ < map->size_x - 1)
00164       enqueue(map, current_cell.i_+1, current_cell.j_, 
00165               current_cell.src_i_, current_cell.src_j_,
00166               Q, cdm, marked);
00167     if((int)current_cell.j_ < map->size_y - 1)
00168       enqueue(map, current_cell.i_, current_cell.j_+1, 
00169               current_cell.src_i_, current_cell.src_j_,
00170               Q, cdm, marked);
00171 
00172     Q.pop();
00173   }
00174 
00175   delete[] marked;
00176 }
00177 
00178 #if 0
00179 // TODO: replace this with a more efficient implementation.  Not crucial,
00180 // because we only do it once, at startup.
00181 void map_update_cspace(map_t *map, double max_occ_dist)
00182 {
00183   int i, j;
00184   int ni, nj;
00185   int s;
00186   double d;
00187   map_cell_t *cell, *ncell;
00188 
00189   map->max_occ_dist = max_occ_dist;
00190   s = (int) ceil(map->max_occ_dist / map->scale);
00191 
00192   // Reset the distance values
00193   for (j = 0; j < map->size_y; j++)
00194   {
00195     for (i = 0; i < map->size_x; i++)
00196     {
00197       cell = map->cells + MAP_INDEX(map, i, j);
00198       cell->occ_dist = map->max_occ_dist;
00199     }
00200   }
00201 
00202   // Find all the occupied cells and update their neighbours
00203   for (j = 0; j < map->size_y; j++)
00204   {
00205     for (i = 0; i < map->size_x; i++)
00206     {
00207       cell = map->cells + MAP_INDEX(map, i, j);
00208       if (cell->occ_state != +1)
00209         continue;
00210           
00211       cell->occ_dist = 0;
00212 
00213       // Update adjacent cells
00214       for (nj = -s; nj <= +s; nj++)
00215       {
00216         for (ni = -s; ni <= +s; ni++)
00217         {
00218           if (!MAP_VALID(map, i + ni, j + nj))
00219             continue;
00220 
00221           ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
00222           d = map->scale * sqrt(ni * ni + nj * nj);
00223 
00224           if (d < ncell->occ_dist)
00225             ncell->occ_dist = d;
00226         }
00227       }
00228     }
00229   }
00230   
00231   return;
00232 }
00233 #endif


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:02