00001 /* 00002 * wavefront_map_accessor.h 00003 * 00004 * Created on: May 2, 2012 00005 * Author: tkruse 00006 */ 00007 00008 #ifndef WAVEFRONT_MAP_ACCESSOR_H_ 00009 #define WAVEFRONT_MAP_ACCESSOR_H_ 00010 #include <costmap_2d/cost_values.h> 00011 namespace base_local_planner { 00012 00017 class WavefrontMapAccessor : public costmap_2d::Costmap2D { 00018 public: 00019 WavefrontMapAccessor(MapGrid* map, double outer_radius) 00020 : costmap_2d::Costmap2D(map->size_x_, map->size_y_, 1, 0, 0), 00021 map_(map), outer_radius_(outer_radius) { 00022 synchronize(); 00023 } 00024 00025 virtual ~WavefrontMapAccessor(){}; 00026 00027 void synchronize(){ 00028 // Write Cost Data from the map 00029 for(unsigned int x = 0; x < size_x_; x++){ 00030 for (unsigned int y = 0; y < size_y_; y++){ 00031 unsigned int ind = x + (y * size_x_); 00032 if(map_->operator ()(x, y).target_dist == 1) { 00033 costmap_[ind] = costmap_2d::LETHAL_OBSTACLE; 00034 } else { 00035 costmap_[ind] = 0; 00036 } 00037 } 00038 } 00039 } 00040 00041 private: 00042 MapGrid* map_; 00043 double outer_radius_; 00044 }; 00045 00046 } 00047 00048 00049 00050 #endif /* WAVEFRONT_MAP_ACCESSOR_H_ */