VehicleApp.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, LABUST, UNIZG-FER
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the LABUST nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *
00034 *  Author: Dula Nad
00035 *  Created: 12.02.2013.
00036 *********************************************************************/
00037 #include <labust/vehicles/VehicleApp.hpp>
00038 #include <labust/vehicles/VehicleDriver.hpp>
00039 #include <labust/vehicles/VehicleFactoryName.hpp>
00040 #include <labust/xml/GyrosReader.hpp>
00041 #include <labust/xml/GyrosWriter.hpp>
00042 
00043 using namespace labust::vehicles;
00044 
00045 VehicleApp::VehicleApp():
00046                 nhandle(),
00047                 phandle("~")
00048 {
00049         //Find plugin configuration
00050         std::string pluginName, pluginConfig, pluginId;
00051         phandle.param("PluginName",pluginName, pluginName);
00052         phandle.param("PluginConfig",pluginConfig,pluginConfig);
00053         if (pluginName.empty() || pluginConfig.empty())
00054         {
00055                 throw std::runtime_error("Plugin name and configuration missing.");
00056         }
00057         phandle.param("PluginId",pluginId,pluginId);
00058         this->loadPlugin(pluginName, pluginConfig, pluginId);
00059 
00060         //Handle publisher names.
00061         std::string stateName("state"), dataName("data");
00062         phandle.param("StateOut",stateName,stateName);
00063         phandle.param("DataOut",dataName,dataName);
00064         this->publish(stateName, dataName);
00065         //Handle subscriber names.
00066         std::string tauName("tau"), cmdName("cmd");
00067         phandle.param("TauIn",tauName,tauName);
00068         phandle.param("CommandIn",cmdName,cmdName);
00069         this->subscribe(tauName, cmdName);
00070 
00071         std::cerr<<"VehicleApp successfully configured."<<std::endl;
00072 }
00073 
00074 void VehicleApp::loadPlugin(const std::string& pluginName, const std::string& pluginConfig, const std::string& pluginId)
00075 {
00076         //Release the vehicle driver.
00077         uuv.reset();
00078         //Release and load the plugin.
00079         plugin.reset(new labust::vehicles::VehiclePlugin(pluginName,FactoryCreatorName::value));
00080         //Load the XML configuration.
00081         labust::xml::ReaderPtr reader(new labust::xml::Reader(pluginConfig,true));
00082         reader->useNode(reader->value<_xmlNode*>("//configurations"));
00083         //Instantiate the driver.
00084         uuv.reset((*plugin)(reader));
00085 }
00086 
00087 void VehicleApp::subscribe(const std::string& tauName, const std::string& cmdName)
00088 {
00089         tau = nhandle.subscribe<std_msgs::String>(tauName,msgBuf,boost::bind(&VehicleApp::onTau,this,_1));
00090         cmd = nhandle.subscribe<std_msgs::String>(tauName,msgBuf,boost::bind(&VehicleApp::onCmd,this,_1));
00091 }
00092 
00093 void VehicleApp::publish(const std::string& stateName, const std::string& dataName)
00094 {
00095         state = nhandle.advertise<std_msgs::String>(stateName,msgBuf);
00096         data = nhandle.advertise<std_msgs::String>(dataName,msgBuf);
00097 }
00098 
00099 void VehicleApp::onTau(const std_msgs::String::ConstPtr& tau)
00100 {
00101         std::cerr<<"Tau received."<<std::endl;
00102         labust::xml::GyrosReader reader(tau->data);
00103         std::map<int,double> tauM;
00104         reader.dictionary(tauM);
00105 
00106         ROS_INFO("Tau decoded:%f",tauM[labust::vehicles::tau::N]);
00107 
00108         labust::vehicles::tauMap tauC;
00109         for (size_t i=labust::vehicles::tau::X;
00110                         i<=labust::vehicles::tau::N; ++i) tauC[i] = tauM[i];
00111 
00112         ROS_INFO("Tau setting:%f",tauC[labust::vehicles::tau::N]);
00113 
00114         uuv->setTAU(tauC);
00115 
00116         std::cerr<<"Tau set."<<std::endl;
00117 
00118         labust::vehicles::stateMapPtr state(new labust::vehicles::stateMap());
00119         uuv->getState(*state);
00120         std::cerr<<"State:"<<(*state)[labust::vehicles::state::yaw]<<std::endl;
00121         //(*state)[labust::vehicles::state::x] = time;
00122         labust::xml::GyrosWriter writer(state->begin(),state->end());
00123         writer.SetTimeStamp(true);
00124         std_msgs::StringPtr str(new std_msgs::String());
00125         str->data = writer.GyrosXML();
00126         std::cerr<<"State publishing."<<std::endl;
00127         this->state.publish(str);
00128 
00129         std::cerr<<"State published."<<std::endl;
00130 }
00131 
00132 void VehicleApp::onCmd(const std_msgs::String::ConstPtr& cmd)
00133 {
00134         uuv->setCommand(cmd->data);
00135 }
00136 
00137 
00138 


labust_uvapp
Author(s): Dula Nad
autogenerated on Fri Feb 7 2014 11:36:37