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 #include <map>
29 #include <set>
30 #include <memory>
31 
32 using namespace fawkes;
33 
34 /** @class LaserFilterPlugin "laser_filter_plugin.h"
35  * Laser filter plugin for Fawkes.
36  * This plugin filters laser data. It reads laser data from one or more
37  * interfaces, filters it, and writes to an output interface. It supports
38  * a virtually arbitrary number of active filters.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor.
43  * @param config Fawkes configuration
44  */
46  : Plugin(config)
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 
69  std::string cfg_prefix = prefix + cfg_name + "/";
70 
71  bool active = true;
72  try {
73  active = config->get_bool((cfg_prefix + "active").c_str());
74  } catch (Exception &e) {} // 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();
88  i != thread_list.end(); ++i) {
89  delete *i;
90  }
91  throw;
92  }
93  }
94  }
95 
96  if ( thread_list.empty() ) {
97  throw Exception("No active laser filters configured, aborting");
98  }
99 
100  // Read input and output information for spawned configurations
101  // for dependency detection
102  std::map<std::string, std::list<std::string> > inputs;
103  std::map<std::string, std::list<std::string> > outputs;
104  std::set<std::string>::iterator c, d;
105 
106  for (c = configs.begin(); c != configs.end(); ++c) {
107  std::string cinp = prefix + *c + "/in/";
108  std::list<std::string> cinputs;
109 #if __cplusplus >= 201103L
110  std::unique_ptr<Configuration::ValueIterator> in(config->search(cinp.c_str()));
111 #else
112  std::auto_ptr<Configuration::ValueIterator> in(config->search(cinp.c_str()));
113 #endif
114  while (in->next()) {
115  if (in->is_string()) {
116  cinputs.push_back(in->get_string());
117  }
118  }
119 
120  std::string coutp = prefix + *c + "/out/";
121  std::list<std::string> coutputs;
122 #if __cplusplus >= 201103L
123  std::unique_ptr<Configuration::ValueIterator> out(config->search(coutp.c_str()));
124 #else
125  std::auto_ptr<Configuration::ValueIterator> out(config->search(coutp.c_str()));
126 #endif
127  while (out->next()) {
128  if (out->is_string()) {
129  coutputs.push_back(out->get_string());
130  }
131  }
132 
133  inputs[*c] = cinputs;
134  outputs[*c] = coutputs;
135  }
136 
137  // Detect inter-thread dependencies, setup proper serialization by
138  // create a list of threads that one threads depends on and setting
139  // it. Setup common "end of filtering" barrier.
140  try {
141  bool has_deps = false;
142  for (c = configs.begin(); c != configs.end(); ++c) {
143 
144  //printf("Config %s\n", c->c_str());
145 
146  std::list<LaserFilterThread *> depthreads;
147 
148  std::list<std::string>::iterator i, o;
149  std::list<std::string> &cinputs = inputs[*c];
150  for (i = cinputs.begin(); i != cinputs.end(); ++i) {
151  //printf(" Input %s\n", i->c_str());
152 
153  for (d = configs.begin(); d != configs.end(); ++d) {
154  if (*c == *d) continue;
155  //printf(" Config %s\n", d->c_str());
156 
157  std::list<std::string> &coutputs = outputs[*d];
158  for (o = coutputs.begin(); o != coutputs.end(); ++o) {
159  //printf(" Output %s\n", o->c_str());
160  if (*i == *o) {
161  has_deps = true;
162  //printf(" *** Dep Thread matches %s for %s\n",
163  // d->c_str(), o->c_str());
164  depthreads.push_back(threads[*d]);
165  break;
166  }
167  }
168  }
169  }
170 
171  if (! depthreads.empty()) {
172  depthreads.sort();
173  depthreads.unique();
174  threads[*c]->set_wait_threads(depthreads);
175  }
176  }
177 
178  // If any dependencies have been detected, have all threads wait at
179  // a common "end of filtering" barrier, which allows for resetting
180  // a "need to wait for done" flag.
181  if (has_deps) {
182  std::map<std::string, LaserFilterThread *>::iterator t;
183  __barrier = new Barrier(threads.size());
184  for (t = threads.begin(); t != threads.end(); ++t) {
185  t->second->set_wait_barrier(__barrier);
186  }
187  }
188 
189  } catch (Exception &e) {
190  ThreadList::iterator t;
191  for (t = thread_list.begin(); t != thread_list.end(); ++t) {
192  delete *t;
193  }
194  throw;
195  }
196 }
197 
198 
199 LaserFilterPlugin::~LaserFilterPlugin()
200 {
201  delete __barrier;
202 }
203 
204 
205 PLUGIN_DESCRIPTION("Filter laser data in blackboard")
206 EXPORT_PLUGIN(LaserFilterPlugin)
Plugin interface class.
Definition: plugin.h:33
Fawkes library namespace.
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.
virtual bool next()=0
Check if there is another element and advance to this if possible.
ThreadList & threads()
Get a list of threads.
Definition: plugin.cpp:110
virtual bool is_string() const =0
Check if current value is a string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
virtual std::string get_string() const =0
Get string value.
Laser filter thread.
Definition: filter_thread.h:47
virtual const char * path() const =0
Path of value.
void push_back(Thread *thread)
Add thread to the end.
Laser filter plugin for Fawkes.
LaserFilterPlugin(fawkes::Configuration *config)
Constructor.
Interface for configuration handling.
Definition: config.h:67
A barrier is a synchronization tool which blocks until a given number of threads have reached the bar...
Definition: barrier.h:32