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