Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
amcl_utils.cpp
1 /***************************************************************************
2  * amcl_utils.cpp - AMCL utils
3  *
4  * Created: Thu Aug 23 18:10:03 2012
5  * Copyright 2012 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "amcl_utils.h"
22 #include <fvutils/readers/png.h>
23 #include <config/config.h>
24 #include <cstdlib>
25 
26 using namespace firevision;
27 
28 namespace fawkes {
29  namespace amcl {
30 #if 0 /* just to make Emacs auto-indent happy */
31  }
32 }
33 #endif
34 
35 // compute linear index for given map coords
36 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
37 
38 map_t *
39 read_map(const char *map_file,
40  float origin_x, float origin_y, float resolution,
41  float occupied_threshold, float free_threshold,
42  std::vector<std::pair<int, int> > &free_space_indices)
43 {
44  map_t *map;
45 
46  firevision::PNGReader png_reader(map_file);
47  unsigned int map_width = png_reader.pixel_width();
48  unsigned int map_height = png_reader.pixel_height();
49  unsigned char *img_buffer = malloc_buffer(firevision::YUV422_PLANAR,
50  map_width, map_height);
51  png_reader.set_buffer(img_buffer);
52  png_reader.read();
53 
54  map = map_alloc();
55  map->size_x = map_width;
56  map->size_y = map_height;
57  map->scale = resolution;
58  map->origin_x = origin_x + (map->size_x / 2) * map->scale;
59  map->origin_y = origin_y + (map->size_y / 2) * map->scale;
60  map->cells =
61  (map_cell_t*) malloc(sizeof(map_cell_t) * map->size_x * map->size_y);
62 
63  for (unsigned int h = 0; h < map_height; ++h) {
64  for (unsigned int w = 0; w < map_width; ++w) {
65  unsigned int i = h * map_width + w;
66  float y = (255 - img_buffer[i]) / 255.;
67 
68  // Note that we invert the graphics-ordering of the pixels to
69  // produce a map with cell (0,0) in the lower-left corner.
70 
71  if (y > occupied_threshold) {
72  map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = +1;
73  } else if (y <= free_threshold) {
74  map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = -1;
75  free_space_indices.push_back(std::make_pair(w,map_height - h - 1));
76  } else {
77  map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = 0;
78  }
79  }
80  }
81  free(img_buffer);
82 
83  return map;
84 }
85 
86 
87 
88 void
89 read_map_config(Configuration *config,
90  std::string &cfg_map_file, float &cfg_resolution,
91  float &cfg_origin_x, float &cfg_origin_y, float &cfg_origin_theta,
92  float &cfg_occupied_thresh, float &cfg_free_thresh)
93 {
94  cfg_map_file =
95  std::string(CONFDIR) + "/" + config->get_string(CFG_PREFIX"map_file");
96  cfg_resolution = config->get_float(CFG_PREFIX"resolution");
97  cfg_origin_x = config->get_float(CFG_PREFIX"origin_x");
98  cfg_origin_y = config->get_float(CFG_PREFIX"origin_y");
99  cfg_origin_theta = config->get_float(CFG_PREFIX"origin_theta");
100  cfg_occupied_thresh = config->get_float(CFG_PREFIX"occupied_threshold");
101  cfg_free_thresh = config->get_float(CFG_PREFIX"free_threshold");
102 }
103 
104 
105 } // end namespace amcl
106 } // end namespace fawkes
PNG file reader.
Definition: png.h:36