Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
laser_plugin.cpp
1 
2 /***************************************************************************
3  * laser_plugin.cpp - Fawkes Laser Plugin
4  *
5  * Created: Tue Aug 05 13:11:02 2008
6  * Copyright 2006-2009 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 <plugins/laser/laser_plugin.h>
24 
25 #include "sensor_thread.h"
26 #ifdef HAVE_LIBPCAN
27 # include "lase_edl_aqt.h"
28 #endif
29 #ifdef HAVE_URG
30 # include "urg_aqt.h"
31 #endif
32 #ifdef HAVE_URG_GBX
33 # include "urg_gbx_aqt.h"
34 #endif
35 
36 #include <set>
37 #include <memory>
38 
39 using namespace fawkes;
40 
41 /** @class LaserPlugin "laser_plugin.h"
42  * Laser plugin for Fawkes.
43  * This plugin integrates Fawkes with Laser, for example for accessing
44  * a simulator.
45  * @author Tim Niemueller
46  */
47 
48 /** Constructor.
49  * @param config Fawkes configuration
50  */
52  : Plugin(config)
53 {
54  std::set<std::string> configs;
55  std::set<std::string> ignored_configs;
56 
57  std::string prefix = "/hardware/laser/";
58 
59  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
60  while (i->next()) {
61  std::string cfg_name = std::string(i->path()).substr(prefix.length());
62  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
63 
64  if ( (configs.find(cfg_name) == configs.end()) &&
65  (ignored_configs.find(cfg_name) == ignored_configs.end()) ) {
66 
67  std::string cfg_prefix = prefix + cfg_name + "/";
68 
69  bool active = true;
70  try {
71  active = config->get_bool((cfg_prefix + "active").c_str());
72  } catch (Exception &e) {} // ignored, assume enabled
73 
74  try {
75 
76  if (active) {
77  std::string type = config->get_string((cfg_prefix + "type").c_str());
78 
79  //printf("Adding laser acquisition thread for %s\n", cfg_name.c_str());
80  LaserAcquisitionThread *aqt = NULL;
81 #ifdef HAVE_URG
82  if ( type == "urg" ) {
83  aqt = new HokuyoUrgAcquisitionThread(cfg_name, cfg_prefix);
84  } else
85 #endif
86 
87 #ifdef HAVE_LIBPCAN
88  if ( type == "lase_edl" ) {
89  aqt = new LaseEdlAcquisitionThread(cfg_name, cfg_prefix);
90  } else
91 #endif
92 
93 #ifdef HAVE_URG_GBX
94  if ( type == "urg_gbx" ) {
95  aqt = new HokuyoUrgGbxAcquisitionThread(cfg_name, cfg_prefix);
96  } else
97 #endif
98 
99  {
100  throw Exception("Unknown lasertype '%s' for config %s",
101  type.c_str(), cfg_name.c_str());
102  }
103 
104  thread_list.push_back(aqt);
105  thread_list.push_back(new LaserSensorThread(cfg_name, cfg_prefix, aqt));
106 
107  configs.insert(cfg_name);
108  } else {
109  //printf("Ignoring laser config %s\n", cfg_name.c_str());
110  ignored_configs.insert(cfg_name);
111  }
112  } catch(Exception &e) {
113  for (ThreadList::iterator i = thread_list.begin();
114  i != thread_list.end(); ++i) {
115  delete *i;
116  }
117  throw;
118  }
119  }
120  }
121 
122  if ( thread_list.empty() ) {
123  throw Exception("No laser devices configured, aborting");
124  } else {
125  }
126 }
127 
128 
129 PLUGIN_DESCRIPTION("Hardware driver for laser range finders")
130 EXPORT_PLUGIN(LaserPlugin)