SimulatedDevices.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013 Tallinn University of Technology.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  *
00008  * Contributors:
00009  *     Yuri Gavshin
00010  */
00011 
00012 #include <uwsim/SimulatedDevices.h>
00013 #include <uwsim/ConfigXMLParser.h>
00014 #include <ros/ros.h>
00015 #include <osg/Notify>
00016 
00017 #include <pluginlib/class_loader.h>
00018 using namespace uwsim;
00019 
00020 class SimulatedDevicesLoader
00021 {
00022   boost::shared_ptr<pluginlib::ClassLoader<SimulatedDeviceFactory> > simdev_loader;
00023   vector<string> available_plugins;
00024 public:
00025   //a list of "factories" to initialize and apply a device and/or rosinterface
00026   std::vector<SimulatedDeviceFactory::Ptr> factories;
00027 
00028   SimulatedDevicesLoader()
00029   {
00030     simdev_loader.reset(new pluginlib::ClassLoader<SimulatedDeviceFactory>("uwsim", "uwsim::SimulatedDeviceFactory"));
00031 
00032     available_plugins = simdev_loader->getDeclaredClasses();
00033     for (size_t i = 0; i < available_plugins.size(); ++i)
00034     {
00035       OSG_ALWAYS << "Loading SimulatedDevices plugin: '" << available_plugins.at(i) << "'" << std::endl;
00036       factories.push_back(simdev_loader->createInstance(available_plugins.at(i)));
00037     }
00038 
00039     for (size_t i = 0; i < factories.size(); ++i)
00040       for (size_t j = 0; j < i; ++j)
00041         if (factories[i]->getType() == factories[j]->getType())
00042           OSG_FATAL << "SimulatedDevices factories types must be unique, but the same type '" << factories[i]->getType()
00043               << "' is specified at indexes " << j << " and " << i << " in initFacotries() in SimulatedDevices.cpp"
00044               << std::endl;
00045   }
00046 
00047   ~SimulatedDevicesLoader()
00048   {
00049     factories.clear();
00050     for (size_t i = 0; i < available_plugins.size(); ++i)
00051     {
00052       simdev_loader->unloadLibraryForClass(available_plugins.at(i));
00053     }
00054   }
00055 };
00056 
00057 boost::shared_ptr<SimulatedDevicesLoader> loader(new SimulatedDevicesLoader());
00058 
00059 SimulatedDevices::SimulatedDevices()
00060 {
00061 }
00062 
00063 std::vector<boost::shared_ptr<ROSInterface> > SimulatedDevices::getInterfaces(
00064     ROSInterfaceInfo & rosInterface, std::vector<boost::shared_ptr<SimulatedIAUV> > & iauvFile)
00065 {
00066   std::vector < boost::shared_ptr<ROSInterface> > ifaces;
00067   bool isFactoryFound = false;
00068   if (rosInterface.type == ROSInterfaceInfo::SimulatedDevice)
00069   {
00070     for (size_t i = 0; i < loader->factories.size(); ++i)
00071       if (loader->factories[i]->getType() == rosInterface.subtype)
00072       {
00073         isFactoryFound = true;
00074         std::vector < boost::shared_ptr<ROSInterface> > ifaces_ = loader->factories[i]->getInterface(rosInterface,
00075                                                                                                      iauvFile);
00076         for (size_t j = 0; j < ifaces_.size(); ++j)
00077           ifaces.push_back(ifaces_[j]);
00078       }
00079 
00080     if (!isFactoryFound)
00081       OSG_FATAL << "Unknown ROSIterface '" << rosInterface.subtype << "ROS', skipping..." << std::endl;
00082   }
00083   return ifaces;
00084 }
00085 
00086 void SimulatedDevices::applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *oscene)
00087 {
00088   for (size_t iteration = 0; iteration < 10; ++iteration) //executed max 10 times
00089   {
00090     bool configured = true;
00091     for (size_t i = 0; i < loader->factories.size(); ++i)
00092     {
00093       if (!loader->factories[i]->applyConfig(auv, vehicleChars, oscene, iteration))
00094         configured = false;
00095     }
00096     if (configured)
00097       break;
00098   }
00099 }
00100 
00101 static void processConfigNode(const xmlpp::Node* node, ConfigFile * config, SimulatedDeviceConfig::Ptr cfg)
00102 {
00103   if (!cfg)
00104     return;
00105 
00106   xmlpp::Node::NodeList list = node->get_children();
00107   for (xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter)
00108   {
00109     const xmlpp::Node* child = dynamic_cast<const xmlpp::Node*>(*iter);
00110     if (child->get_name() == "name" && cfg->name.length() == 0)
00111       config->extractStringChar(child, cfg->name);
00112   }
00113 }
00114 
00115 std::vector<SimulatedDeviceConfig::Ptr> SimulatedDevices::processConfig(const xmlpp::Node* node, ConfigFile * config,
00116                                                                         bool isDevice)
00117 {
00118   std::vector < SimulatedDeviceConfig::Ptr > devs;
00119   if (node->get_name() == "text" || node->get_name() == "comment")
00120     return devs;
00121   if (isDevice)
00122   {
00123     bool isFactoryFound = false;
00124     for (size_t i = 0; i < loader->factories.size(); ++i)
00125       if (loader->factories[i]->getType() == node->get_name())
00126       {
00127         isFactoryFound = true;
00128         SimulatedDeviceConfig::Ptr dev = loader->factories[i]->processConfig(node, config);
00129         if (dev)
00130           processConfigNode(node, config, dev);
00131         devs.push_back(dev);
00132       }
00133     if (!isFactoryFound)
00134       OSG_FATAL << "Unknown SimulatedDevice '" << node->get_name() << "', skipping..." << std::endl;
00135   }
00136   else
00137   {
00138     xmlpp::Node::NodeList list = node->get_children();
00139     for (xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter)
00140     {
00141       const xmlpp::Node* child = dynamic_cast<const xmlpp::Node*>(*iter);
00142       std::vector < SimulatedDeviceConfig::Ptr > devs_ = processConfig(child, config, true);
00143       for (size_t i = 0; i < devs_.size(); ++i)
00144         devs.push_back(devs_.at(i));
00145     }
00146   }
00147   return devs;
00148 }
00149 
00150 SimulatedDeviceConfig::SimulatedDeviceConfig(std::string type)
00151 {
00152   this->type = type;
00153 }
00154 
00155 SimulatedDeviceFactory::SimulatedDeviceFactory(std::string type)
00156 {
00157   this->type = type;
00158 }
00159 
00160 SimulatedDevice::SimulatedDevice(SimulatedDeviceConfig * cfg)
00161 {
00162   this->type = cfg->getType();
00163   this->name = cfg->name;
00164 }


uwsim
Author(s): Mario Prats , Javier Perez
autogenerated on Fri Aug 28 2015 13:28:58