Mission.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2007, Tarun Nimmagadda
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  *
00006  *  $Id: Mission.cc 440 2010-08-21 03:40:25Z jack.oquin $
00007  */
00008 
00009 //The University of Texas, Austin
00010 
00011 //Defines the Mission Data Structure
00012 
00013 #include <art_nav/Mission.h>
00014 
00015 //Copy Checkpoints in reverse order
00016 Mission::Mission(const MDF& mdf){
00017   int previous_id = -1;
00018   for (int i = 0; i < (int)mdf.checkpoint_ids.size(); i++){
00019     int checkpoint_id = mdf.checkpoint_ids[i];
00020     if (previous_id != checkpoint_id) 
00021       checkpoint_ids.push_back(checkpoint_id);
00022     previous_id = checkpoint_id;
00023   }
00024 };
00025 
00026 
00027 Mission::Mission(const Mission& that){
00028   checkpoint_ids = std::deque<int>(that.checkpoint_ids);
00029   checkpoint_elementid = std::deque<ElementID>(that.checkpoint_elementid);
00030 };
00031 
00032 bool Mission::compare(const Mission &that){
00033   bool ids_match = checkpoint_ids == that.checkpoint_ids;
00034   bool eids_match = true;
00035   for (uint i = 0; i < (uint) checkpoint_elementid.size(); i++)
00036     eids_match = (eids_match
00037                   && checkpoint_elementid[i] == that.checkpoint_elementid[i]);
00038   return (ids_match && eids_match);
00039 };
00040 
00041 //Populate checkpoint index field for convinience
00042 bool Mission::populate_elementid(const Graph& graph)
00043 {
00044   std::deque<int>::iterator itr;
00045   WayPointNode wpn;
00046   for(itr = checkpoint_ids.begin(); itr != checkpoint_ids.end(); itr++){
00047     for(uint i = 0; i < graph.nodes_size; i++){
00048       if (graph.nodes[i].checkpoint_id == *itr) {
00049         checkpoint_elementid.push_back(graph.nodes[i].id);
00050         break;
00051       }
00052     }
00053   }
00054   if (checkpoint_elementid.size() != checkpoint_ids.size())
00055     {
00056       ROS_WARN_STREAM("[" << checkpoint_elementid.size() << "] != ["
00057                       << checkpoint_ids.size() << "]");
00058       return false;
00059     }
00060   else
00061     return true;
00062 };
00063 
00064 bool Mission::nextPoint(){
00065   if (checkpoint_ids.empty())
00066     return false;
00067 
00068   checkpoint_ids.pop_front();
00069   checkpoint_elementid.pop_front();
00070   
00071   if (checkpoint_ids.empty())
00072     return false;
00073 
00074   return true;
00075 };
00076 
00077 int Mission::current_checkpoint_id(){
00078   if (checkpoint_ids.empty())
00079     return -1;
00080   else return checkpoint_ids.front();
00081 };
00082 
00083 int Mission::next_checkpoint_id(){
00084   int size = (int)checkpoint_ids.size();
00085   if ( size < 2)
00086     return -1;
00087   else return checkpoint_ids[1];
00088 };
00089 
00090 ElementID Mission::current_checkpoint_elementid(){
00091   if (checkpoint_elementid.empty())
00092     return ElementID();
00093   else return checkpoint_elementid.front();
00094 };
00095 
00096 ElementID Mission::next_checkpoint_elementid(){
00097   int size = (int)checkpoint_elementid.size();  
00098   if (size < 2)
00099     return ElementID();
00100   else return checkpoint_elementid[1];
00101 };
00102 
00103 void Mission::save(const char* fName){
00104   FILE* f = fopen(fName,"wb");
00105   fprintf(f, "MISSION-STATE\n");
00106   fprintf(f, "Number %ld\n", (long) checkpoint_ids.size());
00107   for (int i = 0; i < (int)checkpoint_ids.size(); i++)
00108     fprintf(f, "Id %d\n", checkpoint_ids[i]);
00109   for (int i = 0; i < (int)checkpoint_elementid.size(); i++){
00110     ElementID eid = checkpoint_elementid[i];
00111     fprintf(f, "ElementID %d.%d.%d\n", eid.seg, eid.lane, eid.pt);
00112   }
00113   fclose (f);
00114 }
00115 
00116 bool Mission::load(const char* fName, const Graph& graph)
00117 {
00118   int number_of_checkpoints = 0;
00119   std::ifstream mission_file;
00120   mission_file.open(fName);
00121   if (!mission_file){
00122     ROS_ERROR("Error in opening Mission Log file");
00123     return false;
00124   }
00125   
00126   int line_number = 0;
00127   bool valid = true;
00128   std::string lineread;
00129   while(getline(mission_file, lineread) ) // Read line by line
00130     {
00131       line_number++;
00132       std::string token;
00133       char token_char [lineread.size()+1];
00134       //Read in one line
00135       sscanf(lineread.c_str(), "%s", token_char);
00136       token.assign(token_char);
00137 
00138       if (line_number == 1){
00139         if (!(token.compare("MISSION-STATE") == 0)) return false;
00140       }
00141       
00142       else if (line_number == 2){
00143         if (!(token.compare("Number") == 0)) {return false;}
00144         else {
00145           number_of_checkpoints = 
00146             parse_integer(lineread, std::string("Number"), valid);
00147         }
00148         if (!valid) return false;
00149       }
00150       else if (token.compare("Id") == 0){
00151         int id = parse_integer(lineread, std::string("Id"), valid);
00152         if (!valid) return false;
00153         else checkpoint_ids.push_back(id);
00154       }
00155       else if (token.compare("ElementID") == 0){
00156         ElementID eid = parse_elementid(lineread, valid);
00157         if (!valid) return false;
00158         else checkpoint_elementid.push_back(eid);
00159       }
00160       else return false;
00161     }
00162   if (line_number < 4) return false;
00163   else if ((int)checkpoint_ids.size() != number_of_checkpoints) 
00164     return false;
00165   else if ((int)checkpoint_elementid.size() != number_of_checkpoints)
00166     return false;
00167   
00168   for (uint j=0; j<checkpoint_ids.size(); j++)
00169     for(uint i = 0; i < graph.nodes_size; i++){
00170       if (graph.nodes[i].checkpoint_id == checkpoint_ids.at(j)) 
00171         if (checkpoint_elementid.at(j)!=graph.nodes[i].id)
00172           {
00173             ROS_ERROR("Checkpoint has improper element ID");
00174             return false;
00175           }
00176     }
00177   
00178   return true;
00179 }
00180 
00181 void Mission::clear(){
00182   checkpoint_ids.clear();
00183   checkpoint_elementid.clear();
00184 }
00185 
00186 void Mission::print(){
00187   printf("\n MISSION: \n");
00188   std::deque<int>::iterator itr;
00189   std::deque<ElementID>::iterator eid_itr;
00190   if (checkpoint_ids.size() > 0){
00191     printf("Checkpoint IDs:");
00192     for(itr = checkpoint_ids.begin(); itr != checkpoint_ids.end()-1; itr++){
00193       printf("[%d]->", *itr); 
00194     }
00195     if (itr == checkpoint_ids.end() - 1){
00196       printf("[%d]", *itr); 
00197       printf("\n");
00198     }
00199   }
00200   else {
00201     printf("Checkpoint ID List is empty\n");  
00202   }
00203   
00204   if (checkpoint_elementid.size() > 0){
00205     printf("Checkpoint ElementID");
00206     for(eid_itr = checkpoint_elementid.begin(); 
00207         eid_itr != checkpoint_elementid.end()-1; eid_itr++){
00208       printf("[%d.%d.%d]->", eid_itr->seg, eid_itr->lane, eid_itr->pt); 
00209     }
00210     if (eid_itr == checkpoint_elementid.end() - 1){
00211       printf("[%d.%d.%d]", eid_itr->seg, eid_itr->lane, eid_itr->pt); 
00212       printf("\n");
00213     }
00214   }
00215   else {
00216     printf("Checkpoint Element ID List is empty\n");  
00217   }
00218 };
00219 
00220 ElementID parse_elementid(std::string line, bool& valid)
00221 {
00222   int seg = -1;
00223   int lane = -1;
00224   int pt = -1;
00225   if (!sscanf(line.c_str(), "ElementID %d.%d.%d", &seg, &lane, &pt))
00226     {
00227       valid=false;
00228     }
00229   return ElementID(seg, lane, pt); 
00230 };
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:42:11