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 
33 using 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  */
74 bool
75 LaserBoxFilterDataFilter::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 
104 LaserBoxFilterDataFilter::Vector
105 LaserBoxFilterDataFilter::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 
114 inline double
115 LaserBoxFilterDataFilter::dot(LaserBoxFilterDataFilter::Vector u,
116  LaserBoxFilterDataFilter::Vector v)
117 {
118  return u.x * v.x + u.y * v.y;
119 }
120 
121 void
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 }
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
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
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
Base class for fawkes tf exceptions.
Definition: exceptions.h:30
Fawkes library namespace.
A class for handling time.
Definition: time.h:92
void set_num_boxes(const uint32_t new_num_boxes)
Set num_boxes value.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
LaserBoxFilterInterface Fawkes BlackBoard Interface.
virtual void filter()
Filter the incoming data.
Definition: box_filter.cpp:122
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1164
uint32_t num_boxes() const
Get num_boxes value.
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
CreateNewBoxFilterMessage Fawkes BlackBoard Interface Message.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
The BlackBoard abstract class.
Definition: blackboard.h:45
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 Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
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