Fawkes API  Fawkes Development Version
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 /** Read map.
39  * @param map_file filename of map
40  * @param origin_x origin x offset
41  * @param origin_y origin y offset
42  * @param resolution map resolution
43  * @param occupied_threshold minimum threshold when to consider a cell occupied
44  * @param free_threshold maximum threshold when to consider a cell free
45  * @param free_space_indices upon return contains indices of free cells
46  * @return loaded map
47  */
48 map_t *
49 read_map(const char *map_file,
50  float origin_x, float origin_y, float resolution,
51  float occupied_threshold, float free_threshold,
52  std::vector<std::pair<int, int> > &free_space_indices)
53 {
54  map_t *map;
55 
56  firevision::PNGReader png_reader(map_file);
57  unsigned int map_width = png_reader.pixel_width();
58  unsigned int map_height = png_reader.pixel_height();
59  unsigned char *img_buffer = malloc_buffer(firevision::YUV422_PLANAR,
60  map_width, map_height);
61  png_reader.set_buffer(img_buffer);
62  png_reader.read();
63 
64  map = map_alloc();
65  map->size_x = map_width;
66  map->size_y = map_height;
67  map->scale = resolution;
68  map->origin_x = origin_x + (map->size_x / 2) * map->scale;
69  map->origin_y = origin_y + (map->size_y / 2) * map->scale;
70  map->cells =
71  (map_cell_t*) malloc(sizeof(map_cell_t) * map->size_x * map->size_y);
72 
73  for (unsigned int h = 0; h < map_height; ++h) {
74  for (unsigned int w = 0; w < map_width; ++w) {
75  unsigned int i = h * map_width + w;
76  float y = (255 - img_buffer[i]) / 255.;
77 
78  // Note that we invert the graphics-ordering of the pixels to
79  // produce a map with cell (0,0) in the lower-left corner.
80 
81  if (y > occupied_threshold) {
82  map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = +1;
83  } else if (y <= free_threshold) {
84  map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = -1;
85  free_space_indices.push_back(std::make_pair(w,map_height - h - 1));
86  } else {
87  map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = 0;
88  }
89  }
90  }
91  free(img_buffer);
92 
93  return map;
94 }
95 
96 
97 /** Read map configuration.
98  * @param config configuration to read from
99  * @param cfg_map_file upon returns contains map filename
100  * @param cfg_resolution upon return contains map resolution
101  * @param cfg_origin_x upon return contains origin x offset
102  * @param cfg_origin_y upon return contains origin y offset
103  * @param cfg_occupied_thresh upon return contains minimum threshold
104  * when to consider a cell occupied
105  * @param cfg_free_thresh upon return contains maximum threshold when
106  * to consider a cell free
107  * @param cfg_prefix optional config path prefix
108  */
109 void
110 read_map_config(Configuration *config,
111  std::string &cfg_map_file, float &cfg_resolution,
112  float &cfg_origin_x, float &cfg_origin_y, float &cfg_origin_theta,
113  float &cfg_occupied_thresh, float &cfg_free_thresh,
114  std::string cfg_prefix)
115 {
116  cfg_map_file =
117  std::string(CONFDIR) + "/" + config->get_string((cfg_prefix + "map_file").c_str());
118  cfg_resolution = config->get_float((cfg_prefix + "resolution").c_str());
119  cfg_origin_x = config->get_float((cfg_prefix + "origin_x").c_str());
120  cfg_origin_y = config->get_float((cfg_prefix + "origin_y").c_str());
121  cfg_origin_theta = config->get_float((cfg_prefix + "origin_theta").c_str());
122  cfg_occupied_thresh = config->get_float((cfg_prefix + "occupied_threshold").c_str());
123  cfg_free_thresh = config->get_float((cfg_prefix + "free_threshold").c_str());
124 }
125 
126 
127 } // end namespace amcl
128 } // end namespace fawkes
Fawkes library namespace.
PNG file reader.
Definition: png.h:36