21 #include "amcl_utils.h"
22 #include <fvutils/readers/png.h>
23 #include <config/config.h>
26 using namespace firevision;
36 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
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)
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);
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;
61 (map_cell_t*) malloc(
sizeof(map_cell_t) * map->size_x * map->size_y);
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.;
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));
77 map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = 0;
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)
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");