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