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 */
70map_t *
71LaserMapFilterDataFilter::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 */
105bool
106LaserMapFilterDataFilter::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
114void
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