Fawkes API Fawkes Development Version

laser_plugin.cpp

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)
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends