21#include "amcl_utils.h"
23#include <config/config.h>
24#include <fvutils/readers/png.h>
28using namespace firevision;
34#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
47read_map(
const char * map_file,
51 float occupied_threshold,
53 std::vector<std::pair<int, int>> &free_space_indices)
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);
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);
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.;
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));
86 map->cells[MAP_IDX(map_width, w, map_height - h - 1)].occ_state = 0;
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)
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());
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.
Fawkes library namespace.