Fawkes API  Fawkes Development Version
amcl_utils.cpp
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