24 #include "map_filter.h"
26 #include <config/config.h>
27 #include <core/exception.h>
28 #include <utils/math/coord.h>
29 #include <utils/time/time.h>
50 unsigned int in_data_size,
51 std::vector<LaserDataFilter::Buffer *> &in,
54 const std::string & prefix,
58 tf_listener_ = tf_listener;
62 frame_map_ = config_->
get_string(
"/frames/fixed");
64 cfg_occupied_thresh_ = std::numeric_limits<float>::max();
71 LaserMapFilterDataFilter::load_map()
73 std::vector<std::pair<int, int>> free_space_indices;
74 std::string cfg_map_file;
78 float cfg_origin_theta;
79 float cfg_free_thresh;
81 fawkes::amcl::read_map_config(config_,
90 return fawkes::amcl::read_map(cfg_map_file.c_str(),
106 LaserMapFilterDataFilter::is_in_map(
int cell_x,
int cell_y)
108 if (cell_x < 0 || cell_x > map_->size_x || cell_y < 0 || cell_y > map_->size_y) {
117 const unsigned int vecsize =
in.size();
121 for (
unsigned int a = 0; a < vecsize; ++a) {
139 "Can't transform laser-data (%s -> %s)",
141 in[a]->frame.c_str());
146 out[a]->frame =
in[a]->frame;
147 out[a]->timestamp =
in[a]->timestamp;
152 if (std::isfinite(
in[a]->values[i])) {
161 p.setValue(x, y, 0.);
165 int cell_x = (int)MAP_GXWX(map_, p.getX());
166 int cell_y = (int)MAP_GYWY(map_, p.getY());
170 for (
int ox = -num_pixels_; add && ox <= num_pixels_; ++ox) {
171 for (
int oy = -num_pixels_; oy <= num_pixels_; ++oy) {
174 if (MAP_VALID(map_, x, y)) {
175 if (map_->cells[MAP_INDEX(map_, x, y)].occ_state > 0) {
184 out[a]->values[i] =
in[a]->values[i];
186 out[a]->values[i] = std::numeric_limits<float>::quiet_NaN();