mission_parser.cpp
Go to the documentation of this file.
00001 //\todo napravi da parser pamti pokazivac na zadnji poslani node koko se ne bi svaki put trebalo parsati korz cijeli xml
00002 //\todo napraviti missionParser (tj klasu koja direktno poziva primitive) klasu tako da extenda razlicite klase za parsanje (u buducnosti pojednostavljeno
00003 //prebacivanje na razlicite mission planere)
00004 //\todo Pogledja treba li poslati sve evente kao jednu poruku na poectku, kako bi se kasnije smanjila kolicina podataka koja se salje skupa s primitivom
00005 //\TODO Dodati mogucnost odabira vise evenata na koje primitiv može reagirati. (Nema potrebe za svaki slučaj nanovo definirati event)
00006 
00007 /*********************************************************************
00008  * mission_parser.cpp
00009  *
00010  *  Created on: Apr 18, 2014
00011  *      Author: Filip Mandic
00012  *
00013  ********************************************************************/
00014 
00015 /*********************************************************************
00016 * Software License Agreement (BSD License)
00017 *
00018 *  Copyright (c) 2014, LABUST, UNIZG-FER
00019 *  All rights reserved.
00020 *
00021 *  Redistribution and use in source and binary forms, with or without
00022 *  modification, are permitted provided that the following conditions
00023 *  are met:
00024 *
00025 *   * Redistributions of source code must retain the above copyright
00026 *     notice, this list of conditions and the following disclaimer.
00027 *   * Redistributions in binary form must reproduce the above
00028 *     copyright notice, this list of conditions and the following
00029 *     disclaimer in the documentation and/or other materials provided
00030 *     with the distribution.
00031 *   * Neither the name of the LABUST nor the names of its
00032 *     contributors may be used to endorse or promote products derived
00033 *     from this software without specific prior written permission.
00034 *
00035 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00036 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00037 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00038 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00039 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00040 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00041 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00042 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00043 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00044 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00045 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00046 *  POSSIBILITY OF SUCH DAMAGE.
00047 *********************************************************************/
00048 
00049 #include <labust_mission/labustMission.hpp>
00050 
00051 #include <misc_msgs/StartParser.h>
00052 #include <misc_msgs/EvaluateExpression.h>
00053 
00054 #include <tinyxml2.h>
00055 
00056 using namespace tinyxml2;
00057 
00058 namespace labust
00059 {
00060         namespace mission
00061         {
00062 
00063                 /*********************************************************************
00064                  ***  MissionParser class definition
00065                  ********************************************************************/
00066 
00067                 class MissionParser
00068                 {
00069                 public:
00070 
00071                         /*****************************************************************
00072                          ***  Class functions
00073                          ****************************************************************/
00074 
00075                         MissionParser();
00076 
00077                         ~MissionParser(){};
00078 
00079                         void sendPrimitve(uint32_t id);
00080 
00081                         uint32_t parseMission(uint32_t id);
00082 
00083                         uint32_t parseEvents();
00084 
00085                         uint32_t parseMissionParam();
00086 
00087                         void onRequestPrimitive(const std_msgs::UInt16::ConstPtr& req);
00088 
00089                         void onEventString(const std_msgs::String::ConstPtr& msg);
00090 
00091                         void onReceiveMission(const misc_msgs::StartParser::ConstPtr& msg);
00092 
00093                         /*****************************************************************
00094                          ***  Helper functions
00095                          ****************************************************************/
00096 
00097                         //void serializePrimitive(int id, vector<uint8_t> serializedData);
00098 
00099                         void onEventNextParse(XMLElement *elem2);
00100 
00101                         void resetParser();
00102 
00103                         /*****************************************************************
00104                          ***  Class variables
00105                          ****************************************************************/
00106 
00107                         int ID, lastID, eventID;
00108                         double newTimeout;
00109 
00110                         string missionEvents, missionParams;
00111 
00112                         vector<uint8_t> onEventNextActive, onEventNext;
00113 
00114                         ros::Publisher pubSendPrimitive, pubRiseEvent, pubMissionSetup;
00115                         ros::Subscriber subRequestPrimitive, subEventString, subReceiveXmlPath;
00116                         ros::ServiceClient srvExprEval;
00117 
00118                         auv_msgs::NED offset;
00119                         uint16_t breakpoint;
00120 
00122                         stringstream primitiveString;
00123 
00124                         PrimitiveParams PP;
00125 
00126                         XMLDocument xmlDoc;
00127                 };
00128 
00129 
00130                 /*****************************************************************
00131                  ***  Class functions
00132                  ****************************************************************/
00133 
00134                 MissionParser::MissionParser():ID(0),
00135                                                                                    lastID(0),
00136                                                                                    newTimeout(0),
00137                                                                                    eventID(0),
00138                                                                                    breakpoint(1),
00139                                                                                    missionEvents("")
00140                 {
00141                         ros::NodeHandle nh;
00142 
00144                         subRequestPrimitive = nh.subscribe<std_msgs::UInt16>("requestPrimitive",1,&MissionParser::onRequestPrimitive, this);
00145                         subEventString = nh.subscribe<std_msgs::String>("eventString",1,&MissionParser::onEventString, this);
00146                         subReceiveXmlPath = nh.subscribe<misc_msgs::StartParser>("startParser",1,&MissionParser::onReceiveMission, this);
00147 
00149                         pubSendPrimitive = nh.advertise<misc_msgs::SendPrimitive>("sendPrimitive",1);
00150                         pubRiseEvent = nh.advertise<std_msgs::String>("eventString",1);
00151                         pubMissionSetup = nh.advertise<misc_msgs::MissionSetup>("missionSetup",1);
00152 
00154                         srvExprEval = nh.serviceClient<misc_msgs::EvaluateExpression>("evaluate_expression");
00155                 }
00156 
00157                 void MissionParser::sendPrimitve(uint32_t id)
00158                 {
00159                         uint32_t primitive_type = parseMission(id);
00160                         ROS_ERROR("PRIMITIVE TYPE: %s", PRIMITIVES[primitive_type]);
00161 
00162                         if(primitive_type != none){
00163 
00164                                 misc_msgs::SendPrimitive sendContainer;
00165                                 sendContainer.primitiveID = primitive_type;
00166                                 //sendContainer.primitiveData = serializedData; /* Remove from msg */
00167                                 sendContainer.event.timeout = newTimeout;
00168                                 sendContainer.event.onEventNextActive = onEventNextActive;
00169                                 sendContainer.event.onEventNext = onEventNext;
00170 
00171                                 sendContainer.primitiveString.data = primitiveString.str();
00172 
00173                                 pubSendPrimitive.publish(sendContainer);
00174 
00175                         } else {
00176 
00177                                 ROS_ERROR("Mission ended.");
00178                                 std_msgs::String tmp;
00179                                 tmp.data = "/STOP"; //TODO Change to /missionEnded
00180                                 pubRiseEvent.publish(tmp);
00181                         }
00182 
00183                 }
00184 
00185                 /* Function for parsing primitives in XML mission file */
00186                 uint32_t MissionParser::parseMission(uint32_t id)
00187                 {
00188                    XMLNode *mission;
00189                    XMLNode *primitive;
00190                    XMLNode *primitiveParam;
00191 
00192                    /* Find mission node */
00193                    mission = xmlDoc.FirstChildElement("main")->FirstChildElement("mission");
00194                    if(mission)
00195                    {
00196                            /* Loop through primitive nodes */
00197                            for (primitive = mission->FirstChildElement("primitive"); primitive != NULL; primitive = primitive->NextSiblingElement())
00198                            {
00199                                    XMLElement *elem = primitive->ToElement();
00200                                    string primitiveName = elem->Attribute("name");
00201                                    ROS_INFO("%s", primitiveName.c_str());
00202 
00203                                    primitiveParam = primitive->FirstChildElement("id");
00204                                    XMLElement *elemID = primitiveParam->ToElement();
00205 
00206                                    /* If ID is correct process primitive data */
00207                                    string id_string = static_cast<ostringstream*>( &(ostringstream() << id) )->str();
00208                                    string tmp = elemID->GetText();
00209 
00210                                    if (tmp.compare(id_string) == 0)
00211                                    {
00213                                                 newTimeout = 0;
00214                                                 onEventNextActive.clear();
00215                                                 onEventNext.clear();
00216 
00217                                                 primitiveString.str(string());
00218 
00220                                                 misc_msgs::EvaluateExpression evalExpr;
00221 
00222                                                 for(int i = 1; i <= primitiveNum; ++i){
00223 
00224                                                         if(primitiveName.compare(PRIMITIVES[i]) == 0)
00225                                                         {
00226                                                            for (primitiveParam = primitive->FirstChildElement("param"); primitiveParam != NULL; primitiveParam = primitiveParam->NextSiblingElement())
00227                                                            {
00228                                                                    XMLElement *elem2 = primitiveParam->ToElement();
00229                                                                    string primitiveParamName = elem2->Attribute("name");
00230 
00231                                                                    for(std::vector<std::string>::iterator it = PP.primitive_params[i].begin(); it != PP.primitive_params[i].end(); ++it)
00232                                                                    {
00233                                                                            if(primitiveParamName.compare((*it).c_str()) == 0)
00234                                                                            {
00235                                                                                    primitiveString << (*it).c_str() <<":"<< elem2->GetText() << ":";
00236                                                                                    break;
00237                                                                            }
00238                                                                    }
00239 
00240                                                                    if(primitiveParamName.compare("onEventNext") == 0)
00241                                                                    {
00242                                                                            onEventNextParse(elem2);
00243                                                                    }
00244                                                            }
00245 
00246                                                            return i;
00247                                                         }
00248                                                 }
00249                                    }
00250                            }
00251                            return none;
00252 
00253                    }
00254                    else
00255                    {
00256                            ROS_ERROR("No mission defined");
00257                            return -1;
00258                    }
00259                 }
00260 
00261                 void MissionParser::onEventNextParse(XMLElement *elem2)
00262                 {
00263                         string primitiveNext = elem2->GetText();
00264 
00265                         if(primitiveNext.empty()==0)
00266                         {
00267                                 if(strcmp(primitiveNext.c_str(),"bkp") == 0)
00268                                 {
00269                                         onEventNext.push_back(breakpoint);
00270                                 }
00271                                 else
00272                                 {
00273                                         onEventNext.push_back(atoi(primitiveNext.c_str()));
00274                                 }
00275                         }
00276                         else
00277                         {
00278                                         onEventNext.push_back(ID+1); // provjeri teba li ovo
00279                         }
00280                         onEventNextActive.push_back(atoi(elem2->Attribute("event")));
00281                 }
00282 
00283 
00284                 /*************************************************************
00285                  *** Initial parse of XML mission file
00286                  ************************************************************/
00287 
00288                 /* Parse events on mission load */
00289                 uint32_t MissionParser::parseEvents()
00290                 {
00291 
00292                    XMLNode *events;
00293                    XMLNode *event;
00294                    XMLNode *primitiveParam;
00295 
00296                    /* Find events node */
00297                    events = xmlDoc.FirstChildElement("main")->FirstChildElement("events");
00298                    if(events)
00299                    {
00300                            for (event = events->FirstChildElement("event"); event != NULL; event = event->NextSiblingElement())
00301                            {
00302                                    //eventsContainer.push_back(event->ToElement()->GetText());
00303                                    missionEvents.append(event->ToElement()->GetText());
00304                                    missionEvents.append(":");
00305                            }
00306                    }
00307                    else
00308                    {
00309                            ROS_ERROR("No events defined");
00310                            return -1;
00311                    }
00312                    return 0;
00313                 }
00314 
00315                 /* Parse mission parameters on mission load */
00316                 uint32_t MissionParser::parseMissionParam()
00317                 {
00318                    XMLNode *events;
00319                    XMLNode *event;
00320                    XMLNode *primitiveParam;
00321 
00322                    /* Find Mission param node */
00323                    events = xmlDoc.FirstChildElement("main")->FirstChildElement("params");
00324                    if(events)
00325                    {
00326                            for (event = events->FirstChildElement("param"); event != NULL; event = event->NextSiblingElement())
00327                            {
00328                                    missionParams.append(event->ToElement()->Attribute("name"));
00329                                    missionParams.append(":");
00330                                    missionParams.append(event->ToElement()->GetText());
00331                                    missionParams.append(":");
00332                            }
00333                    }
00334                    else
00335                    {
00336                            ROS_ERROR("No mission parameters defined");
00337                            return -1;
00338                    }
00339                    return 0;
00340                 }
00341 
00342                 void MissionParser::resetParser()
00343                 {
00344                         /*** On mission stop reset variables */
00345                         ID = 0;
00346                         missionParams.clear();
00347                         missionEvents.clear();
00348                         xmlDoc.Clear();
00349                 }
00350 
00351                 /*************************************************************
00352                  *** ROS subscriptions
00353                  ************************************************************/
00354 
00355                 void MissionParser::onRequestPrimitive(const std_msgs::UInt16::ConstPtr& req)
00356                 {
00357                         if(req->data > 0)
00358                         {
00359                                 ROS_ERROR("REQUESTED PRIMITIVE ID: %d", req->data); //TODO
00360                                 sendPrimitve(req->data);
00361                         }
00362                         else
00363                         {
00364                                 ROS_FATAL("REQUESTED INVALID ID");
00365                         }
00366                 }
00367 
00368                 void MissionParser::onEventString(const std_msgs::String::ConstPtr& msg)
00369                 {
00370                         if(strcmp(msg->data.c_str(),"/STOP") == 0)
00371                         {
00372                                 resetParser();
00373                         }
00374                 }
00375 
00376                 void MissionParser::onReceiveMission(const misc_msgs::StartParser::ConstPtr& msg)
00377                 {
00378                         XMLError err_status;
00379 
00380                         if(msg->method == misc_msgs::StartParser::FILENAME)
00381                         {
00382                                 err_status = xmlDoc.LoadFile(msg->missionData.c_str());
00383                         }
00384                         else if(msg->method == misc_msgs::StartParser::STRING)
00385                         {
00386                                 err_status = xmlDoc.Parse(msg->missionData.c_str());
00387                         }
00388 
00389                         if(err_status == XML_SUCCESS)
00390                         {
00391                                 /* Set mission start offset */
00392                                 if(msg->relative)
00393                                 {
00394                                         offset.north = -msg->startPosition.north;
00395                                         offset.east = -msg->startPosition.east;
00396                                 }
00397                                 else
00398                                 {
00399                                         offset.north = offset.east = 0;
00400                                 }
00401 
00402                                 /* On XML load parse mission parameters and mission events */
00403                                 parseEvents();
00404                                 parseMissionParam();
00405 
00406                                 /* Publish mission setup */
00407                                 misc_msgs::MissionSetup missionSetup;
00408                                 missionSetup.missionEvents = missionEvents;
00409                                 missionSetup.missionParams = missionParams;
00410                                 missionSetup.missionOffset = offset;
00411                                 pubMissionSetup.publish(missionSetup);
00412 
00413                                 std_msgs::String tmp;
00414                                 tmp.data = "/START_DISPATCHER"; //TODO vidi da li to prebaciti sve na mission setup
00415                                 pubRiseEvent.publish(tmp);
00416                         }
00417                         else
00418                         {
00419                                 ROS_FATAL("INVALID MISSION PARSER REQUEST");
00420                         }
00421                 }
00422         }
00423 }
00424 
00425 /*********************************************************************
00426  ***  Main function
00427  ********************************************************************/
00428 
00429 int main(int argc, char** argv)
00430 {
00431         ros::init(argc, argv, "mission_parser");
00432         labust::mission::MissionParser MP;
00433         ros::spin();
00434         return 0;
00435 }
00436 
00437 
00438 
00439 
00440 
00441

labust_mission
Author(s): Filip Mandic
autogenerated on Fri Aug 28 2015 11:23:04