23 #include "filter_thread.h" 24 #include "filters/max_circle.h" 25 #include "filters/720to360.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" 36 #include <core/threading/barrier.h> 37 #include <core/threading/mutex.h> 38 #include <core/threading/wait_condition.h> 40 #include <interfaces/Laser360Interface.h> 41 #include <interfaces/Laser720Interface.h> 63 std::string &cfg_prefix)
64 :
Thread(
"LaserFilterThread",
Thread::OPMODE_WAITFORWAKEUP),
67 set_name(
"LaserFilterThread(%s)", cfg_name.c_str());
68 __cfg_name = cfg_name;
69 __cfg_prefix = cfg_prefix;
70 __wait_barrier = NULL;
78 open_interfaces(__cfg_prefix +
"in/", __in, __in_bufs,
false);
79 open_interfaces(__cfg_prefix +
"out/", __out, __out_bufs,
true);
82 throw Exception(
"No input interfaces defined for %s", __cfg_name.c_str());
85 throw Exception(
"No output interfaces defined for %s", __cfg_name.c_str());
89 std::map<std::string, std::string> filters;
91 std::string fpfx = __cfg_prefix +
"filters/";
92 std::auto_ptr<Configuration::ValueIterator> i(
config->
search(fpfx.c_str()));
94 std::string filter_name = std::string(i->
path()).substr(fpfx.length());
95 if (filter_name.find(
"/") != std::string::npos) {
101 throw Exception(
"Filter value %s is not a string", i->
path());
106 if (filters.empty()) {
107 throw Exception(
"No filters defined for %s", __cfg_name.c_str());
110 if (filters.size() == 1) {
111 std::string filter_name = filters.
begin()->first;
113 filter_name.c_str(), filters[filter_name].c_str());
114 __filter = create_filter(filters[filter_name], fpfx + filter_name +
"/",
115 __in[0].is_360 ? 360 : 720, __in_bufs);
121 std::map<std::string, std::string>::iterator f;
122 for (f = filters.begin(); f != filters.end(); ++f) {
124 f->first.c_str(), f->second.c_str(), __in_bufs.size(),
126 cascade->
add_filter(create_filter(f->second, fpfx + f->first +
"/",
139 Exception e(
"Output interface and filter data size for %s do not match (%u != 360)",
144 Exception e(
"Output interface and filter data size for %s do not match (%u != 720)",
153 for (
unsigned int i = 0; i < __in.size(); ++i) {
156 for (
unsigned int i = 0; i < __out.size(); ++i) {
162 std::list<LaserFilterThread *>::iterator wt;
163 for (wt = __wait_threads.begin(); wt != __wait_threads.end(); ++wt) {
168 __wait_mutex =
new Mutex();
180 for (
unsigned int i = 0; i < __in.size(); ++i) {
184 for (
unsigned int i = 0; i < __out.size(); ++i) {
194 if (__wait_barrier) {
195 std::list<LaserFilterThread *>::iterator wt;
196 for (wt = __wait_threads.begin(); wt != __wait_threads.end(); ++wt) {
202 const size_t in_num = __in.size();
203 for (
size_t i = 0; i != in_num; ++i) {
204 __in[i].interface->read();
205 if (__in[i].is_360) {
206 __in_bufs[i]->frame = __in[i].interface_typed.as360->frame();
208 __in_bufs[i]->frame = __in[i].interface_typed.as720->frame();
221 const size_t num = __out.size();
222 for (
size_t i = 0; i < num; ++i) {
223 if (__out[i].is_360) {
224 __out[i].interface_typed.as360->set_frame(__out_bufs[i]->frame.c_str());
226 __out[i].interface_typed.as720->set_frame(__out_bufs[i]->frame.c_str());
228 __out[i].interface->write();
231 if (__wait_barrier) {
232 __wait_mutex->
lock();
236 __wait_barrier->
wait();
237 __wait_mutex->
lock();
251 __wait_mutex->
lock();
252 while (__wait_done) {
261 LaserFilterThread::open_interfaces(std::string prefix,
262 std::vector<LaserInterface> &ifs,
263 std::vector<LaserDataFilter::Buffer *> &bufs,
bool writing)
265 std::auto_ptr<Configuration::ValueIterator> in(
config->
search(prefix.c_str()));
268 throw Exception(
"Config value %s is not of type string", in->
path());
273 if ((sf = uid.find(
"::")) == std::string::npos) {
274 throw Exception(
"Interface '%s' is not a UID", uid.c_str());
276 std::string type = uid.substr(0, sf);
277 std::string
id = uid.substr(sf + 2);
280 lif.interface = NULL;
282 if (type ==
"Laser360Interface") {
284 }
else if (type ==
"Laser720Interface") {
287 throw Exception(
"Interfaces must be of type Laser360Interface or " 288 "Laser720Interface, but it is '%s'", type.c_str());
297 throw Exception(
"No interfaces defined at %s", prefix.c_str());
300 bufs.resize(ifs.size());
302 bool must_360 = ifs[0].is_360;
306 for (
unsigned int i = 0; i < ifs.size(); ++i) {
309 throw Exception(
"Interfaces of mixed sizes for %s",
316 ifs[i].interface_typed.as360 = laser360;
317 ifs[i].interface = laser360;
323 throw Exception(
"Interfaces of mixed sizes for %s",
331 ifs[i].interface_typed.as720 = laser720;
332 ifs[i].interface = laser720;
338 for (
unsigned int i = 0; i < ifs.size(); ++i) {
344 ifs[i].interface_typed.as360 = laser360;
345 ifs[i].interface = laser360;
347 bufs[i]->frame = laser360->
frame();
355 ifs[i].interface_typed.as720 = laser720;
356 ifs[i].interface = laser720;
358 bufs[i]->frame = laser720->
frame();
364 for (
unsigned int i = 0; i < ifs.size(); ++i) {
375 LaserFilterThread::create_filter(std::string filter_type, std::string prefix,
376 unsigned int in_data_size,
377 std::vector<LaserDataFilter::Buffer *> &inbufs)
379 if (filter_type ==
"720to360") {
380 bool average =
false;
385 }
else if (filter_type ==
"reverse") {
387 }
else if (filter_type ==
"max_circle") {
390 }
else if (filter_type ==
"min_circle") {
393 }
else if (filter_type ==
"circle_sector") {
397 }
else if (filter_type ==
"deadspots") {
399 }
else if (filter_type ==
"min_merge") {
401 }
else if (filter_type ==
"projection") {
403 const float not_from_x =
config->
get_float((prefix +
"not_from_x").c_str());
404 const float not_to_x =
config->
get_float((prefix +
"not_to_x").c_str());
405 const float not_from_y =
config->
get_float((prefix +
"not_from_y").c_str());
406 const float not_to_y =
config->
get_float((prefix +
"not_to_y").c_str());
407 const float only_from_z =
config->
get_float((prefix +
"only_from_z").c_str());
408 const float only_to_z =
config->
get_float((prefix +
"only_to_z").c_str());
409 const std::string frame =
412 not_from_x, not_to_x,
413 not_from_y, not_to_y,
414 only_from_z, only_to_z,
415 in_data_size, inbufs);
417 throw Exception(
"Projection filter unavailable, tf missing");
420 throw Exception(
"Unknown filter type %s", filter_type.c_str());
434 __wait_threads = threads;
447 __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.
virtual void finalize()
Finalize the thread.
Erase beams outside specified circle sector.
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.
virtual std::string get_string() const =0
Get string value.
virtual bool is_string() const =0
Check if current value is a string.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Thread class encapsulation of pthreads.
virtual const char * path() const =0
Path of value.
char * frame() const
Get frame value.
Downsample filter from 720 to 360 values.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
Erase beams below a certain minimum distance distance.
void set_wait_barrier(fawkes::Barrier *barrier)
Set wait barrier.
Thread aspect to use blocked timing.
char * frame() const
Get frame value.
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.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
Cut of laser data at max distance.
iterator begin()
Get iterator for messages.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Projects one laser into another laser's plane.
void wait()
Wait for the condition forever.
const char * name() const
Get name of thread.
float * distances() const
Get distances value.
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)=0
Open interface for reading.
void lock()
Lock this mutex.
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.
float * distances() const
Get distances value.
Mutex mutual exclusion lock.
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 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.