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 <core/exception.h>
27 #include <utils/math/coord.h>
28 #include <utils/time/time.h>
29 
30 #include <cmath>
31 #include <limits>
32 #include <string>
33 
34 /** @class LaserMapFilterDataFilter "map_filter.h
35  * Removes static laser data (laser beams near occupied map cells)
36  * @author Tobias Neumann
37  */
38 
39 /** Constructor.
40  * @param filter_name name of this filter
41  * @param in_data_size number of entries input value arrays
42  * @param in vector of input arrays
43  * @param tf_listener to access the tf::Transformer aspect
44  * @param config to access the Configuration aspect
45  * @param logger to access the Logger aspect
46  */
48  unsigned int in_data_size,
49  std::vector<LaserDataFilter::Buffer *> &in,
50  fawkes::tf::Transformer *tf_listener,
51  fawkes::Configuration * config,
52  fawkes::Logger * logger)
53 : LaserDataFilter(filter_name, in_data_size, in, 1)
54 {
55  tf_listener_ = tf_listener;
56  config_ = config;
57  logger_ = logger;
58  map_ = load_map();
59  frame_map_ = config_->get_string("/frames/fixed");
60  cfg_occupied_thresh_ = std::numeric_limits<float>::max();
61 }
62 
63 /** loads map using amcl
64  * @return the loaded map
65  */
66 map_t *
67 LaserMapFilterDataFilter::load_map()
68 {
69  std::vector<std::pair<int, int>> free_space_indices;
70  std::string cfg_map_file;
71  float cfg_resolution;
72  float cfg_origin_x;
73  float cfg_origin_y;
74  float cfg_origin_theta;
75  float cfg_free_thresh;
76 
77  fawkes::amcl::read_map_config(config_,
78  cfg_map_file,
79  cfg_resolution,
80  cfg_origin_x,
81  cfg_origin_y,
82  cfg_origin_theta,
83  cfg_occupied_thresh_,
84  cfg_free_thresh);
85 
86  return fawkes::amcl::read_map(cfg_map_file.c_str(),
87  cfg_origin_x,
88  cfg_origin_y,
89  cfg_resolution,
90  cfg_occupied_thresh_,
91  cfg_free_thresh,
92  free_space_indices);
93 }
94 
95 /** Returnes whenever a given cell is within the map or not
96  * @param cell_x the x position of the cell
97  * @param cell_y the y position of the cell
98  * @return true if the cell is within the map
99  * false otherwise
100  */
101 bool
102 LaserMapFilterDataFilter::is_in_map(int cell_x, int cell_y)
103 {
104  if (cell_x < 0 || cell_x > map_->size_x || cell_y < 0 || cell_y > map_->size_y) {
105  return false;
106  }
107  return true;
108 }
109 
110 void
112 {
113  const unsigned int vecsize = in.size();
114  if (vecsize == 0)
115  return;
116 
117  for (unsigned int a = 0; a < vecsize; ++a) {
118  // get tf to map of laser input
120  try {
121  tf_listener_->lookup_transform(frame_map_.c_str(),
122  in[a]->frame,
123  *(in[a]->timestamp),
124  transform);
125  } catch (fawkes::tf::TransformException &e) {
126  try {
127  tf_listener_->lookup_transform(frame_map_.c_str(),
128  in[a]->frame,
129  fawkes::Time(0, 0),
130  transform);
131  //logger_->log_debug("map_filter", "Can't transform laser-data using newest tf\n(%s\t%s\t\%lf)",
132  // frame_map_.c_str(), in[a]->frame.c_str(), in[a]->timestamp->in_sec());
133  } catch (fawkes::tf::TransformException &e) {
134  logger_->log_warn("map_filter",
135  "Can't transform laser-data (%s -> %s)",
136  frame_map_.c_str(),
137  in[a]->frame.c_str());
138  return;
139  }
140  }
141  // set out meta info
142  out[a]->frame = in[a]->frame;
143  out[a]->timestamp = in[a]->timestamp;
144  // for each point
145  for (unsigned int i = 0; i < out_data_size; ++i) {
146  bool add = true;
147  // check nan
148  if (std::isfinite(in[a]->values[i])) {
149  // transform to cartesian
150  double angle = M_PI * (360.f / out_data_size * i) / 180;
151 
152  float x, y;
153  fawkes::polar2cart2d(angle, in[a]->values[i], &x, &y);
154 
155  // transform into map
156  fawkes::tf::Point p;
157  p.setValue(x, y, 0.);
158  p = transform * p;
159 
160  // transform to map cells
161  int cell_x = (int)MAP_GXWX(map_, p.getX());
162  int cell_y = (int)MAP_GYWY(map_, p.getY());
163 
164  // search in 8-neighborhood and itself for occupied pixels in map
165  for (int ox = -2; add && ox <= 2; ++ox) {
166  for (int oy = -2; oy <= 2; ++oy) {
167  int x = cell_x + ox;
168  int y = cell_y + oy;
169  if (MAP_VALID(map_, x, y)) {
170  if (map_->cells[MAP_INDEX(map_, x, y)].occ_state > 0) {
171  add = false;
172  break;
173  }
174  }
175  }
176  }
177  }
178  if (add) {
179  out[a]->values[i] = in[a]->values[i];
180  } else {
181  out[a]->values[i] = std::numeric_limits<float>::quiet_NaN();
182  }
183  }
184  }
185 }
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
virtual void filter()
Filter the incoming data.
Definition: map_filter.cpp:111
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
Base class for fawkes tf exceptions.
Definition: exceptions.h:30
A class for handling time.
Definition: time.h:92
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
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.
Definition: map_filter.cpp:47
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
Coordinate transforms between any two frames in a system.
Definition: transformer.h:64
Interface for configuration handling.
Definition: config.h:64
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.
Definition: filter.h:89
Laser data filter.
Definition: filter.h:32
Interface for logging.
Definition: logger.h:41