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 
33 using 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 
197 LaserFilterPlugin::~LaserFilterPlugin()
198 {
199  delete barrier_;
200 }
201 
202 PLUGIN_DESCRIPTION("Filter laser data in blackboard")
203 EXPORT_PLUGIN(LaserFilterPlugin)
fawkes::Plugin::config
Configuration * config
Fawkes configuration.
Definition: plugin.h:58
LaserFilterPlugin::LaserFilterPlugin
LaserFilterPlugin(fawkes::Configuration *config)
Constructor.
Definition: laser_filter_plugin.cpp:46
fawkes::Plugin::thread_list
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
fawkes::ThreadList::push_back
void push_back(Thread *thread)
Add thread to the end.
Definition: thread_list.cpp:773
fawkes::Configuration::ValueIterator::is_string
virtual bool is_string() const =0
Check if current value is a string.
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
fawkes::Configuration::ValueIterator::get_string
virtual std::string get_string() const =0
Get string value.
fawkes::Configuration
Interface for configuration handling.
Definition: config.h:65
fawkes::Configuration::search
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
fawkes
Fawkes library namespace.
fawkes::Plugin::threads
ThreadList & threads()
Get a list of threads.
Definition: plugin.cpp:111
LaserFilterThread
Laser filter thread.
Definition: filter_thread.h:55
fawkes::Barrier
A barrier is a synchronization tool which blocks until a given number of threads have reached the bar...
Definition: barrier.h:32
fawkes::Configuration::ValueIterator::path
virtual const char * path() const =0
Path of value.
fawkes::Configuration::ValueIterator::next
virtual bool next()=0
Check if there is another element and advance to this if possible.
fawkes::Plugin
Plugin interface class.
Definition: plugin.h:34
LaserFilterPlugin
Laser filter plugin for Fawkes.
Definition: laser_filter_plugin.h:33
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36