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();
71LaserMapFilterDataFilter::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(),
106LaserMapFilterDataFilter::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();
unsigned int out_data_size
Number of entries in output arrays.
std::vector< Buffer * > out
Vector of output arrays.
std::vector< Buffer * > in
Vector of input arrays.
virtual void filter()
Filter the incoming data.
LaserMapFilterDataFilter(const std::string &filter_name, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in, fawkes::tf::Transformer *tf_listener, fawkes::Configuration *config, const std::string &prefix, fawkes::Logger *logger)
Constructor.
Interface for configuration handling.
virtual int get_int_or_default(const char *path, const int &default_val)
Get value from configuration which is of type int, or the given default if the path does not exist.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
A class for handling time.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.