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
28using namespace firevision;
29
30namespace fawkes {
31namespace 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 */
46map_t *
47read_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 */
107void
108read_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
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.
PNG file reader.
Definition: png.h:34
Fawkes library namespace.