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")
108 throw Exception(
"Filter value %s is not a string", i->
path());
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,
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;
Laser360Interface Fawkes BlackBoard Interface.
void set_wait_threads(std::list< LaserFilterThread * > &threads)
Set threads to wait for in loop.
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)
const char * id() const
Get identifier of interface.
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.
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.
Removes laser data which is represented by a set of boxes.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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.
Copy laser data without modification to a new name.
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.
virtual void set_out_vector(std::vector< Buffer * > &out)
Set filtered data array.
Mutex mutual exclusion lock.
float * distances() const
Get distances value.
char * frame() const
Get frame value.
Reverse the angle of beams.
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.