labust_mission: neptus_parser.cpp Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002  * neptus_parser.cpp
00003  *
00004  *  Created on: Apr 3, 2014
00005  *      Author: Filip Mandic
00006  *
00007  ********************************************************************/
00008 
00009 /*********************************************************************
00010 * Software License Agreement (BSD License)
00011 *
00012 *  Copyright (c) 2014, LABUST, UNIZG-FER
00013 *  All rights reserved.
00014 *
00015 *  Redistribution and use in source and binary forms, with or without
00016 *  modification, are permitted provided that the following conditions
00017 *  are met:
00018 *
00019 *   * Redistributions of source code must retain the above copyright
00020 *     notice, this list of conditions and the following disclaimer.
00021 *   * Redistributions in binary form must reproduce the above
00022 *     copyright notice, this list of conditions and the following
00023 *     disclaimer in the documentation and/or other materials provided
00024 *     with the distribution.
00025 *   * Neither the name of the LABUST nor the names of its
00026 *     contributors may be used to endorse or promote products derived
00027 *     from this software without specific prior written permission.
00028 *
00029 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00030 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00031 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00032 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00033 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00034 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00035 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00036 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00037 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00038 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00039 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00040 *  POSSIBILITY OF SUCH DAMAGE.
00041 *********************************************************************/
00042 
00043 #include <labust_mission/labustMission.hpp>
00044 #include <labust_mission/maneuverGenerator.hpp>
00045 
00046 #include <sensor_msgs/NavSatFix.h>
00047 #include <geometry_msgs/TransformStamped.h>
00048 #include <misc_msgs/StartNeptusParser.h>
00049 
00050 #include <tf2_ros/transform_listener.h>
00051 #include <labust/tools/conversions.hpp>
00052 #include <labust/tools/GeoUtilities.hpp>
00053 #include <tinyxml2.h>
00054 
00055 using namespace std;
00056 using namespace tinyxml2;
00057 using namespace labust::utils;
00058 
00059 /*********************************************************************
00060  *** NeptusParser Class definition
00061  ********************************************************************/
00062 
00063 class NeptusParser{
00064 
00065 public:
00066 
00067         /*********************************************************************
00068          *** Class functions
00069          ********************************************************************/
00070 
00071         NeptusParser():startPointSet(false),
00072                                          startRelative(true),
00073                                          latLonAbs(false),
00074                                          underactuated(true),
00075                                          speed(0.5),
00076                                          heading(0.0),
00077                                          victory_radius(1.0){
00078 
00079                 offset.north = offset.east = 0;
00080                 xmlSavePath = "";
00081         }
00082 
00083         void neptusParserReset(){
00084 
00085                 startPointSet = false;
00086                 startRelative = true;
00087                 latLonAbs = false;
00088 
00089                 offset.north = offset.east = 0;
00090                 xmlSavePath = "";
00091 
00092         }
00093 
00094         int parseNeptus(string xmlFile){
00095 
00096            /* Open XML file */
00097            if(xmlDoc.LoadFile(xmlFile.c_str()) == XML_SUCCESS) {
00098 
00099                    ROS_INFO("*.nmis mission file successfully loaded.");
00100                    /* Write mission tag */
00101                    MG.writeXML.addMission();
00102 
00103                    /* Go to graph node and loop through it's children */
00104                    XMLElement *graph = xmlDoc.FirstChildElement("mission-def")->FirstChildElement("body")
00105                                                                                            ->FirstChildElement("plan")->FirstChildElement("graph");
00106 
00107                    for (XMLElement* node = graph->FirstChildElement(); node != NULL; node = node->NextSiblingElement()){
00108 
00109                            /* Check if child element is maneuver node */
00110                            if( XMLElement *maneuver = node->FirstChildElement("maneuver")){
00111 
00112                                    /* Loop through maneuver child nodes */
00113                                    for (XMLElement* maneuverType = maneuver->FirstChildElement(); maneuverType != NULL; maneuverType = maneuverType->NextSiblingElement()){
00114 
00115                                            /* Check maneuver type */
00116                                            if(strcmp(maneuverType->ToElement()->Name(),"Goto") == 0){
00117 
00118                                                    ROS_ERROR("GoTo manevar");
00119                                                    parseGoto(maneuverType);
00120 
00121                                            } else  if(strcmp(maneuverType->ToElement()->Name(),"Loiter") == 0){
00122 
00123                                                    ROS_ERROR("loiter manevar");
00124                                                    parseLoiter(maneuverType);
00125 
00126                                            } else  if(strcmp(maneuverType->ToElement()->Name(),"RowsManeuver") == 0){
00127 
00128                                                    ROS_ERROR("lawnmower manevar");
00129                                                    parseRows(maneuverType);
00130 
00131                                            } else  if(strcmp(maneuverType->ToElement()->Name(),"StationKeeping") == 0){
00132 
00133                                                    ROS_ERROR("DP manevar");
00134                                                    parseStationKeeping(maneuverType);
00135                                            }
00136                                    }
00137                            }
00138                    }
00139 
00140                    /* Save XML file */
00141                    MG.writeXML.saveXML(xmlSavePath);
00142                    return 1;
00143            } else {
00144                    ROS_ERROR("Cannot open XML file!");
00145                    return -1;
00146            }
00147         }
00148 
00149 
00150         void parseGoto(XMLElement *maneuverType){
00151 
00152                 ROS_INFO("Parsing goto maneuver...");
00153 
00154                 XMLElement *LatLonPoint = maneuverType->FirstChildElement("finalPoint")->FirstChildElement("point")->
00155                                                                                                                                            FirstChildElement("coordinate")->FirstChildElement("latitude");
00156                 /* Read maneuver parameters */
00157                 ROS_ERROR("Lat: %s", LatLonPoint->ToElement()->GetText());
00158                 string lat = LatLonPoint->ToElement()->GetText();
00159 
00160                 LatLonPoint = LatLonPoint->NextSiblingElement();
00161 
00162                 ROS_ERROR("Lon: %s", LatLonPoint->ToElement()->GetText());
00163                 string lon = LatLonPoint->ToElement()->GetText();
00164 
00165                 auv_msgs::NED position;
00166                 position = str2NED(lat,lon);
00167                 ROS_ERROR("Preracunato: %f,%f", position.north, position.east);
00168 
00169                 /* Set offset if in relative mode */
00170                 setStartPoint(position);
00171 
00172                 /* Set default values */
00173                 double duration = 0;
00174 
00175                 /* Loop through rows parameters */
00176                 for (XMLElement* param = maneuverType->FirstChildElement("duration"); param != NULL; param = param->NextSiblingElement()){
00177 
00178                         /* Check parameter name */
00179                         if( strcmp(param->ToElement()->Name(),"duration") == 0){
00180 
00181                                 duration = atof(param->ToElement()->GetText());
00182                                 ROS_ERROR("Duration: %s", param->ToElement()->GetText());
00183 
00184                         }
00185                 }
00186 
00187                 /* Write point to XML file */
00188                 if(underactuated)
00189                         MG.writeXML.addGo2point_UA(position.north-offset.north, position.east-offset.east,speed,victory_radius);
00190                 else
00191                         MG.writeXML.addGo2point_FA(position.north-offset.north, position.east-offset.east,speed,victory_radius);
00192         }
00193 
00194         void parseRows(XMLElement *maneuverType){
00195 
00196                 ROS_INFO("Parsing rows maneuver...");
00197 
00198                 XMLElement *LatLonPoint = maneuverType->FirstChildElement("basePoint")->FirstChildElement("point")->
00199                                                                                                                                    FirstChildElement("coordinate")->FirstChildElement("latitude");
00200                 /* Read rows maneuver start point parameters */
00201                 ROS_ERROR("Lat: %s", LatLonPoint->ToElement()->GetText());
00202                 string lat = LatLonPoint->ToElement()->GetText();
00203 
00204                 LatLonPoint = LatLonPoint->NextSiblingElement();
00205 
00206                 ROS_ERROR("Lon: %s", LatLonPoint->ToElement()->GetText());
00207                 string lon = LatLonPoint->ToElement()->GetText();
00208 
00209                 auv_msgs::NED position;
00210                 position = str2NED(lat,lon);
00211                 ROS_ERROR("Preracunato: %f,%f", position.north, position.east);
00212 
00213                 /* Set offset if in relative mode */
00214                 setStartPoint(position);
00215 
00216                 /* Set default values */
00217                 double width = 100;
00218                 double length = 200;
00219                 double hstep = 27;
00220                 double bearing = 0;
00221                 double alternationPercent = 100;
00222                 double curvOff = 15;
00223                 double crossAngle = 0;
00224                 bool squareCurve = true;
00225                 bool invertY = false;
00226 
00227                 /* Loop through rows parameters */
00228                 for (XMLElement* param = maneuverType->FirstChildElement("width"); param != NULL; param = param->NextSiblingElement()){
00229 
00230                         /* Check parameter name */
00231                         if( strcmp(param->ToElement()->Name(),"width") == 0){
00232 
00233                                 width = atof(param->ToElement()->GetText());
00234                                 ROS_ERROR("Width: %s", param->ToElement()->GetText());
00235 
00236                         } else if(strcmp(param->ToElement()->Name(),"length") == 0){
00237 
00238                                 length = atof(param->ToElement()->GetText());
00239                                 ROS_ERROR("length: %s", param->ToElement()->GetText());
00240 
00241                         } else if(strcmp(param->ToElement()->Name(),"hstep") == 0){
00242 
00243                                 hstep = atof(param->ToElement()->GetText());
00244                                 ROS_ERROR("Hstep: %s", param->ToElement()->GetText());
00245 
00246                         } else if(strcmp(param->ToElement()->Name(),"bearing") == 0){
00247 
00248                                 bearing = atof(param->ToElement()->GetText());
00249                                 ROS_ERROR("bearing: %s", param->ToElement()->GetText());
00250 
00251 
00252                         } else if(strcmp(param->ToElement()->Name(),"crossAngle") == 0){
00253 
00254                                 crossAngle = atof(param->ToElement()->GetText());
00255                                 ROS_ERROR("crossAngle: %s", param->ToElement()->GetText());
00256 
00257                         } else if(strcmp(param->ToElement()->Name(),"alternationPrecentage") == 0){
00258 
00259                                 alternationPercent = atof(param->ToElement()->GetText());
00260                                 ROS_ERROR("alternationPercent: %s", param->ToElement()->GetText());
00261 
00262                         } else if(strcmp(param->ToElement()->Name(),"squareCurve") == 0){
00263 
00264                                 if(strcmp(param->ToElement()->GetText(),"false")==0){
00265                                         squareCurve = false;
00266                                 } else {
00267                                         squareCurve = true;
00268                                 }
00269                                 ROS_ERROR("squareCurve: %s", param->ToElement()->GetText());
00270 
00271                         } else if(strcmp(param->ToElement()->Name(),"firstCurveRight") == 0){
00272 
00273                                 if(strcmp(param->ToElement()->GetText(),"false")==0){
00274                                         invertY = true;
00275                                 } else {
00276                                         invertY = false;
00277                                 }
00278                                 ROS_ERROR("firstCurveRight: %s", param->ToElement()->GetText());
00279 
00280                         } else if(strcmp(param->ToElement()->Name(),"curveOffset") == 0){
00281 
00282                                 curvOff = atof(param->ToElement()->GetText());
00283                                 ROS_ERROR("curveoffset: %s", param->ToElement()->GetText());
00284 
00285                         } else if(strcmp(param->ToElement()->Name(),"speed") == 0){
00286 
00287                                 if(strcmp(param->ToElement()->Attribute("unit"),"RPM") == 0) {
00288 
00289                                 ROS_ERROR("Speed units RPM. Leaving default speed: %f m/s", speed);
00290 
00291                                 } else {
00292                                         speed = atof(param->ToElement()->GetText());
00293                                         ROS_ERROR("speed: %s", param->ToElement()->GetText());
00294                                 }
00295                         }
00296                 }
00297 
00298 ROS_ERROR("width: %f, length: %f, hstep: %f, bearing: %f, alternationPercent: %f, curvOff: %f, crossAngle: %f, speed: %f, squareCurve: %d, invertY: %d", width, length, hstep, bearing, alternationPercent,curvOff, crossAngle,speed, squareCurve, invertY);
00299 
00300 
00301                 /* Generate maneuver points */
00302                 std::vector<Eigen::Vector4d> tmpPoints;
00303                 tmpPoints = MG.calcRowsPoints(width, length, hstep,
00304                                         alternationPercent/100, curvOff, squareCurve, bearing*M_PI/180,
00305                                         crossAngle*M_PI/180, invertY);
00306 
00307                 /* For each point subtract offset and add start point */
00308                 for(std::vector<Eigen::Vector4d>::iterator it = tmpPoints.begin(); it != tmpPoints.end(); ++it){
00309 
00310                         Eigen::Vector4d vTmp = *it;
00311                         vTmp[X] += -offset.north + position.north;
00312                         vTmp[Y] += -offset.east + position.east;
00313 
00314                         *it = vTmp;
00315                 }
00316 
00317                 /*** Write maneuver points to XML ***/
00318                 if(underactuated)
00319                         MG.writePrimitives(go2point_UA, tmpPoints, heading, speed, victory_radius); /* heading, speed, victoryRadius */
00320                 else
00321                         MG.writePrimitives(go2point_FA, tmpPoints, heading, speed, victory_radius); /* heading, speed, victoryRadius */
00322         }
00323 
00324         void parseStationKeeping(XMLElement *maneuverType){
00325 
00326                 ROS_INFO("Parsing StationKeeping maneuver...");
00327 
00328                 XMLElement *LatLonPoint = maneuverType->FirstChildElement("basePoint")->FirstChildElement("point")->
00329                                                                                                                                            FirstChildElement("coordinate")->FirstChildElement("latitude");
00330                 /* Read maneuver parameters */
00331                 ROS_ERROR("Lat: %s", LatLonPoint->ToElement()->GetText());
00332                 string lat = LatLonPoint->ToElement()->GetText();
00333 
00334                 LatLonPoint = LatLonPoint->NextSiblingElement();
00335 
00336                 ROS_ERROR("Lon: %s", LatLonPoint->ToElement()->GetText());
00337                 string lon = LatLonPoint->ToElement()->GetText();
00338 
00339                 auv_msgs::NED position;
00340                 position = str2NED(lat,lon);
00341                 ROS_ERROR("Preracunato: %f,%f", position.north, position.east);
00342 
00343                 /* Set offset if in relative mode */
00344                 setStartPoint(position);
00345 
00346                 /* Write point to XML file */
00347                 MG.writeXML.addDynamic_positioning(position.north-offset.north, position.east-offset.east, heading);
00348 
00349 
00350         }
00351 
00352         void parseLoiter(XMLElement *maneuverType){
00353 
00354         }
00355 
00356         /*********************************************************************
00357          *** Class helper functions
00358          ********************************************************************/
00359 
00360         auv_msgs::NED str2NED(string Lat, string Lon){
00361 
00362                 auv_msgs::NED position;
00363                 sensor_msgs::NavSatFix LatLon;
00364                 std::pair<double, double> posxy;
00365 
00366                 double DLat = atof(Lat.substr(0,2).c_str());
00367                 double MLat = atof(Lat.substr(3,2).c_str());
00368                 double SLat = atof(Lat.substr(6,17).c_str());
00369 
00370                 ROS_ERROR("DMS %f, %f, %f", DLat, MLat, SLat);
00371 
00372                 double DLon = atof(Lon.substr(0,2).c_str());
00373                 double MLon = atof(Lon.substr(3,2).c_str());
00374                 double SLon = atof(Lon.substr(6,17).c_str());
00375 
00376                 ROS_ERROR("DMS %f, %f, %f", DLon, MLon, SLon);
00377 
00378                 LatLon.latitude = DLat+MLat/60+SLat/3600;
00379             LatLon.longitude = DLon+MLon/60+SLon/3600;
00380 
00381                 ROS_ERROR("LAT LON %f, %f", LatLon.latitude, LatLon.longitude);
00382                 ROS_ERROR("Origin LAT LON %f, %f", origin.latitude, origin.longitude);
00383 
00384 
00385 
00386                 //posxy =       labust::tools::deg2meter(LatLon.latitude - startPoint.latitude, LatLon.longitude - startPoint.longitude, startPoint.longitude);
00387                 posxy = labust::tools::deg2meter(LatLon.latitude - origin.latitude, LatLon.longitude - origin.longitude, origin.latitude);
00388 
00389             position.north = posxy.first;
00390             position.east = posxy.second;
00391             position.depth = 0;
00392 
00393             return position;
00394         }
00395 
00396         void setStartPoint(auv_msgs::NED position){
00397                 if(!startPointSet){
00398                         if(latLonAbs){
00399                                 offset.north = 0;
00400                                 offset.east = 0;
00401                                 ROS_ERROR("LATLON ABS prva tocka");
00402                         }else{
00403                                 offset.north += position.north;
00404                                 offset.east += position.east;
00405                         }
00406                         startPointSet = true;
00407                 }
00408         }
00409 
00410         /*********************************************************************
00411          *** Class variables
00412          ********************************************************************/
00413 
00414         labust::maneuver::ManeuverGenerator MG;
00415         XMLDocument xmlDoc;
00416         sensor_msgs::NavSatFix startPoint;
00417 
00418         auv_msgs::DecimalLatLon origin;
00419 
00420         auv_msgs::NED offset;
00421         bool startPointSet;
00422         bool startRelative;
00423         bool latLonAbs;
00424         string xmlSavePath;
00425 
00426         /*** Mission parameters ***/
00427         bool underactuated;
00428         double speed, heading, victory_radius;
00429 };
00430 
00431 void startParseCallback(ros::Publisher &pubStartDispatcher, const misc_msgs::StartNeptusParser::ConstPtr& msg){
00432 
00433         ros::NodeHandle ph("~");
00434         NeptusParser NP;
00435         ph.param("xml_save_path", NP.xmlSavePath, NP.xmlSavePath);
00436         ROS_ERROR("%s",NP.xmlSavePath.c_str());
00437 
00438         NP.startRelative = msg->relative;
00439         NP.latLonAbs = msg->customStartFlag;
00440         NP.startPoint.latitude = msg->customStart.latitude;
00441         NP.startPoint.longitude = msg->customStart.longitude;
00442 
00443         NP.origin = msg->origin;
00444 
00445         if(NP.startRelative){
00446                 NP.offset.north = -NP.startPoint.latitude;
00447                 NP.offset.east = -NP.startPoint.longitude;
00448         } else {
00449                 NP.offset.north = NP.offset.east = 0;
00450         }
00451 
00452         NP.underactuated = msg->underactuated;
00453         NP.speed = msg->speed; // read from .nmis file
00454         NP.heading = msg->heading;
00455 
00456         NP.victory_radius = msg->victory_radius;
00457 
00458         /*** If Neptus file is successfully parsed  ***/
00459         if(NP.parseNeptus(msg->fileName) == 1){
00460                 std_msgs::String tmp;
00461                 tmp.data = "/START_DISPATCHER";
00462                 pubStartDispatcher.publish(tmp);
00463                 NP.neptusParserReset();
00464         }
00465 }
00466 
00467 int main(int argc, char** argv){
00468 
00469         ros::init(argc, argv, "neptusParser");
00470         ros::NodeHandle nh;
00471 
00472         /*** Publishers ***/
00473         ros::Publisher pubStartDispatcher = nh.advertise<std_msgs::String>("eventString",1);
00474 
00475         /*** Subscribers ***/
00476         ros::Subscriber subStartParse = nh.subscribe<misc_msgs::StartNeptusParser>("startNeptusParse",1, boost::bind(&startParseCallback, boost::ref(pubStartDispatcher), _1));
00477 
00478         ros::spin();
00479         return 0;
00480 }
00481 
00482 
00483 
00484