22 #include "filter_thread.h"
24 #include "filters/1080to360.h"
25 #include "filters/720to360.h"
26 #include "filters/cascade.h"
27 #include "filters/circle_sector.h"
28 #include "filters/copy.h"
29 #include "filters/deadspots.h"
30 #include "filters/max_circle.h"
31 #include "filters/min_circle.h"
32 #include "filters/min_merge.h"
33 #include "filters/reverse_angle.h"
35 # include "filters/box_filter.h"
36 # include "filters/map_filter.h"
37 # include "filters/projection.h"
40 #include <core/threading/barrier.h>
41 #include <core/threading/mutex.h>
42 #include <core/threading/wait_condition.h>
43 #include <interfaces/Laser1080Interface.h>
44 #include <interfaces/Laser360Interface.h>
45 #include <interfaces/Laser720Interface.h>
46 #include <utils/time/time.h>
67 :
Thread(
"LaserFilterThread",
Thread::OPMODE_WAITFORWAKEUP),
70 set_name(
"LaserFilterThread(%s)", cfg_name.c_str());
72 cfg_prefix_ = cfg_prefix;
80 open_interfaces(cfg_prefix_ +
"in/", in_, in_bufs_,
false);
81 open_interfaces(cfg_prefix_ +
"out/", out_, out_bufs_,
true);
84 throw Exception(
"No input interfaces defined for %s", cfg_name_.c_str());
87 throw Exception(
"No output interfaces defined for %s", cfg_name_.c_str());
90 std::map<std::string, std::string> filters;
92 std::string fpfx = cfg_prefix_ +
"filters/";
93 #if __cplusplus >= 201103L
94 std::unique_ptr<Configuration::ValueIterator> i(
config->
search(fpfx.c_str()));
96 std::auto_ptr<Configuration::ValueIterator> i(
config->
search(fpfx.c_str()));
100 std::string suffix = std::string(i->path()).substr(fpfx.length());
101 std::string filter_name = std::string(suffix.substr(0, suffix.find(
"/")));
102 std::string conf_key = std::string(suffix.substr(suffix.find(
"/") + 1, suffix.length()));
104 if (conf_key !=
"type")
107 if (!i->is_string()) {
108 throw Exception(
"Filter value %s is not a string", i->path());
111 filters[filter_name] = i->get_string();
113 if (filters.empty()) {
114 throw Exception(
"No filters defined for %s", cfg_name_.c_str());
117 if (filters.size() == 1) {
118 std::string filter_name = filters.begin()->first;
120 "Adding filter %s (%s)",
122 filters[filter_name].c_str());
123 filter_ = create_filter(cfg_name_ +
"/" + filter_name,
124 filters[filter_name],
125 fpfx + filter_name +
"/",
133 std::map<std::string, std::string>::iterator f;
134 for (f = filters.begin(); f != filters.end(); ++f) {
136 "Adding filter %s (%s) %zu %zu",
141 cascade->
add_filter(create_filter(cfg_name_ +
"/" + f->first,
143 fpfx + f->first +
"/",
156 Exception e(
"Output interface and filter data size for %s do not match (%u != %u)",
167 for (
unsigned int i = 0; i < in_.size(); ++i) {
170 for (
unsigned int i = 0; i < out_.size(); ++i) {
176 std::list<LaserFilterThread *>::iterator wt;
177 for (wt = wait_threads_.begin(); wt != wait_threads_.end(); ++wt) {
182 wait_mutex_ =
new Mutex();
193 for (
unsigned int i = 0; i < in_.size(); ++i) {
197 for (
unsigned int i = 0; i < out_.size(); ++i) {
208 std::list<LaserFilterThread *>::iterator wt;
209 for (wt = wait_threads_.begin(); wt != wait_threads_.end(); ++wt) {
215 const size_t in_num = in_.size();
216 for (
size_t i = 0; i != in_num; ++i) {
217 in_[i].interface->read();
218 if (in_[i].size == 360) {
219 in_bufs_[i]->frame = in_[i].interface_typed.as360->frame();
220 *in_bufs_[i]->timestamp = in_[i].interface_typed.as360->timestamp();
221 }
else if (in_[i].size == 720) {
222 in_bufs_[i]->frame = in_[i].interface_typed.as720->frame();
223 *in_bufs_[i]->timestamp = in_[i].interface_typed.as720->timestamp();
224 }
else if (in_[i].size == 1080) {
225 in_bufs_[i]->frame = in_[i].interface_typed.as1080->frame();
226 *in_bufs_[i]->timestamp = in_[i].interface_typed.as1080->timestamp();
239 const size_t num = out_.size();
240 for (
size_t i = 0; i < num; ++i) {
241 if (out_[i].size == 360) {
242 out_[i].interface_typed.as360->set_timestamp(out_bufs_[i]->timestamp);
243 out_[i].interface_typed.as360->set_frame(out_bufs_[i]->frame.c_str());
244 }
else if (out_[i].size == 720) {
245 out_[i].interface_typed.as720->set_timestamp(out_bufs_[i]->timestamp);
246 out_[i].interface_typed.as720->set_frame(out_bufs_[i]->frame.c_str());
247 }
else if (out_[i].size == 1080) {
248 out_[i].interface_typed.as1080->set_timestamp(out_bufs_[i]->timestamp);
249 out_[i].interface_typed.as1080->set_frame(out_bufs_[i]->frame.c_str());
251 out_[i].interface->write();
259 wait_barrier_->
wait();
282 LaserFilterThread::open_interfaces(std::string prefix,
283 std::vector<LaserInterface> & ifs,
284 std::vector<LaserDataFilter::Buffer *> &bufs,
287 #if __cplusplus >= 201103L
288 std::unique_ptr<Configuration::ValueIterator> in(
config->
search(prefix.c_str()));
290 std::auto_ptr<Configuration::ValueIterator> in(
config->
search(prefix.c_str()));
293 if (!in->is_string()) {
294 throw Exception(
"Config value %s is not of type string", in->path());
296 std::string uid = in->get_string();
299 if ((sf = uid.find(
"::")) == std::string::npos) {
300 throw Exception(
"Interface '%s' is not a UID", uid.c_str());
302 std::string type = uid.substr(0, sf);
303 std::string
id = uid.substr(sf + 2);
306 lif.interface = NULL;
308 if (type ==
"Laser360Interface") {
310 }
else if (type ==
"Laser720Interface") {
312 }
else if (type ==
"Laser1080Interface") {
315 throw Exception(
"Interfaces must be of type Laser360Interface, "
316 "Laser720Interface, or Laser1080Interface, "
327 throw Exception(
"No interfaces defined at %s", prefix.c_str());
330 bufs.resize(ifs.size());
332 unsigned int req_size = ifs[0].size;
336 for (
unsigned int i = 0; i < ifs.size(); ++i) {
337 if (req_size != ifs[i].size) {
338 throw Exception(
"Interfaces of mixed sizes for %s", cfg_name_.c_str());
341 if (ifs[i].size == 360) {
348 ifs[i].interface_typed.as360 = laser360;
349 ifs[i].interface = laser360;
351 bufs[i]->name = laser360->
uid();
354 }
else if (ifs[i].size == 720) {
361 ifs[i].interface_typed.as720 = laser720;
362 ifs[i].interface = laser720;
364 bufs[i]->name = laser720->
uid();
367 }
else if (ifs[i].size == 1080) {
374 ifs[i].interface_typed.as1080 = laser1080;
375 ifs[i].interface = laser1080;
377 bufs[i]->name = laser1080->
uid();
378 bufs[i]->values = laser1080->
distances();
382 for (
unsigned int i = 0; i < ifs.size(); ++i) {
383 if (ifs[i].size == 360) {
388 ifs[i].interface_typed.as360 = laser360;
389 ifs[i].interface = laser360;
391 bufs[i]->name = laser360->
uid();
392 bufs[i]->frame = laser360->
frame();
395 }
else if (ifs[i].size == 720) {
400 ifs[i].interface_typed.as720 = laser720;
401 ifs[i].interface = laser720;
403 bufs[i]->name = laser720->
uid();
404 bufs[i]->frame = laser720->
frame();
407 }
else if (ifs[i].size == 1080) {
412 ifs[i].interface_typed.as1080 = laser1080;
413 ifs[i].interface = laser1080;
415 bufs[i]->name = laser1080->
uid();
416 bufs[i]->frame = laser1080->
frame();
417 bufs[i]->values = laser1080->
distances();
422 for (
unsigned int i = 0; i < ifs.size(); ++i) {
432 LaserFilterThread::create_filter(std::string filter_name,
433 std::string filter_type,
435 unsigned int in_data_size,
436 std::vector<LaserDataFilter::Buffer *> &inbufs)
438 if (filter_type ==
"copy") {
440 }
else if (filter_type ==
"720to360") {
441 bool average =
false;
447 }
else if (filter_type ==
"1080to360") {
448 bool average =
false;
454 }
else if (filter_type ==
"reverse") {
456 }
else if (filter_type ==
"max_circle") {
459 }
else if (filter_type ==
"min_circle") {
462 }
else if (filter_type ==
"circle_sector") {
466 }
else if (filter_type ==
"deadspots") {
468 }
else if (filter_type ==
"min_merge") {
469 std::string timestamp_selection;
471 timestamp_selection =
config->
get_string((prefix +
"timestamp_selection").c_str());
475 if (timestamp_selection ==
"latest") {
478 }
else if (timestamp_selection ==
"first") {
481 }
else if (timestamp_selection ==
"index") {
482 unsigned int timestamp_if_index =
config->
get_uint((prefix +
"timestamp_index").c_str());
489 }
else if (timestamp_selection !=
"") {
490 throw Exception(
"Laser filter: unknown timestamp selection method '%s'",
491 timestamp_selection.c_str());
496 }
else if (filter_type ==
"projection") {
498 const float not_from_x =
config->
get_float((prefix +
"not_from_x").c_str());
499 const float not_to_x =
config->
get_float((prefix +
"not_to_x").c_str());
500 const float not_from_y =
config->
get_float((prefix +
"not_from_y").c_str());
501 const float not_to_y =
config->
get_float((prefix +
"not_to_y").c_str());
502 const float only_from_z =
config->
get_float((prefix +
"only_from_z").c_str());
503 const float only_to_z =
config->
get_float((prefix +
"only_to_z").c_str());
504 const std::string frame =
config->
get_string((prefix +
"target_frame").c_str());
517 throw Exception(
"Projection filter unavailable, tf missing");
519 }
else if (filter_type ==
"map_filter") {
522 filter_name, in_data_size, inbufs, tf_listener,
config, prefix,
logger);
524 throw Exception(
"Projection filter unavailable, tf missing");
526 }
else if (filter_type ==
"box_filter") {
531 throw Exception(
"Projection filter unavailable, tf missing");
534 throw Exception(
"Unknown filter type %s", filter_type.c_str());
547 wait_threads_ = threads;
559 wait_barrier_ = barrier;
Downsample filter from 1080 to 360 values.
Downsample filter from 720 to 360 values.
Removes laser data which is represented by a set of boxes.
Erase beams outside specified circle sector.
Copy laser data without modification to a new name.
Cascade of several laser filters to one.
void add_filter(LaserDataFilter *filter)
Add a filter to the cascade.
virtual std::vector< Buffer * > & get_out_vector()
Get filtered data array.
virtual unsigned int get_out_data_size()
Get size of filtered data array.
virtual void filter()=0
Filter the incoming data.
virtual void set_out_vector(std::vector< Buffer * > &out)
Set filtered data array.
virtual void init()
Initialize the thread.
void set_wait_barrier(fawkes::Barrier *barrier)
Set wait barrier.
void set_wait_threads(std::list< LaserFilterThread * > &threads)
Set threads to wait for in loop.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
void wait_done()
Wait until thread is done.
LaserFilterThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Removes static laser data (laser beams near occupied map cells)
Cut of laser data at max distance.
Erase beams below a certain minimum distance distance.
Merge multiple laser data arrays into one.
@ TIMESTAMP_LATEST
use the latest of all timestamps
@ TIMESTAMP_FIRST
use the first (oldest) of all timestamps
@ TIMESTAMP_INDEX
use a specific index in the input buffer list
Projects one laser into another laser's plane.
Reverse the angle of beams.
A barrier is a synchronization tool which blocks until a given number of threads have reached the bar...
virtual void wait()
Wait for other threads.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
const char * id() const
Get identifier of interface.
const char * uid() const
Get unique identifier of interface.
Laser1080Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
char * frame() const
Get frame value.
Laser720Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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.
Mutex mutual exclusion lock.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
void set_name(const char *format,...)
Set name of thread.
Wait until a given condition holds.
void wait()
Wait for the condition forever.
void wake_all()
Wake up all waiting threads.
Fawkes library namespace.