24 #include "map_filter.h" 26 #include <core/exception.h> 27 #include <utils/math/coord.h> 28 #include <utils/time/time.h> 48 unsigned int in_data_size,
49 std::vector<LaserDataFilter::Buffer *> &in,
55 tf_listener_ = tf_listener;
59 frame_map_ = config_->
get_string(
"/frames/fixed");
60 cfg_occupied_thresh_ = std::numeric_limits<float>::max();
67 LaserMapFilterDataFilter::load_map()
69 std::vector<std::pair<int, int>> free_space_indices;
70 std::string cfg_map_file;
74 float cfg_origin_theta;
75 float cfg_free_thresh;
77 fawkes::amcl::read_map_config(config_,
86 return fawkes::amcl::read_map(cfg_map_file.c_str(),
102 LaserMapFilterDataFilter::is_in_map(
int cell_x,
int cell_y)
104 if (cell_x < 0 || cell_x > map_->size_x || cell_y < 0 || cell_y > map_->size_y) {
113 const unsigned int vecsize =
in.size();
117 for (
unsigned int a = 0; a < vecsize; ++a) {
135 "Can't transform laser-data (%s -> %s)",
137 in[a]->frame.c_str());
142 out[a]->frame =
in[a]->frame;
143 out[a]->timestamp =
in[a]->timestamp;
148 if (std::isfinite(
in[a]->values[i])) {
157 p.setValue(x, y, 0.);
161 int cell_x = (int)MAP_GXWX(map_, p.getX());
162 int cell_y = (int)MAP_GYWY(map_, p.getY());
165 for (
int ox = -2; add && ox <= 2; ++ox) {
166 for (
int oy = -2; oy <= 2; ++oy) {
169 if (MAP_VALID(map_, x, y)) {
170 if (map_->cells[MAP_INDEX(map_, x, y)].occ_state > 0) {
179 out[a]->values[i] =
in[a]->values[i];
181 out[a]->values[i] = std::numeric_limits<float>::quiet_NaN();
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
virtual void filter()
Filter the incoming data.
std::vector< Buffer * > out
Vector of output arrays.
A class for handling time.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
LaserMapFilterDataFilter(const std::string &filter_name, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in, fawkes::tf::Transformer *tf_listener, fawkes::Configuration *config, fawkes::Logger *logger)
Constructor.
unsigned int out_data_size
Number of entries in output arrays.
Interface for configuration handling.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
std::vector< Buffer * > in
Vector of input arrays.