22 #include "filter_thread.h" 23 #include "filters/max_circle.h" 24 #include "filters/720to360.h" 25 #include "filters/1080to360.h" 26 #include "filters/deadspots.h" 27 #include "filters/cascade.h" 28 #include "filters/reverse_angle.h" 29 #include "filters/min_circle.h" 30 #include "filters/circle_sector.h" 31 #include "filters/min_merge.h" 33 # include "filters/projection.h" 34 #include "filters/map_filter.h" 37 #include <core/threading/barrier.h> 38 #include <core/threading/mutex.h> 39 #include <core/threading/wait_condition.h> 41 #include <utils/time/time.h> 43 #include <interfaces/Laser360Interface.h> 44 #include <interfaces/Laser720Interface.h> 45 #include <interfaces/Laser1080Interface.h> 67 std::string &cfg_prefix)
68 :
Thread(
"LaserFilterThread",
Thread::OPMODE_WAITFORWAKEUP),
71 set_name(
"LaserFilterThread(%s)", cfg_name.c_str());
72 __cfg_name = cfg_name;
73 __cfg_prefix = cfg_prefix;
74 __wait_barrier = NULL;
82 open_interfaces(__cfg_prefix +
"in/", __in, __in_bufs,
false);
83 open_interfaces(__cfg_prefix +
"out/", __out, __out_bufs,
true);
86 throw Exception(
"No input interfaces defined for %s", __cfg_name.c_str());
89 throw Exception(
"No output interfaces defined for %s", __cfg_name.c_str());
93 std::map<std::string, std::string> filters;
95 std::string fpfx = __cfg_prefix +
"filters/";
96 #if __cplusplus >= 201103L 97 std::unique_ptr<Configuration::ValueIterator> i(
config->
search(fpfx.c_str()));
99 std::auto_ptr<Configuration::ValueIterator> i(
config->
search(fpfx.c_str()));
104 std::string suffix = std::string(i->
path()).substr(fpfx.length());
105 std::string filter_name = std::string(suffix.substr(0,suffix.find(
"/")));
106 std::string conf_key = std::string(suffix.substr(suffix.find(
"/")+1,suffix.length()));
108 if (conf_key !=
"type")
continue;
111 throw Exception(
"Filter value %s is not a string", i->
path());
116 if (filters.empty()) {
117 throw Exception(
"No filters defined for %s", __cfg_name.c_str());
120 if (filters.size() == 1) {
121 std::string filter_name = filters.
begin()->first;
123 filter_name.c_str(), filters[filter_name].c_str());
124 __filter = create_filter(__cfg_name +
"/" + filter_name, filters[filter_name], fpfx + filter_name +
"/",
125 __in[0].size, __in_bufs);
131 std::map<std::string, std::string>::iterator f;
132 for (f = filters.begin(); f != filters.end(); ++f) {
134 f->first.c_str(), f->second.c_str(), __in_bufs.size(),
136 cascade->
add_filter(create_filter(__cfg_name +
"/" + f->first, f->second, fpfx + f->first +
"/",
148 Exception e(
"Output interface and filter data size for %s do not match (%u != %u)",
157 for (
unsigned int i = 0; i < __in.size(); ++i) {
160 for (
unsigned int i = 0; i < __out.size(); ++i) {
166 std::list<LaserFilterThread *>::iterator wt;
167 for (wt = __wait_threads.begin(); wt != __wait_threads.end(); ++wt) {
172 __wait_mutex =
new Mutex();
184 for (
unsigned int i = 0; i < __in.size(); ++i) {
188 for (
unsigned int i = 0; i < __out.size(); ++i) {
198 if (__wait_barrier) {
199 std::list<LaserFilterThread *>::iterator wt;
200 for (wt = __wait_threads.begin(); wt != __wait_threads.end(); ++wt) {
206 const size_t in_num = __in.size();
207 for (
size_t i = 0; i != in_num; ++i) {
208 __in[i].interface->read();
209 if (__in[i].size == 360) {
210 __in_bufs[i]->frame = __in[i].interface_typed.as360->frame();
211 *__in_bufs[i]->timestamp = __in[i].interface_typed.as360->timestamp();
212 }
else if (__in[i].size == 720) {
213 __in_bufs[i]->frame = __in[i].interface_typed.as720->frame();
214 *__in_bufs[i]->timestamp = __in[i].interface_typed.as720->timestamp();
215 }
else if (__in[i].size == 1080) {
216 __in_bufs[i]->frame = __in[i].interface_typed.as1080->frame();
217 *__in_bufs[i]->timestamp = __in[i].interface_typed.as1080->timestamp();
230 const size_t num = __out.size();
231 for (
size_t i = 0; i < num; ++i) {
232 if (__out[i].size == 360) {
233 __out[i].interface_typed.as360->set_timestamp(__out_bufs[i]->timestamp);
234 __out[i].interface_typed.as360->set_frame(__out_bufs[i]->frame.c_str());
235 }
else if (__out[i].size == 720) {
236 __out[i].interface_typed.as720->set_timestamp(__out_bufs[i]->timestamp);
237 __out[i].interface_typed.as720->set_frame(__out_bufs[i]->frame.c_str());
238 }
else if (__out[i].size == 1080) {
239 __out[i].interface_typed.as1080->set_timestamp(__out_bufs[i]->timestamp);
240 __out[i].interface_typed.as1080->set_frame(__out_bufs[i]->frame.c_str());
242 __out[i].interface->write();
245 if (__wait_barrier) {
246 __wait_mutex->
lock();
250 __wait_barrier->
wait();
251 __wait_mutex->
lock();
265 __wait_mutex->
lock();
266 while (__wait_done) {
275 LaserFilterThread::open_interfaces(std::string prefix,
276 std::vector<LaserInterface> &ifs,
277 std::vector<LaserDataFilter::Buffer *> &bufs,
bool writing)
279 #if __cplusplus >= 201103L 280 std::unique_ptr<Configuration::ValueIterator> in(
config->
search(prefix.c_str()));
282 std::auto_ptr<Configuration::ValueIterator> in(
config->
search(prefix.c_str()));
286 throw Exception(
"Config value %s is not of type string", in->
path());
291 if ((sf = uid.find(
"::")) == std::string::npos) {
292 throw Exception(
"Interface '%s' is not a UID", uid.c_str());
294 std::string type = uid.substr(0, sf);
295 std::string
id = uid.substr(sf + 2);
298 lif.interface = NULL;
300 if (type ==
"Laser360Interface") {
302 }
else if (type ==
"Laser720Interface") {
304 }
else if (type ==
"Laser1080Interface") {
307 throw Exception(
"Interfaces must be of type Laser360Interface, " 308 "Laser720Interface, or Laser1080Interface, " 309 "but it is '%s'", type.c_str());
318 throw Exception(
"No interfaces defined at %s", prefix.c_str());
321 bufs.resize(ifs.size());
323 unsigned int req_size = ifs[0].size;
327 for (
unsigned int i = 0; i < ifs.size(); ++i) {
328 if (req_size != ifs[i].size) {
329 throw Exception(
"Interfaces of mixed sizes for %s",
333 if (ifs[i].size == 360) {
340 ifs[i].interface_typed.as360 = laser360;
341 ifs[i].interface = laser360;
343 bufs[i]->name = laser360->
uid();
346 }
else if (ifs[i].size == 720) {
353 ifs[i].interface_typed.as720 = laser720;
354 ifs[i].interface = laser720;
356 bufs[i]->name = laser720->
uid();
359 }
else if (ifs[i].size == 1080) {
367 ifs[i].interface_typed.as1080 = laser1080;
368 ifs[i].interface = laser1080;
370 bufs[i]->name = laser1080->
uid();
371 bufs[i]->values = laser1080->
distances();
375 for (
unsigned int i = 0; i < ifs.size(); ++i) {
376 if (ifs[i].size == 360) {
381 ifs[i].interface_typed.as360 = laser360;
382 ifs[i].interface = laser360;
384 bufs[i]->name = laser360->
uid();
385 bufs[i]->frame = laser360->
frame();
388 }
else if (ifs[i].size == 720) {
393 ifs[i].interface_typed.as720 = laser720;
394 ifs[i].interface = laser720;
396 bufs[i]->name = laser720->
uid();
397 bufs[i]->frame = laser720->
frame();
400 }
else if (ifs[i].size == 1080) {
406 ifs[i].interface_typed.as1080 = laser1080;
407 ifs[i].interface = laser1080;
409 bufs[i]->name = laser1080->
uid();
410 bufs[i]->frame = laser1080->
frame();
411 bufs[i]->values = laser1080->
distances();
416 for (
unsigned int i = 0; i < ifs.size(); ++i) {
427 LaserFilterThread::create_filter(std::string filter_name,
428 std::string filter_type, std::string prefix,
429 unsigned int in_data_size,
430 std::vector<LaserDataFilter::Buffer *> &inbufs)
432 if (filter_type ==
"720to360") {
433 bool average =
false;
438 }
else if (filter_type ==
"1080to360") {
439 bool average =
false;
444 }
else if (filter_type ==
"reverse") {
446 }
else if (filter_type ==
"max_circle") {
449 }
else if (filter_type ==
"min_circle") {
452 }
else if (filter_type ==
"circle_sector") {
456 }
else if (filter_type ==
"deadspots") {
458 }
else if (filter_type ==
"min_merge") {
459 std::string timestamp_selection;
461 timestamp_selection =
config->
get_string((prefix +
"timestamp_selection").c_str());
464 if (timestamp_selection ==
"latest") {
467 }
else if (timestamp_selection ==
"first") {
470 }
else if (timestamp_selection ==
"index") {
471 unsigned int timestamp_if_index =
476 }
else if (timestamp_selection !=
"") {
477 throw Exception(
"Laser filter: unknown timestamp selection method '%s'",
478 timestamp_selection.c_str());
483 }
else if (filter_type ==
"projection") {
485 const float not_from_x =
config->
get_float((prefix +
"not_from_x").c_str());
486 const float not_to_x =
config->
get_float((prefix +
"not_to_x").c_str());
487 const float not_from_y =
config->
get_float((prefix +
"not_from_y").c_str());
488 const float not_to_y =
config->
get_float((prefix +
"not_to_y").c_str());
489 const float only_from_z =
config->
get_float((prefix +
"only_from_z").c_str());
490 const float only_to_z =
config->
get_float((prefix +
"only_to_z").c_str());
491 const std::string frame =
494 not_from_x, not_to_x,
495 not_from_y, not_to_y,
496 only_from_z, only_to_z,
497 in_data_size, inbufs);
499 throw Exception(
"Projection filter unavailable, tf missing");
501 }
else if (filter_type ==
"map_filter") {
505 throw Exception(
"Projection filter unavailable, tf missing");
508 throw Exception(
"Unknown filter type %s", filter_type.c_str());
522 __wait_threads = threads;
535 __wait_barrier = barrier;
Laser360Interface Fawkes BlackBoard Interface.
Wait until a given condition holds.
use the latest of all timestamps
virtual void finalize()
Finalize the thread.
Laser1080Interface Fawkes BlackBoard Interface.
Erase beams outside specified circle sector.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void unlock()
Unlock the mutex.
virtual void wait()
Wait for other threads.
void wake_all()
Wake up all waiting threads.
Removes static laser data (laser beams near occupied map cells)
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
Downsample filter from 1080 to 360 values.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Thread class encapsulation of pthreads.
Downsample filter from 720 to 360 values.
Logger * logger
This is the Logger member used to access the logger.
Erase beams below a certain minimum distance distance.
char * frame() const
Get frame value.
void set_wait_barrier(fawkes::Barrier *barrier)
Set wait barrier.
Thread aspect to use blocked timing.
virtual unsigned int get_out_data_size()
Get size of filtered data array.
LaserFilterThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Cascade of several laser filters to one.
virtual void filter()=0
Filter the incoming data.
virtual bool is_string() const =0
Check if current value is a string.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
char * frame() const
Get frame value.
Cut of laser data at max distance.
iterator begin()
Get iterator for messages.
virtual std::string get_string() const =0
Get string value.
const char * name() const
Get name of thread.
const char * uid() const
Get unique identifier of interface.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_wait_threads(std::list< LaserFilterThread *> &threads)
Set threads to wait for in loop.
virtual const char * path() const =0
Path of value.
Projects one laser into another laser's plane.
float * distances() const
Get distances value.
void wait()
Wait for the condition forever.
use a specific index in the input buffer list
void wait_done()
Wait until thread is done.
void add_filter(LaserDataFilter *filter)
Add a filter to the cascade.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void init()
Initialize the thread.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
float * distances() const
Get distances value.
void lock()
Lock this mutex.
use the first (oldest) of all timestamps
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Mutex mutual exclusion lock.
float * distances() const
Get distances value.
char * frame() const
Get frame value.
Reverse the angle of beams.
virtual void set_out_vector(std::vector< Buffer *> &out)
Set filtered data array.
Configuration * config
This is the Configuration member used to access the configuration.
Merge multiple laser data arrays into one.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
Laser720Interface Fawkes BlackBoard Interface.
virtual std::vector< Buffer * > & get_out_vector()
Get filtered data array.
A barrier is a synchronization tool which blocks until a given number of threads have reached the bar...
virtual void loop()
Code to execute in the thread.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.