Fawkes API  Fawkes Development Version
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 "sensor_thread.h"
24 
25 #include <plugins/laser/laser_plugin.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 #ifdef HAVE_LIBUSB
36 # include "sick_tim55x_usb_aqt.h"
37 #endif
38 #ifdef HAVE_SICK55X_BOOST
39 # include "sick_tim55x_ethernet_aqt.h"
40 #endif
41 
42 #include <memory>
43 #include <set>
44 
45 using namespace fawkes;
46 
47 /** @class LaserPlugin "laser_plugin.h"
48  * Laser plugin for Fawkes.
49  * This plugin integrates Fawkes with Laser, for example for accessing
50  * a simulator.
51  * @author Tim Niemueller
52  */
53 
54 /** Constructor.
55  * @param config Fawkes configuration
56  */
58 {
59  std::set<std::string> configs;
60  std::set<std::string> ignored_configs;
61 
62  std::string prefix = "/hardware/laser/";
63 
64 #if __cplusplus >= 201103L
65  std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
66 #else
67  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
68 #endif
69  while (i->next()) {
70  std::string cfg_name = std::string(i->path()).substr(prefix.length());
71  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
72 
73  if ((configs.find(cfg_name) == configs.end())
74  && (ignored_configs.find(cfg_name) == ignored_configs.end())) {
75  std::string cfg_prefix = prefix + cfg_name + "/";
76 
77  bool active = true;
78  try {
79  active = config->get_bool((cfg_prefix + "active").c_str());
80  } catch (Exception &e) {
81  } // ignored, assume enabled
82 
83  try {
84  if (active) {
85  std::string type = config->get_string((cfg_prefix + "type").c_str());
86 
87  //printf("Adding laser acquisition thread for %s\n", cfg_name.c_str());
88  LaserAcquisitionThread *aqt = NULL;
89 #ifdef HAVE_URG
90  if (type == "urg") {
91  aqt = new HokuyoUrgAcquisitionThread(cfg_name, cfg_prefix);
92  } else
93 #endif
94 
95 #ifdef HAVE_LIBPCAN
96  if (type == "lase_edl") {
97  aqt = new LaseEdlAcquisitionThread(cfg_name, cfg_prefix);
98  } else
99 #endif
100 
101 #ifdef HAVE_URG_GBX
102  if (type == "urg_gbx") {
103  aqt = new HokuyoUrgGbxAcquisitionThread(cfg_name, cfg_prefix);
104  } else
105 #endif
106 
107 #ifdef HAVE_LIBUSB
108  if (type == "TiM55x-USB") {
109  aqt = new SickTiM55xUSBAcquisitionThread(cfg_name, cfg_prefix);
110  } else
111 #endif
112 
113 #ifdef HAVE_SICK55X_BOOST
114  if (type == "TiM55x-Ethernet") {
115  aqt = new SickTiM55xEthernetAcquisitionThread(cfg_name, cfg_prefix);
116  } else
117 #endif
118 
119  {
120  throw Exception("Unknown lasertype '%s' for config %s", type.c_str(), cfg_name.c_str());
121  }
122 
123  thread_list.push_back(aqt);
124  thread_list.push_back(new LaserSensorThread(cfg_name, cfg_prefix, aqt));
125 
126  configs.insert(cfg_name);
127  } else {
128  //printf("Ignoring laser config %s\n", cfg_name.c_str());
129  ignored_configs.insert(cfg_name);
130  }
131  } catch (Exception &e) {
132  for (ThreadList::iterator i = thread_list.begin(); i != thread_list.end(); ++i) {
133  delete *i;
134  }
135  throw;
136  }
137  }
138  }
139 
140  if (thread_list.empty()) {
141  throw Exception("No laser devices configured, aborting");
142  }
143 }
144 
145 PLUGIN_DESCRIPTION("Hardware driver for laser range finders")
146 EXPORT_PLUGIN(LaserPlugin)
Laser acqusition thread for Hokuyo URG laser range finders.
Definition: urg_aqt.h:40
Laser acqusition thread for Hokuyo URG laser range finders.
Definition: urg_gbx_aqt.h:44
Laser acqusition thread for Lase EDL L A laser scanner.
Definition: lase_edl_aqt.h:37
Laser acqusition thread.
Laser plugin for Fawkes.
Definition: laser_plugin.h:29
LaserPlugin(fawkes::Configuration *config)
Constructor.
Laser sensor thread.
Definition: sensor_thread.h:47
Laser acqusition thread for Sick TiM55x laser range finders.
Laser acqusition thread for Sick TiM55x laser range finders.
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.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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.