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>
47 using namespace fawkes;
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()));
267 if (! in->is_string()) {
268 throw Exception(
"Config value %s is not of type string", in->path());
270 std::string uid = in->get_string();
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;