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
27using namespace fawkes;
28
29/** Plugin to detect a cluster in 2D laser data.
30 * @author Tim Niemueller
31 */
33{
34public:
35 /** Constructor.
36 * @param config Fawkes configuration
37 */
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
89PLUGIN_DESCRIPTION("Detect cluster in 2D laser data")
90EXPORT_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
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
Configuration * config
Fawkes configuration.
Definition: plugin.h:58
void push_back(Thread *thread)
Add thread to the end.
Fawkes library namespace.