Fawkes API  Fawkes Development Version
map_filter.cpp
1 
2 /***************************************************************************
3  * map_filter.cpp - Laser map data filter
4  *
5  * Created: Fri Jul 17 20:38:14 2015
6  * Copyright 2006-2015 Tim Niemueller [www.niemueller.de]
7  * 2015 Tobias Neumann
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "map_filter.h"
25 
26 #include <config/config.h>
27 #include <core/exception.h>
28 #include <utils/math/coord.h>
29 #include <utils/time/time.h>
30 
31 #include <cmath>
32 #include <limits>
33 #include <string>
34 
35 /** @class LaserMapFilterDataFilter "map_filter.h
36  * Removes static laser data (laser beams near occupied map cells)
37  * @author Tobias Neumann
38  */
39 
40 /** Constructor.
41  * @param filter_name name of this filter
42  * @param in_data_size number of entries input value arrays
43  * @param in vector of input arrays
44  * @param tf_listener to access the tf::Transformer aspect
45  * @param config to access the Configuration aspect
46  * @param prefix configuration prefix specifying the config path
47  * @param logger to access the Logger aspect
48  */
50  unsigned int in_data_size,
51  std::vector<LaserDataFilter::Buffer *> &in,
52  fawkes::tf::Transformer *tf_listener,
53  fawkes::Configuration * config,
54  const std::string & prefix,
55  fawkes::Logger * logger)
56 : LaserDataFilter(filter_name, in_data_size, in, 1)
57 {
58  tf_listener_ = tf_listener;
59  config_ = config;
60  logger_ = logger;
61  map_ = load_map();
62  frame_map_ = config_->get_string("/frames/fixed");
63  num_pixels_ = config_->get_int_or_default((prefix + "num_pixels").c_str(), 2);
64  cfg_occupied_thresh_ = std::numeric_limits<float>::max();
65 }
66 
67 /** loads map using amcl
68  * @return the loaded map
69  */
70 map_t *
71 LaserMapFilterDataFilter::load_map()
72 {
73  std::vector<std::pair<int, int>> free_space_indices;
74  std::string cfg_map_file;
75  float cfg_resolution;
76  float cfg_origin_x;
77  float cfg_origin_y;
78  float cfg_origin_theta;
79  float cfg_free_thresh;
80 
81  fawkes::amcl::read_map_config(config_,
82  cfg_map_file,
83  cfg_resolution,
84  cfg_origin_x,
85  cfg_origin_y,
86  cfg_origin_theta,
87  cfg_occupied_thresh_,
88  cfg_free_thresh);
89 
90  return fawkes::amcl::read_map(cfg_map_file.c_str(),
91  cfg_origin_x,
92  cfg_origin_y,
93  cfg_resolution,
94  cfg_occupied_thresh_,
95  cfg_free_thresh,
96  free_space_indices);
97 }
98 
99 /** Returnes whenever a given cell is within the map or not
100  * @param cell_x the x position of the cell
101  * @param cell_y the y position of the cell
102  * @return true if the cell is within the map
103  * false otherwise
104  */
105 bool
106 LaserMapFilterDataFilter::is_in_map(int cell_x, int cell_y)
107 {
108  if (cell_x < 0 || cell_x > map_->size_x || cell_y < 0 || cell_y > map_->size_y) {
109  return false;
110  }
111  return true;
112 }
113 
114 void
116 {
117  const unsigned int vecsize = in.size();
118  if (vecsize == 0)
119  return;
120 
121  for (unsigned int a = 0; a < vecsize; ++a) {
122  // get tf to map of laser input
124  try {
125  tf_listener_->lookup_transform(frame_map_.c_str(),
126  in[a]->frame,
127  *(in[a]->timestamp),
128  transform);
129  } catch (fawkes::tf::TransformException &e) {
130  try {
131  tf_listener_->lookup_transform(frame_map_.c_str(),
132  in[a]->frame,
133  fawkes::Time(0, 0),
134  transform);
135  //logger_->log_debug("map_filter", "Can't transform laser-data using newest tf\n(%s\t%s\t\%lf)",
136  // frame_map_.c_str(), in[a]->frame.c_str(), in[a]->timestamp->in_sec());
137  } catch (fawkes::tf::TransformException &e) {
138  logger_->log_warn("map_filter",
139  "Can't transform laser-data (%s -> %s)",
140  frame_map_.c_str(),
141  in[a]->frame.c_str());
142  return;
143  }
144  }
145  // set out meta info
146  out[a]->frame = in[a]->frame;
147  out[a]->timestamp = in[a]->timestamp;
148  // for each point
149  for (unsigned int i = 0; i < out_data_size; ++i) {
150  bool add = true;
151  // check nan
152  if (std::isfinite(in[a]->values[i])) {
153  // transform to cartesian
154  double angle = M_PI * (360.f / out_data_size * i) / 180;
155 
156  float x, y;
157  fawkes::polar2cart2d(angle, in[a]->values[i], &x, &y);
158 
159  // transform into map
160  fawkes::tf::Point p;
161  p.setValue(x, y, 0.);
162  p = transform * p;
163 
164  // transform to map cells
165  int cell_x = (int)MAP_GXWX(map_, p.getX());
166  int cell_y = (int)MAP_GYWY(map_, p.getY());
167 
168  // search in for a neighborhood in num_pixels_ * num_pixels_ - 1
169  // and itself for occupied pixels in map
170  for (int ox = -num_pixels_; add && ox <= num_pixels_; ++ox) {
171  for (int oy = -num_pixels_; oy <= num_pixels_; ++oy) {
172  int x = cell_x + ox;
173  int y = cell_y + oy;
174  if (MAP_VALID(map_, x, y)) {
175  if (map_->cells[MAP_INDEX(map_, x, y)].occ_state > 0) {
176  add = false;
177  break;
178  }
179  }
180  }
181  }
182  }
183  if (add) {
184  out[a]->values[i] = in[a]->values[i];
185  } else {
186  out[a]->values[i] = std::numeric_limits<float>::quiet_NaN();
187  }
188  }
189  }
190 }
Laser data filter.
Definition: filter.h:33
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:89
virtual void filter()
Filter the incoming data.
Definition: map_filter.cpp:115
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.
Definition: map_filter.cpp:49
Interface for configuration handling.
Definition: config.h:68
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.
Definition: config.cpp:716
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
A class for handling time.
Definition: time.h:93
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Base class for fawkes tf exceptions.
Definition: exceptions.h:31
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
Definition: coord.h:72