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 <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 #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 <set>
43 #include <memory>
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  : Plugin(config)
59 {
60  std::set<std::string> configs;
61  std::set<std::string> ignored_configs;
62 
63  std::string prefix = "/hardware/laser/";
64 
65 #if __cplusplus >= 201103L
66  std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
67 #else
68  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
69 #endif
70  while (i->next()) {
71  std::string cfg_name = std::string(i->path()).substr(prefix.length());
72  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
73 
74  if ( (configs.find(cfg_name) == configs.end()) &&
75  (ignored_configs.find(cfg_name) == ignored_configs.end()) ) {
76 
77  std::string cfg_prefix = prefix + cfg_name + "/";
78 
79  bool active = true;
80  try {
81  active = config->get_bool((cfg_prefix + "active").c_str());
82  } catch (Exception &e) {} // ignored, assume enabled
83 
84  try {
85 
86  if (active) {
87  std::string type = config->get_string((cfg_prefix + "type").c_str());
88 
89  //printf("Adding laser acquisition thread for %s\n", cfg_name.c_str());
90  LaserAcquisitionThread *aqt = NULL;
91 #ifdef HAVE_URG
92  if ( type == "urg" ) {
93  aqt = new HokuyoUrgAcquisitionThread(cfg_name, cfg_prefix);
94  } else
95 #endif
96 
97 #ifdef HAVE_LIBPCAN
98  if ( type == "lase_edl" ) {
99  aqt = new LaseEdlAcquisitionThread(cfg_name, cfg_prefix);
100  } else
101 #endif
102 
103 #ifdef HAVE_URG_GBX
104  if ( type == "urg_gbx" ) {
105  aqt = new HokuyoUrgGbxAcquisitionThread(cfg_name, cfg_prefix);
106  } else
107 #endif
108 
109 #ifdef HAVE_LIBUSB
110  if ( type == "TiM55x-USB" ) {
111  aqt = new SickTiM55xUSBAcquisitionThread(cfg_name, cfg_prefix);
112  } else
113 #endif
114 
115 #ifdef HAVE_SICK55X_BOOST
116  if ( type == "TiM55x-Ethernet" ) {
117  aqt = new SickTiM55xEthernetAcquisitionThread(cfg_name, cfg_prefix);
118  } else
119 #endif
120 
121  {
122  throw Exception("Unknown lasertype '%s' for config %s",
123  type.c_str(), cfg_name.c_str());
124  }
125 
126  thread_list.push_back(aqt);
127  thread_list.push_back(new LaserSensorThread(cfg_name, cfg_prefix, aqt));
128 
129  configs.insert(cfg_name);
130  } else {
131  //printf("Ignoring laser config %s\n", cfg_name.c_str());
132  ignored_configs.insert(cfg_name);
133  }
134  } catch(Exception &e) {
135  for (ThreadList::iterator i = thread_list.begin();
136  i != thread_list.end(); ++i) {
137  delete *i;
138  }
139  throw;
140  }
141  }
142  }
143 
144  if ( thread_list.empty() ) {
145  throw Exception("No laser devices configured, aborting");
146  } else {
147  }
148 }
149 
150 
151 PLUGIN_DESCRIPTION("Hardware driver for laser range finders")
152 EXPORT_PLUGIN(LaserPlugin)
Plugin interface class.
Definition: plugin.h:33
Laser acqusition thread for Lase EDL L A laser scanner.
Definition: lase_edl_aqt.h:36
LaserPlugin(fawkes::Configuration *config)
Constructor.
Laser acqusition thread.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Laser sensor thread.
Definition: sensor_thread.h:42
Laser plugin for Fawkes.
Definition: laser_plugin.h:28
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Laser acqusition thread for Hokuyo URG laser range finders.
Definition: urg_aqt.h:39
Base class for exceptions in Fawkes.
Definition: exception.h:36
Laser acqusition thread for Sick TiM55x laser range finders.
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
virtual const char * path() const =0
Path of value.
void push_back(Thread *thread)
Add thread to the end.
Laser acqusition thread for Hokuyo URG laser range finders.
Definition: urg_gbx_aqt.h:43
Laser acqusition thread for Sick TiM55x laser range finders.
Interface for configuration handling.
Definition: config.h:67
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.