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 "laser-cluster-thread.h"
24 
25 #include <core/plugin.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  */
38  explicit LaserClusterPlugin(Configuration *config) : Plugin(config)
39  {
40  std::set<std::string> configs;
41  std::set<std::string> ignored_configs;
42 
43  std::string prefix = "/laser-cluster/";
44 
45  // Read configurations and spawn LaserFilterThreads
46 #if __cplusplus >= 201103L
47  std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
48 #else
49  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
50 #endif
51  while (i->next()) {
52  std::string cfg_name = std::string(i->path()).substr(prefix.length());
53  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
54 
55  if ((configs.find(cfg_name) == configs.end())
56  && (ignored_configs.find(cfg_name) == ignored_configs.end())) {
57  std::string cfg_prefix = prefix + cfg_name + "/";
58 
59  bool active = true;
60  try {
61  active = config->get_bool((cfg_prefix + "active").c_str());
62  } catch (Exception &e) {
63  } // ignored, assume enabled
64 
65  try {
66  if (active) {
67  LaserClusterThread *thread = new LaserClusterThread(cfg_name, cfg_prefix);
68  thread_list.push_back(thread);
69  configs.insert(cfg_name);
70  } else {
71  //printf("Ignoring laser config %s\n", cfg_name.c_str());
72  ignored_configs.insert(cfg_name);
73  }
74  } catch (Exception &e) {
75  for (ThreadList::iterator i = thread_list.begin(); i != thread_list.end(); ++i) {
76  delete *i;
77  }
78  throw;
79  }
80  }
81  }
82 
83  if (thread_list.empty()) {
84  throw Exception("No active laser filters configured, aborting");
85  }
86  }
87 };
88 
89 PLUGIN_DESCRIPTION("Detect cluster in 2D laser data")
90 EXPORT_PLUGIN(LaserClusterPlugin)
Plugin to detect a cluster in 2D laser data.
LaserClusterPlugin(Configuration *config)
Constructor.
Main thread of laser-cluster plugin.
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
Fawkes library namespace.