Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * amcl_utils.cpp - AMCL utils 00003 * 00004 * Created: Thu Aug 23 18:10:03 2012 00005 * Copyright 2012 Tim Niemueller [www.niemueller.de] 00006 ****************************************************************************/ 00007 00008 /* This program is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation; either version 2 of the License, or 00011 * (at your option) any later version. 00012 * 00013 * This program is distributed in the hope that it will be useful, 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 * GNU Library General Public License for more details. 00017 * 00018 * Read the full text in the LICENSE.GPL file in the doc directory. 00019 */ 00020 00021 #include "amcl_utils.h" 00022 #include <fvutils/readers/png.h> 00023 #include <config/config.h> 00024 #include <cstdlib> 00025 00026 using namespace firevision; 00027 00028 namespace fawkes { 00029 namespace amcl { 00030 #if 0 /* just to make Emacs auto-indent happy */ 00031 } 00032 } 00033 #endif 00034 00035 // compute linear index for given map coords 00036 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) 00037 00038 map_t * 00039 read_map(const char *map_file, 00040 float origin_x, float origin_y, float resolution, 00041 float occupied_threshold, float free_threshold, 00042 std::vector<std::pair<int, int> > &free_space_indices) 00043 { 00044 map_t *map; 00045 00046 firevision::PNGReader png_reader(map_file); 00047 unsigned int map_width = png_reader.pixel_width(); 00048 unsigned int map_height = png_reader.pixel_height(); 00049 unsigned char *img_buffer = malloc_buffer(firevision::YUV422_PLANAR, 00050 map_width, map_height); 00051 png_reader.set_buffer(img_buffer); 00052 png_reader.read(); 00053 00054 map = map_alloc(); 00055 map->size_x = map_width; 00056 map->size_y = map_height; 00057 map->scale = resolution; 00058 map->origin_x = origin_x + (map->size_x / 2) * map->scale; 00059 map->origin_y = origin_y + (map->size_y / 2) * map->scale; 00060 map->cells = 00061 (map_cell_t*) malloc(sizeof(map_cell_t) * map->size_x * map->size_y); 00062 00063 for (unsigned int h = 0; h < map_height; ++h) { 00064 for (unsigned int w = 0; w < map_width; ++w) { 00065 unsigned int i = h * map_width + w; 00066 float y = (255 - img_buffer[i]) / 255.; 00067 00068 // Note that we invert the graphics-ordering of the pixels to 00069 // produce a map with cell (0,0) in the lower-left corner. 00070 00071 if (y > occupied_threshold) { 00072 map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = +1; 00073 } else if (y <= free_threshold) { 00074 map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = -1; 00075 free_space_indices.push_back(std::make_pair(w,map_height - h - 1)); 00076 } else { 00077 map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = 0; 00078 } 00079 } 00080 } 00081 free(img_buffer); 00082 00083 return map; 00084 } 00085 00086 00087 00088 void 00089 read_map_config(Configuration *config, 00090 std::string &cfg_map_file, float &cfg_resolution, 00091 float &cfg_origin_x, float &cfg_origin_y, float &cfg_origin_theta, 00092 float &cfg_occupied_thresh, float &cfg_free_thresh) 00093 { 00094 cfg_map_file = 00095 std::string(CONFDIR) + "/" + config->get_string(CFG_PREFIX"map_file"); 00096 cfg_resolution = config->get_float(CFG_PREFIX"resolution"); 00097 cfg_origin_x = config->get_float(CFG_PREFIX"origin_x"); 00098 cfg_origin_y = config->get_float(CFG_PREFIX"origin_y"); 00099 cfg_origin_theta = config->get_float(CFG_PREFIX"origin_theta"); 00100 cfg_occupied_thresh = config->get_float(CFG_PREFIX"occupied_threshold"); 00101 cfg_free_thresh = config->get_float(CFG_PREFIX"free_threshold"); 00102 } 00103 00104 00105 } // end namespace amcl 00106 } // end namespace fawkes