- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ModeEstimation/me_driver.cpp

    00001 //=============================================================================
    00002 //
    00003 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Author:                      Oliver B. Martin
    00010 // Organization:        Model-based Embedded and Robotic Systems Group
    00011 //                                      Massachusetts Institute of Technology
    00012 //
    00013 // Mode Estimation driver
    00014 //
    00015 //=============================================================================
    00016
    00017 #ifdef _MSC_VER
    00018 #pragma warning( disable : 4786 )
    00019 #endif
    00020
    00021 #include <iostream>
    00022 #include <fstream>
    00023 #include <vector>
    00024 #include <Mers/Utils/Lang/String.h>
    00025 #include <Mers/Utils/Containers/IntegerArray.h>
    00026
    00027 #include <Mers/Models/CCA/CCAModel.h>
    00028 #include <Mers/Models/CCA/Io/Mof/MOFReader.h>
    00029
    00030 #include <Mers/Models/Base/BaseModel.h>
    00031 #include <Mers/Models/Base/BaseModelVariable.h>
    00032 #include <Mers/Models/Base/BaseModelVariableArray.h>
    00033 #include <Mers/Models/Base/BaseModelAssignment.h>
    00034 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00035
    00036 #include <Mers/Utils/Logging/logger.h>
    00037
    00038 #include "../Dissent.h"
    00039 #include "../CompiledTransition.h"
    00040 #include "../Estimate.h"
    00041 #include "../Trajectories.h"
    00042 #include "ModeEstimator.h"
    00043
    00044 #include "../MEdriverUtils.h"
    00045
    00046
    00047 using namespace Mers::Estimation::Discrete::ModeEstimation;
    00048 using namespace Mers::Estimation::Discrete;
    00049 using Mers::Models::CCA::CCAModel;
    00050 using Mers::Models::CCA::Io::Mof::MOFReader;
    00051 using std::ifstream;
    00052 using std::ofstream;
    00053 using std::istream;
    00054 using std::ostream;
    00055 using std::cin;
    00056 using std::cout;
    00057 using std::cerr;
    00058 using std::endl;
    00059 using std::flush;
    00060
    00061
    00062 // Function Prototypes
    00063 void    printUse(void);
    00064 void    run_ME_shell(ModeEstimator * me, BaseModel &model, istream &inputStream, ostream &outStream);
    00065 void    print_ME_shell_help(const char ch, ostream &outStream);
    00066
    00067
    00068
    00069 void
    00070 printUse(void)
    00071 {
    00072         cout << "Use:" << endl;
    00073         cout << "ModeEstimation_test.exe" << endl;
    00074         cout << "\t-m <CCA File (.mof)>" << endl;
    00075         cout << "\t[-b <Initialization Script File>]" << endl;
    00076         cout << "\t[-i <Input Script File>]" << endl;
    00077         cout << "\t[-o <Output File>]" << endl;
    00078         cout << "\t[-help]" << endl;
    00079         cout << "OR" << endl;
    00080         cout << "\t<CCA File>" << endl;
    00081         cout << endl;
    00082         return;
    00083 }
    00084
    00085
    00086 void
    00087 run_ME_shell(ModeEstimator * me, BaseModel &model, istream &inputStream, ostream &outStream)
    00088 {
    00089         String          var_string, val_string;
    00090         String          inStr;
    00091         char            char_p[300];
    00092         bool            quit_shell = false;
    00093         bool            echoInput = false;
    00094         unsigned int    beliefEntryNum = 0;
    00095         bool            wasBeliefEntry = false;
    00096
    00097         outStream << endl << "Titan ME Shell: Enter 'h' for help.";
    00098         outStream << endl << endl;
    00099
    00100         for(unsigned int entryNumber=1; !quit_shell && !inputStream.eof(); entryNumber++)
    00101         {
    00102                 outStream << entryNumber << "> " << flush;
    00103                 eat_space_or_tab(inputStream);
    00104
    00105                 inputStream >> char_p;
    00106                 if(echoInput)
    00107                         outStream << char_p << " ";
    00108                 inStr = char_p;
    00109
    00110                 if(isNumeric(inStr) || (inStr.size() > 1 && inStr.find('/') != 0))
    00111                 {                                                       // Asserting command/observation/state
    00112                         var_string = inStr;
    00113                         eat_space_or_tab(inputStream);
    00114                         if(inputStream.peek() != '\n')
    00115                         {
    00116                                 inputStream >> char_p;
    00117                                 if(echoInput)
    00118                                         outStream << char_p << " ";
    00119                                 val_string = char_p;
    00120                                 eat_space_or_tab(inputStream);
    00121                                 assign(var_string, val_string, me, model, outStream);
    00122                                 while(get_assignment_pairs(inputStream, outStream, var_string, val_string, echoInput))
    00123                                         assign(var_string, val_string, me, model, outStream);
    00124                         }
    00125                         if(echoInput)
    00126                                 outStream << endl;
    00127                 }
    00128                 else if(inStr.find('/') != 0)   // Input is command since there are no 1 char variable names
    00129                 {
    00130
    00131                         switch (*inStr.c_str())
    00132                         {
    00133                                 case 'a':
    00134                                 case 'A':
    00135                                         // 'a' or 'A' corresponds to assert a sequence of (var=val)
    00136                                         eat_space_or_tab(inputStream);
    00137                                         if (inputStream.peek() != '\n')
    00138                                         {
    00139                                                 if (inputStream.peek() == '-')
    00140                                                 {
    00141                                                         inputStream >> char_p;
    00142                                                         if(echoInput)
    00143                                                                 outStream << char_p << endl;
    00144                                                         if(entryNumber - beliefEntryNum == 1 && wasBeliefEntry)         // If there has only been one loop and prev entry was a belief entry
    00145                                                         {
    00146                                                                 if(get_directed_assign(char_p[1], inputStream, outStream, me, model, true, echoInput) && (char_p[1] == 'b' || char_p[1] == 'B'))
    00147                                                                         wasBeliefEntry = true;
    00148                                                         }
    00149                                                         else
    00150                                                         {
    00151                                                                 if(get_directed_assign(char_p[1], inputStream, outStream, me, model, false, echoInput) && (char_p[1] == 'b' || char_p[1] == 'B'))
    00152                                                                         wasBeliefEntry = true;
    00153                                                         }
    00154                                                         if(wasBeliefEntry)              // Reset entry number on last belief input
    00155                                                                 beliefEntryNum = entryNumber;
    00156                                                 }
    00157                                                 else
    00158                                                 {
    00159                                                         while(get_assignment_pairs(inputStream, outStream, var_string, val_string, echoInput))
    00160                                                                 assign(var_string, val_string, me, model, outStream);
    00161                                                 }
    00162                                         }
    00163                                         if(echoInput)
    00164                                                 outStream << endl;
    00165                                         break;
    00166                                 case 'e':
    00167                                 case 'E':
    00168                                         // 'e' or 'E' corresponds to estimate. '-p' progresses into the estimate
    00169                                         eat_space_or_tab(inputStream);
    00170                                         if (inputStream.peek() == '\n')
    00171                                         {
    00172                                                 if(echoInput)
    00173                                                         outStream << endl;
    00174                                                 outStream << "Calculating next trajectories..." << endl << endl;
    00175                                                 me->findModeEstimate();
    00176                                                 outStream << endl;
    00177                                                 me->showNextTrajectories(outStream);
    00178                                         }
    00179                                         else
    00180                                         {
    00181                                                 inputStream >> char_p;
    00182                                                 if(echoInput)
    00183                                                         outStream << char_p << endl;
    00184                                                 if (char_p[0] == '-' && char_p[1] == 'p')
    00185                                                 {
    00186                                                         outStream << "Calculating next trajectories..." << endl;
    00187                                                         me->progressToEstimate();
    00188                                                         outStream << "Progressing into most likely estimate..." << endl << endl;
    00189                                                         me->showCurrentTrajectories(outStream);
    00190                                                 }
    00191                                                 else
    00192                                                 {
    00193                                                         outStream << "Calculating next trajectories..." << endl;
    00194                                                         me->findModeEstimate();
    00195                                                         outStream << endl;
    00196                                                         me->showNextTrajectories(outStream);
    00197                                                 }
    00198                                         }
    00199                                         break;
    00200                                 case 'v':
    00201                                 case 'V':
    00202                                         // 'v' or 'V' corresponds to viewing data. Default is all variables
    00203                                         eat_space_or_tab(inputStream);
    00204                                         if (inputStream.peek() == '\n')
    00205                                         {
    00206                                                 if(echoInput)
    00207                                                         outStream << endl;
    00208                                                 print_estimator_variables('a', me, outStream);
    00209                                         }
    00210                                         else
    00211                                         {
    00212                                                 inputStream >> char_p;
    00213                                                 if(echoInput)
    00214                                                         outStream << char_p << endl;
    00215                                                 if (char_p[0] == '-' && char_p[1] != '\0')
    00216                                                         print_estimator_variables(char_p[1], me, outStream);
    00217                                                 else
    00218                                                         print_estimator_variables('a', me, outStream);
    00219                                         }
    00220                                         break;
    00221                                 case 'd':
    00222                                 case 'D':
    00223                                         // 'd' or 'D' sets the debug level
    00224                                         eat_space_or_tab(inputStream);
    00225                                         if(inputStream.peek() != '\n')
    00226                                         {
    00227                                                 inputStream >> char_p;
    00228                                                 if(echoInput)
    00229                                                         outStream << char_p << endl;
    00230                                                 if(isNumeric(String(char_p)))
    00231                                                 {
    00232                                                         me->setDebug(atoi(char_p));
    00233                                                         outStream << "Debug level set to " << char_p << endl;
    00234                                                 }
    00235                                                 else
    00236                                                         outStream << "Invalid debug level." << endl;
    00237                                         }
    00238                                         break;
    00239                                 case 'k':
    00240                                 case 'K':
    00241                                         // 'k' or 'K' sets the maximum number of trajectories
    00242                                         eat_space_or_tab(inputStream);
    00243                                         if(inputStream.peek() != '\n')
    00244                                         {
    00245                                                 inputStream >> char_p;
    00246                                                 if(echoInput)
    00247                                                         outStream << char_p << endl;
    00248                                                 if(isNumeric(String(char_p)))
    00249                                                 {
    00250                                                         outStream << "Setting maximum number of trajectories set to " << char_p << endl;
    00251                                                         me->setNumberTrajectories(atoi(char_p));
    00252                                                 }
    00253                                                 else
    00254                                                         outStream << "Invalid maximum number of trajectories" << endl;
    00255                                         }
    00256                                         break;
    00257                                 case 'h':
    00258                                 case 'H':
    00259                                 case '?':
    00260                                         // 'h' or 'H' or '?' corresponds to display help.
    00261                                         eat_space_or_tab(inputStream);
    00262                                         if (inputStream.peek() == '\n')
    00263                                         {
    00264                                                 if(echoInput)
    00265                                                         outStream << endl;
    00266                                                 print_ME_shell_help('h', outStream);
    00267                                         }
    00268                                         else
    00269                                         {
    00270                                                 inputStream >> char_p;
    00271                                                 if(echoInput)
    00272                                                         outStream << char_p << endl;
    00273                                                 if (char_p[0] == '-' && char_p[1] != '\0')
    00274                                                 {
    00275                                                         print_ME_shell_help(char_p[1], outStream);
    00276                                                 }
    00277                                                 else
    00278                                                 {
    00279                                                         print_ME_shell_help('h', outStream);
    00280                                                 }
    00281                                         }
    00282                                         break;
    00283                                 case 'r':
    00284                                 case 'R':
    00285                                         // 'r' or 'R' corresponds to reseting asserted variables
    00286                                         me->resetCommandsAndObservations();
    00287                                         break;
    00288                                 case 'q':
    00289                                 case 'Q':
    00290                                         // 'q' or 'Q' corresponds to quit.
    00291                                         quit_shell = true;
    00292                                         break;
    00293                                 case '#':
    00294                                         echoInput = !echoInput;
    00295                                         outStream << "Echo input: " << (echoInput ? "ON" : "OFF") << endl << endl;
    00296                                         break;
    00297                                 case '\0':
    00298                                 case '\n':
    00299                                         // a blank line - skip.
    00300                                 case '/':
    00301                                         // assumed to be a comment
    00302                                         break;
    00303                                 default:
    00304                                         outStream << "WARNING: '" << inStr << "' is not a recognized command." << endl;
    00305                                         break;
    00306                         }
    00307                         if(*inStr.c_str() != 'a' && *inStr.c_str() != 'A') // assert functions' responsible for this
    00308                                 clear_stream(inputStream);
    00309                 }
    00310                 else
    00311                 {
    00312                         if(echoInput)
    00313                         {
    00314                                 inputStream.get(char_p, 300, '\n');
    00315                                 outStream << char_p;
    00316                         }
    00317                         clear_stream(inputStream);
    00318                 }
    00319                 if(echoInput)
    00320                         outStream << endl;
    00321         }
    00322         return;
    00323 }
    00324
    00325
    00326
    00327
    00328 void
    00329 print_ME_shell_help(const char ch, ostream &outStream)
    00330 {
    00331         switch (ch)
    00332         {
    00333                 case 'a':
    00334                 case 'A':
    00335                         outStream << "NAME" << endl;
    00336                         outStream << "\ta - assert" << endl << endl;
    00337                         outStream <<    "SYNOPSIS" << endl;
    00338                         outStream << "\ta [OPTION] var val..." << endl << endl;
    00339                         outStream << "DESCRIPTION" << endl;
    00340                         outStream << "\tAssert (var = val) into ME" << endl << endl;
    00341                         outStream << "\t-s\tassert state variable assignment (trajectories truncated)" << endl;
    00342                         outStream << "\t-u\tassert command variable assignment" << endl;
    00343                         outStream << "\t-o\tassert observation variable assignment" << endl;
    00344                         outStream << "\t-b #\tassert trajectory with probability #" << endl;
    00345                         break;
    00346                 case 'e':
    00347                 case 'E':
    00348                         outStream << "NAME" << endl;
    00349                         outStream << "\te - estimate" << endl << endl;
    00350                         outStream <<    "SYNOPSIS" << endl;
    00351                         outStream << "\te [OPTION]" << endl << endl;
    00352                         outStream << "DESCRIPTION" << endl;
    00353                         outStream << "\tEstimates the next state of the system and displays the most" << endl;
    00354                         outStream << "\tlikely estimate to the screen." << endl << endl;
    00355                         outStream << "\t-e\tcalculates and displays the most likely estimate" << endl;
    00356                         outStream << "\t-p\tprogresses Titan into the most likely estimate" << endl;
    00357                         break;
    00358                 case 'r':
    00359                 case 'R':
    00360                         outStream << "NAME" << endl;
    00361                         outStream << "\tr - reset" << endl << endl;
    00362                         outStream <<    "SYNOPSIS" << endl;
    00363                         outStream << "\tr " << endl << endl;
    00364                         outStream << "DESCRIPTION" << endl;
    00365                         outStream << "\tResets all commands and observations."
    00366                                  << endl;
    00367                         break;
    00368                 case 'h':
    00369                 case 'H':
    00370                         outStream << "  var val ...            : assert (var = val) using integer UIDs or string names" << endl;
    00371                         outStream << "  a [option] var val ... : assert (var = val) for specified types and trajectories" << endl;
    00372                         outStream << "  e [option]             : estimate the next state" << endl;
    00373                         outStream << "  k #                    : sets the maximum number of trajectories" << endl;
    00374                         outStream << "  r                      : resets commands and observations" << endl;
    00375                         outStream << "  v [option]             : displays variables" << endl;
    00376                         outStream << "  h [option]             : displays this help" << endl;
    00377                         outStream << "  d #                    : sets the debug level" << endl;
    00378                         outStream << "  #                      : toggle echo input" << endl;
    00379                         outStream << "  q                      : quit" << endl;
    00380                         outStream << endl;
    00381                         break;
    00382                 case 'd':
    00383                 case 'D':
    00384                         outStream << "NAME" << endl;
    00385                         outStream << "\td - Debug level" << endl << endl;
    00386                         outStream <<    "SYNOPSIS" << endl;
    00387                         outStream << "\td #" << endl << endl;
    00388                         outStream << "DESCRIPTION" << endl;
    00389                         outStream << "\tSets the debug level to #."
    00390                                  << endl;
    00391                         break;
    00392                 case 'k':
    00393                 case 'K':
    00394                         outStream << "NAME" << endl;
    00395                         outStream << "\tk - Maximum number of trajectories" << endl << endl;
    00396                         outStream <<    "SYNOPSIS" << endl;
    00397                         outStream << "\tk #" << endl << endl;
    00398                         outStream << "DESCRIPTION" << endl;
    00399                         outStream << "\tSets the maximum number of trajectories to #."
    00400                                  << endl;
    00401                         break;
    00402                 case 'q':
    00403                 case 'Q':
    00404                         outStream << "NAME" << endl;
    00405                         outStream << "\tq - quit Titan Interface test" << endl << endl;
    00406                         outStream <<    "SYNOPSIS" << endl;
    00407                         outStream << "\tq" << endl << endl;
    00408                         outStream << "DESCRIPTION" << endl;
    00409                         outStream << "\tQuits out of Titan Interface test."
    00410                                  << endl;
    00411                         break;
    00412                 case 'v':
    00413                 case 'V':
    00414                         outStream << "NAME" << endl;
    00415                         outStream << "\tv - displays the model variables and their corresponding indicies" << endl;
    00416                         outStream << "\t\t>: current assignment, +: enabled next mode" << endl << endl;
    00417                         outStream <<    "SYNOPSIS" << endl;
    00418                         outStream << "\tv [OPTION]" << endl << endl;
    00419                         outStream << "DESCRIPTION" << endl;
    00420                         outStream << "\tDisplays the model variables and the corresponding indicies" << endl;
    00421                         outStream << "\tand domain." << endl << endl;
    00422                         outStream << "\t-a\tdisplays all model variables." << endl;
    00423                         outStream << "\t-s\tdisplays only state variables." << endl;
    00424                         outStream << "\t-u\tdisplays only control variables." << endl;
    00425                         outStream << "\t-o\tdisplays only observable variables." << endl;
    00426                         outStream << "\t-t\tdisplays all compiled transitions." << endl;
    00427                         outStream << "\t-e\tdisplays all enabled compiled transitions." << endl;
    00428                         outStream << "\t-b\tdisplays the current trajectories (t)." << endl;
    00429                         break;
    00430         }
    00431 }
    00432
    00433
    00434 int
    00435 main(int argc, char** argv)
    00436 {
    00437         argc--;
    00438         argv++;
    00439
    00440         unsigned int i;
    00441
    00442         // See if log4cplus.conf exists.
    00443         FILE * f = fopen( "log4cplus.conf", "r" );
    00444         if( f != NULL ) {
    00445                 fclose( f );
    00446                 PropertyConfigurator::doConfigure("log4cplus.conf");
    00447         } else {
    00448                 BasicConfigurator::doConfigure();
    00449         }
    00450
    00451         BaseModel * modelRef = NULL;
    00452
    00453         CCAModel myCCAmodel;
    00454         ifstream beliefIn, inFile;
    00455         ofstream outFile, nullFile;
    00456
    00457         bool modelLoaded = false;
    00458         bool initLoaded = false;
    00459         bool inFileLoaded = false;
    00460         bool outFileLoaded = false;
    00461
    00462         ModeEstimator * me = NULL;
    00463
    00464         for (i=0; ((int)i)<argc; i++)
    00465         {
    00466                 if(argv[i][0] == '-')
    00467                 {
    00468                         if (argv[i][1] == 'h' || argv[i][1] == 'H' || String(argv[i]).compare(String("-help")) == 0)
    00469                         {
    00470                                 printUse();
    00471                                 exit(0);
    00472                         // Every input argument below here should be in pairs of <option, file name>
    00473                         } else if(((int)i+1) >= argc)
    00474                         {
    00475                                 cout << "Expected file name following " << argv[i] << ". Ignoring option." << endl;
    00476                         } else if(argv[i+1][0] == '-')
    00477                         {
    00478                                 cout << "Expected file name following " << argv[i] << ". Ignoring option." << endl;
    00479                         } else if (argv[i][1] == 'm' || argv[i][1] == 'M')
    00480                         {
    00481                                 if(!modelLoaded)
    00482                                 {
    00483                                         cout << "Loading model '" << argv[i+1] << "'..." << endl;
    00484                                         if(MOFReader::loadMOF(argv[i+1], myCCAmodel))
    00485                                         {
    00486                                                 cerr << endl << "Could not load " << argv[i+1] << "." << endl;
    00487                                                 exit(-1);
    00488                                         }
    00489                                         cout << "Success!" << endl;
    00490                                         modelRef = (BaseModel *)(&myCCAmodel);
    00491                                         modelLoaded = true;
    00492                                 }
    00493                         } else if (argv[i][1] == 'b' || argv[i][1] == 'B')
    00494                         {       // This could actually be an initialization script
    00495                                 if(!initLoaded)
    00496                                 {
    00497                                         beliefIn.open(argv[i+1]);
    00498                                         if(!beliefIn)
    00499                                                 cout << "ERROR: Couldn't open ME initialization file. Ignoring." << endl;
    00500                                         else
    00501                                                 initLoaded = true;
    00502                                 }
    00503                         } else if (argv[i][1] == 'i' || argv[i][1] == 'I')
    00504                         {
    00505                                 if(!inFileLoaded)
    00506                                 {
    00507                                         inFile.open(argv[i+1]);
    00508                                         if(!inFile)
    00509                                                 cout << "ERROR: Couldn't open input script. Ignoring." << endl;
    00510                                         else
    00511                                                 inFileLoaded = true;
    00512                                 }
    00513                         } else if (argv[i][1] == 'o' || argv[i][1] == 'O')
    00514                         {
    00515                                 if(!outFileLoaded)
    00516                                 {
    00517                                         outFile.open(argv[i+1]);
    00518                                         if(!outFile)
    00519                                                 cout << "ERROR: Couldn't open output file. Ignoring." << endl;
    00520                                         else
    00521                                                 outFileLoaded = true;
    00522                                 }
    00523                         } else
    00524                         {
    00525                                 cerr << "Unrecognized option '" << argv[i] << "'." << endl;
    00526                                 printUse();
    00527                         }
    00528                 }
    00529         }
    00530
    00531         if(argc == 1)
    00532         {
    00533                 cout << "Loading model '" << argv[0] << "'..." << endl;
    00534                 if(MOFReader::loadMOF(argv[0], myCCAmodel))
    00535                 {
    00536                         cerr << endl << "Could not load " << argv[0] << "." << endl;
    00537                         exit(-1);
    00538                 }
    00539                 cout << "Success!" << endl;
    00540                 modelRef = (BaseModel *)(&myCCAmodel);
    00541                 modelLoaded = true;
    00542         }
    00543
    00544         BaseModel &model = *modelRef;
    00545
    00546         if(!modelLoaded)
    00547         {
    00548                 printUse();
    00549                 exit(-1);
    00550         }
    00551
    00552         me = new ModeEstimator(myCCAmodel);
    00553
    00554         if(initLoaded)
    00555         {
    00556                 cout << "Loading initialization script... ";
    00557                 run_ME_shell(me, model, beliefIn, nullFile);
    00558                 beliefIn.close();
    00559                 cout << "done." << endl;
    00560         }
    00561         // Choose shell depending on inputs and outputs
    00562         if(inFileLoaded && outFileLoaded)
    00563         {
    00564                 run_ME_shell(me, model, inFile, outFile);
    00565                 inFile.close();
    00566                 outFile.close();
    00567         }
    00568         else if(inFileLoaded && !outFileLoaded)
    00569         {
    00570                 run_ME_shell(me, model, inFile, cout);
    00571                 inFile.close();
    00572         }
    00573         else if(!inFileLoaded && outFileLoaded)
    00574         {
    00575                 run_ME_shell(me, model, cin, outFile);
    00576                 outFile.close();
    00577         }
    00578         else
    00579                 run_ME_shell(me, model, cin, cout);
    00580
    00581         delete me;
    00582         return 0;
    00583 }
    00584