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, 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;