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