laser_plugin.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <plugins/laser/laser_plugin.h>
00024
00025 #include "sensor_thread.h"
00026 #ifdef HAVE_LIBPCAN
00027 # include "lase_edl_aqt.h"
00028 #endif
00029 #ifdef HAVE_URG
00030 # include "urg_aqt.h"
00031 #endif
00032 #ifdef HAVE_URG_GBX
00033 # include "urg_gbx_aqt.h"
00034 #endif
00035
00036 #include <set>
00037 #include <memory>
00038
00039 using namespace fawkes;
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 LaserPlugin::LaserPlugin(Configuration *config)
00052 : Plugin(config)
00053 {
00054 std::set<std::string> configs;
00055 std::set<std::string> ignored_configs;
00056
00057 std::string prefix = "/hardware/laser/";
00058
00059 std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
00060 while (i->next()) {
00061 std::string cfg_name = std::string(i->path()).substr(prefix.length());
00062 cfg_name = cfg_name.substr(0, cfg_name.find("/"));
00063
00064 if ( (configs.find(cfg_name) == configs.end()) &&
00065 (ignored_configs.find(cfg_name) == ignored_configs.end()) ) {
00066
00067 std::string cfg_prefix = prefix + cfg_name + "/";
00068
00069 bool active = true;
00070 try {
00071 active = config->get_bool((cfg_prefix + "active").c_str());
00072 } catch (Exception &e) {}
00073
00074 try {
00075
00076 if (active) {
00077 std::string type = config->get_string((cfg_prefix + "type").c_str());
00078
00079
00080 LaserAcquisitionThread *aqt = NULL;
00081 #ifdef HAVE_URG
00082 if ( type == "urg" ) {
00083 aqt = new HokuyoUrgAcquisitionThread(cfg_name, cfg_prefix);
00084 } else
00085 #endif
00086
00087 #ifdef HAVE_LIBPCAN
00088 if ( type == "lase_edl" ) {
00089 aqt = new LaseEdlAcquisitionThread(cfg_name, cfg_prefix);
00090 } else
00091 #endif
00092
00093 #ifdef HAVE_URG_GBX
00094 if ( type == "urg_gbx" ) {
00095 aqt = new HokuyoUrgGbxAcquisitionThread(cfg_name, cfg_prefix);
00096 } else
00097 #endif
00098
00099 {
00100 throw Exception("Unknown lasertype '%s' for config %s",
00101 type.c_str(), cfg_name.c_str());
00102 }
00103
00104 thread_list.push_back(aqt);
00105 thread_list.push_back(new LaserSensorThread(cfg_name, cfg_prefix, aqt));
00106
00107 configs.insert(cfg_name);
00108 } else {
00109
00110 ignored_configs.insert(cfg_name);
00111 }
00112 } catch(Exception &e) {
00113 for (ThreadList::iterator i = thread_list.begin();
00114 i != thread_list.end(); ++i) {
00115 delete *i;
00116 }
00117 throw;
00118 }
00119 }
00120 }
00121
00122 if ( thread_list.empty() ) {
00123 throw Exception("No synchronization peers configured, aborting");
00124 } else {
00125 }
00126 }
00127
00128
00129 PLUGIN_DESCRIPTION("Reads data from laser range finder and writes to BlackBoard")
00130 EXPORT_PLUGIN(LaserPlugin)