24 #include "map_filter.h" 26 #include <core/exception.h> 27 #include <utils/time/time.h> 28 #include <utils/math/coord.h> 47 unsigned int in_data_size,
48 std::vector<LaserDataFilter::Buffer *> &in,
54 tf_listener_ = tf_listener;
58 frame_map_ = config_->
get_string(
"/frames/fixed");
59 cfg_occupied_thresh_ = std::numeric_limits<float>::max();
66 LaserMapFilterDataFilter::load_map()
68 std::vector<std::pair<int, int>> free_space_indices;
69 std::string cfg_map_file;
73 float cfg_origin_theta;
74 float cfg_free_thresh;
76 fawkes::amcl::read_map_config(config_, cfg_map_file, cfg_resolution, cfg_origin_x,
77 cfg_origin_y, cfg_origin_theta, cfg_occupied_thresh_,
80 return fawkes::amcl::read_map(cfg_map_file.c_str(),
81 cfg_origin_x, cfg_origin_y, cfg_resolution,
82 cfg_occupied_thresh_, cfg_free_thresh, free_space_indices);
92 LaserMapFilterDataFilter::is_in_map(
int cell_x,
int cell_y)
94 if (cell_x < 0 || cell_x > map_->size_x ||
95 cell_y < 0 || cell_y > map_->size_y) {
104 const unsigned int vecsize =
in.size();
105 if (vecsize == 0)
return;
107 for (
unsigned int a = 0; a < vecsize; ++a) {
111 tf_listener_->
lookup_transform(frame_map_.c_str(),
in[a]->frame, *(
in[a]->timestamp), transform);
115 logger_->
log_debug(
"map_filter",
"Can't transform laser-data using newest tf\n(%s\t%s\t\%lf)",
116 frame_map_.c_str(),
in[a]->frame.c_str(),
in[a]->timestamp->in_sec());
118 logger_->
log_debug(
"map_filter",
"Can't transform laser-data at all (%s -> %s)",
119 frame_map_.c_str(),
in[a]->frame.c_str());
124 out[a]->frame =
in[a]->frame;
125 out[a]->timestamp =
in[a]->timestamp;
130 if ( std::isfinite(
in[a]->values[i]) ) {
132 double angle = M_PI * (360.f / out_data_size * i ) / 180;
139 p.setValue(x, y, 0.);
143 int cell_x = (int)MAP_GXWX(map_, p.getX());
144 int cell_y = (int)MAP_GYWY(map_, p.getY());
147 for (
int ox = -2; add && ox <= 2; ++ox) {
148 for (
int oy = -2; oy <= 2; ++oy) {
151 if (MAP_VALID(map_, x, y)) {
152 if (map_->cells[MAP_INDEX(map_, x, y)].occ_state > 0) {
161 out[a]->values[i] =
in[a]->values[i];
163 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_debug(const char *component, const char *format,...)=0
Log debug 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.