Fawkes API  Fawkes Development Version
laser-cluster-plugin.cpp
1 
2 /***************************************************************************
3  * laser-cluster-plugin.cpp - Detect a cluster from 2D laser data
4  *
5  * Created: Sun Apr 21 01:15:54 2013
6  * Copyright 2011-2013 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 <core/plugin.h>
24 
25 #include "laser-cluster-thread.h"
26 
27 using namespace fawkes;
28 
29 /** Plugin to detect a cluster in 2D laser data.
30  * @author Tim Niemueller
31  */
33 {
34  public:
35  /** Constructor.
36  * @param config Fawkes configuration
37  */
39  : Plugin(config)
40  {
41 
42  std::set<std::string> configs;
43  std::set<std::string> ignored_configs;
44 
45  std::string prefix = "/laser-cluster/";
46 
47  // Read configurations and spawn LaserFilterThreads
48 #if __cplusplus >= 201103L
49  std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
50 #else
51  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
52 #endif
53  while (i->next()) {
54  std::string cfg_name = std::string(i->path()).substr(prefix.length());
55  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
56 
57  if ( (configs.find(cfg_name) == configs.end()) &&
58  (ignored_configs.find(cfg_name) == ignored_configs.end()) ) {
59 
60  std::string cfg_prefix = prefix + cfg_name + "/";
61 
62  bool active = true;
63  try {
64  active = config->get_bool((cfg_prefix + "active").c_str());
65  } catch (Exception &e) {} // ignored, assume enabled
66 
67  try {
68  if (active) {
69  LaserClusterThread *thread = new LaserClusterThread(cfg_name, cfg_prefix);
70  thread_list.push_back(thread);
71  configs.insert(cfg_name);
72  } else {
73  //printf("Ignoring laser config %s\n", cfg_name.c_str());
74  ignored_configs.insert(cfg_name);
75  }
76  } catch(Exception &e) {
77  for (ThreadList::iterator i = thread_list.begin();
78  i != thread_list.end(); ++i) {
79  delete *i;
80  }
81  throw;
82  }
83  }
84  }
85 
86  if ( thread_list.empty() ) {
87  throw Exception("No active laser filters configured, aborting");
88  }
89  }
90 };
91 
92 PLUGIN_DESCRIPTION("Detect cluster in 2D laser data")
93 EXPORT_PLUGIN(LaserClusterPlugin)
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.
Plugin to detect a cluster in 2D laser data.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
Main thread of laser-cluster plugin.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * path() const =0
Path of value.
LaserClusterPlugin(Configuration *config)
Constructor.
Interface for configuration handling.
Definition: config.h:67