Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * laser_plugin.cpp - Fawkes Laser Plugin 00004 * 00005 * Created: Tue Aug 05 13:11:02 2008 00006 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 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 /** @class LaserPlugin "laser_plugin.h" 00042 * Laser plugin for Fawkes. 00043 * This plugin integrates Fawkes with Laser, for example for accessing 00044 * a simulator. 00045 * @author Tim Niemueller 00046 */ 00047 00048 /** Constructor. 00049 * @param config Fawkes configuration 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) {} // ignored, assume enabled 00073 00074 try { 00075 00076 if (active) { 00077 std::string type = config->get_string((cfg_prefix + "type").c_str()); 00078 00079 //printf("Adding laser acquisition thread for %s\n", cfg_name.c_str()); 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 //printf("Ignoring laser config %s\n", cfg_name.c_str()); 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)