Fawkes API Fawkes Development Version
laser_filter_plugin.cpp
1
2/***************************************************************************
3 * laser_filter_plugin.cpp - Fawkes Laser Filter Plugin
4 *
5 * Created: Sun Mar 13 01:06:51 2011
6 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "laser_filter_plugin.h"
24
25#include "filter_thread.h"
26
27#include <core/threading/barrier.h>
28
29#include <map>
30#include <memory>
31#include <set>
32
33using namespace fawkes;
34
35/** @class LaserFilterPlugin "laser_filter_plugin.h"
36 * Laser filter plugin for Fawkes.
37 * This plugin filters laser data. It reads laser data from one or more
38 * interfaces, filters it, and writes to an output interface. It supports
39 * a virtually arbitrary number of active filters.
40 * @author Tim Niemueller
41 */
42
43/** Constructor.
44 * @param config Fawkes configuration
45 */
47{
48 barrier_ = NULL;
49
50 std::set<std::string> configs;
51 std::set<std::string> ignored_configs;
52 std::map<std::string, LaserFilterThread *> threads;
53
54 std::string prefix = "/plugins/laser-filter/";
55
56 // Read configurations and spawn LaserFilterThreads
57#if __cplusplus >= 201103L
58 std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
59#else
60 std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
61#endif
62 while (i->next()) {
63 std::string cfg_name = std::string(i->path()).substr(prefix.length());
64 cfg_name = cfg_name.substr(0, cfg_name.find("/"));
65
66 if ((configs.find(cfg_name) == configs.end())
67 && (ignored_configs.find(cfg_name) == ignored_configs.end())) {
68 std::string cfg_prefix = prefix + cfg_name + "/";
69
70 bool active = true;
71 try {
72 active = config->get_bool((cfg_prefix + "active").c_str());
73 } catch (Exception &e) {
74 } // ignored, assume enabled
75
76 try {
77 if (active) {
78 LaserFilterThread *thread = new LaserFilterThread(cfg_name, cfg_prefix);
79 thread_list.push_back(thread);
80 threads[cfg_name] = thread;
81 configs.insert(cfg_name);
82 } else {
83 //printf("Ignoring laser config %s\n", cfg_name.c_str());
84 ignored_configs.insert(cfg_name);
85 }
86 } catch (Exception &e) {
87 for (ThreadList::iterator i = thread_list.begin(); i != thread_list.end(); ++i) {
88 delete *i;
89 }
90 throw;
91 }
92 }
93 }
94
95 if (thread_list.empty()) {
96 throw Exception("No active laser filters configured, aborting");
97 }
98
99 // Read input and output information for spawned configurations
100 // for dependency detection
101 std::map<std::string, std::list<std::string>> inputs;
102 std::map<std::string, std::list<std::string>> outputs;
103 std::set<std::string>::iterator c, d;
104
105 for (c = configs.begin(); c != configs.end(); ++c) {
106 std::string cinp = prefix + *c + "/in/";
107 std::list<std::string> cinputs;
108#if __cplusplus >= 201103L
109 std::unique_ptr<Configuration::ValueIterator> in(config->search(cinp.c_str()));
110#else
111 std::auto_ptr<Configuration::ValueIterator> in(config->search(cinp.c_str()));
112#endif
113 while (in->next()) {
114 if (in->is_string()) {
115 cinputs.push_back(in->get_string());
116 }
117 }
118
119 std::string coutp = prefix + *c + "/out/";
120 std::list<std::string> coutputs;
121#if __cplusplus >= 201103L
122 std::unique_ptr<Configuration::ValueIterator> out(config->search(coutp.c_str()));
123#else
124 std::auto_ptr<Configuration::ValueIterator> out(config->search(coutp.c_str()));
125#endif
126 while (out->next()) {
127 if (out->is_string()) {
128 coutputs.push_back(out->get_string());
129 }
130 }
131
132 inputs[*c] = cinputs;
133 outputs[*c] = coutputs;
134 }
135
136 // Detect inter-thread dependencies, setup proper serialization by
137 // create a list of threads that one threads depends on and setting
138 // it. Setup common "end of filtering" barrier.
139 try {
140 bool has_deps = false;
141 for (c = configs.begin(); c != configs.end(); ++c) {
142 //printf("Config %s\n", c->c_str());
143
144 std::list<LaserFilterThread *> depthreads;
145
146 std::list<std::string>::iterator i, o;
147 std::list<std::string> & cinputs = inputs[*c];
148 for (i = cinputs.begin(); i != cinputs.end(); ++i) {
149 //printf(" Input %s\n", i->c_str());
150
151 for (d = configs.begin(); d != configs.end(); ++d) {
152 if (*c == *d)
153 continue;
154 //printf(" Config %s\n", d->c_str());
155
156 std::list<std::string> &coutputs = outputs[*d];
157 for (o = coutputs.begin(); o != coutputs.end(); ++o) {
158 //printf(" Output %s\n", o->c_str());
159 if (*i == *o) {
160 has_deps = true;
161 //printf(" *** Dep Thread matches %s for %s\n",
162 // d->c_str(), o->c_str());
163 depthreads.push_back(threads[*d]);
164 break;
165 }
166 }
167 }
168 }
169
170 if (!depthreads.empty()) {
171 depthreads.sort();
172 depthreads.unique();
173 threads[*c]->set_wait_threads(depthreads);
174 }
175 }
176
177 // If any dependencies have been detected, have all threads wait at
178 // a common "end of filtering" barrier, which allows for resetting
179 // a "need to wait for done" flag.
180 if (has_deps) {
181 std::map<std::string, LaserFilterThread *>::iterator t;
182 barrier_ = new Barrier(threads.size());
183 for (t = threads.begin(); t != threads.end(); ++t) {
184 t->second->set_wait_barrier(barrier_);
185 }
186 }
187
188 } catch (Exception &e) {
189 ThreadList::iterator t;
190 for (t = thread_list.begin(); t != thread_list.end(); ++t) {
191 delete *t;
192 }
193 throw;
194 }
195}
196
197LaserFilterPlugin::~LaserFilterPlugin()
198{
199 delete barrier_;
200}
201
202PLUGIN_DESCRIPTION("Filter laser data in blackboard")
203EXPORT_PLUGIN(LaserFilterPlugin)
Laser filter plugin for Fawkes.
LaserFilterPlugin(fawkes::Configuration *config)
Constructor.
Laser filter thread.
Definition: filter_thread.h:55
A barrier is a synchronization tool which blocks until a given number of threads have reached the bar...
Definition: barrier.h:32
Interface for configuration handling.
Definition: config.h:68
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Plugin interface class.
Definition: plugin.h:34
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
ThreadList & threads()
Get a list of threads.
Definition: plugin.cpp:111
Configuration * config
Fawkes configuration.
Definition: plugin.h:58
void push_back(Thread *thread)
Add thread to the end.
Fawkes library namespace.