Fawkes API Fawkes Development Version
box_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/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "box_filter.h"
24
25#include <core/exception.h>
26#include <utils/math/coord.h>
27#include <utils/time/time.h>
28
29#include <cmath>
30#include <limits>
31#include <string>
32
33using namespace fawkes;
34
35/** @class LaserBoxFilterDataFilter "box_filter.h
36 * Removes laser data which is represented by a set of boxes.
37 * These boxes can be defined by an interface.
38 * @author Nicolas Limpert
39 */
40
41/** Constructor.
42 * @param filter_name name of this filter
43 * @param in_data_size number of entries input value arrays
44 * @param in vector of input arrays
45 * @param tf_listener to access the tf::Transformer aspect
46 * @param config to access the Configuration aspect
47 * @param logger to access the Logger aspect
48 * @param blackboard to open the LaserBoxFilterInterface for writing
49 */
51 unsigned int in_data_size,
52 std::vector<LaserDataFilter::Buffer *> &in,
53 fawkes::tf::Transformer *tf_listener,
54 fawkes::Configuration * config,
55 fawkes::Logger * logger,
56 BlackBoard * blackboard)
57: LaserDataFilter(filter_name, in_data_size, in, 1)
58{
59 tf_listener_ = tf_listener;
60 config_ = config;
61 logger_ = logger;
62 frame_map_ = config_->get_string("/frames/fixed");
63 cfg_occupied_thresh_ = std::numeric_limits<float>::max();
64 box_filter_if_ =
65 blackboard->open_for_writing<fawkes::LaserBoxFilterInterface>("Laser Box Filter");
66}
67
68/** Returns whether the given coordinates are within one of the boxes or not.
69 * @param x the x position
70 * @param y the y position
71 * @return true if the point is within a box
72 * false otherwise
73 */
74bool
75LaserBoxFilterDataFilter::point_in_rectangle(float x, float y)
76{
77 // Test if given point x, y is inside rectangle is given by:
78 //
79 // 0 <= dot(AB,AM) <= dot(AB,AB) && 0 <= dot(BC,BM) <= dot(BC,BC)
80 //
81 // This approach is based on https://codepen.io/mattburns/pen/jrrprN
82
83 Vector point;
84 point.x = x;
85 point.y = y;
86
87 bool is_in_rect = false;
88
89 std::vector<Box>::iterator it;
90 for (it = boxes_.begin(); is_in_rect == false && it != boxes_.end(); ++it) {
91 Vector AB = d_vec(it->a, it->b);
92 Vector AM = d_vec(it->a, point);
93 Vector BC = d_vec(it->b, it->c);
94 Vector BM = d_vec(it->b, point);
95 double dotABAM = dot(AB, AM);
96 double dotABAB = dot(AB, AB);
97 double dotBCBM = dot(BC, BM);
98 double dotBCBC = dot(BC, BC);
99 is_in_rect = 0 <= dotABAM && dotABAM <= dotABAB && 0 <= dotBCBM && dotBCBM <= dotBCBC;
100 }
101 return is_in_rect;
102}
103
104LaserBoxFilterDataFilter::Vector
105LaserBoxFilterDataFilter::d_vec(LaserBoxFilterDataFilter::Vector p1,
106 LaserBoxFilterDataFilter::Vector p2)
107{
108 LaserBoxFilterDataFilter::Vector ret_val;
109 ret_val.x = (p2.x - p1.x);
110 ret_val.y = (p2.y - p1.y);
111 return ret_val;
112}
113
114inline double
115LaserBoxFilterDataFilter::dot(LaserBoxFilterDataFilter::Vector u,
116 LaserBoxFilterDataFilter::Vector v)
117{
118 return u.x * v.x + u.y * v.y;
119}
120
121void
123{
124 while (!box_filter_if_->msgq_empty()) {
127
128 Box new_box;
129 new_box.a.x = msg->p1(0);
130 new_box.a.y = msg->p1(1);
131 new_box.b.x = msg->p2(0);
132 new_box.b.y = msg->p2(1);
133 new_box.c.x = msg->p3(0);
134 new_box.c.y = msg->p3(1);
135 new_box.d.x = msg->p4(0);
136 new_box.d.y = msg->p4(1);
137
138 boxes_.push_back(new_box);
139 box_filter_if_->set_num_boxes(box_filter_if_->num_boxes() + 1);
140 box_filter_if_->write();
141 }
142
143 box_filter_if_->msgq_pop();
144 }
145
146 const unsigned int vecsize = in.size();
147 if (vecsize == 0)
148 return;
149
150 for (unsigned int a = 0; a < vecsize; ++a) {
151 // get tf to map of laser input
153 try {
154 tf_listener_->lookup_transform(frame_map_.c_str(),
155 in[a]->frame,
156 *(in[a]->timestamp),
157 transform);
158 } catch (fawkes::tf::TransformException &e) {
159 try {
160 tf_listener_->lookup_transform(frame_map_.c_str(),
161 in[a]->frame,
162 fawkes::Time(0, 0),
163 transform);
164 } catch (fawkes::tf::TransformException &e) {
165 logger_->log_warn("box_filter",
166 "Can't transform laser-data (%s -> %s)",
167 frame_map_.c_str(),
168 in[a]->frame.c_str());
169 return;
170 }
171 }
172 // set out meta info
173 out[a]->frame = in[a]->frame;
174 out[a]->timestamp = in[a]->timestamp;
175 // for each point
176 for (unsigned int i = 0; i < out_data_size; ++i) {
177 bool add = true;
178 // check nan
179 if (std::isfinite(in[a]->values[i])) {
180 // transform to cartesian
181 double angle = M_PI * (360.f / out_data_size * i) / 180;
182
183 float x, y;
184 fawkes::polar2cart2d(angle, in[a]->values[i], &x, &y);
185
186 // transform into map
187 fawkes::tf::Point p;
188 p.setValue(x, y, 0.);
189 p = transform * p;
190
191 add = !point_in_rectangle(p.getX(), p.getY());
192 }
193 if (add) {
194 out[a]->values[i] = in[a]->values[i];
195 } else {
196 out[a]->values[i] = std::numeric_limits<float>::quiet_NaN();
197 }
198 }
199 }
200}
LaserBoxFilterDataFilter(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, fawkes::BlackBoard *blackboard)
Constructor.
Definition: box_filter.cpp:50
virtual void filter()
Filter the incoming data.
Definition: box_filter.cpp:122
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
The BlackBoard abstract class.
Definition: blackboard.h:46
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
Interface for configuration handling.
Definition: config.h:68
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
CreateNewBoxFilterMessage Fawkes BlackBoard Interface Message.
LaserBoxFilterInterface Fawkes BlackBoard Interface.
uint32_t num_boxes() const
Get num_boxes value.
void set_num_boxes(const uint32_t new_num_boxes)
Set num_boxes value.
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
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.
Fawkes library namespace.
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