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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/MEdriverUtils.h

    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 utilities
    00014 //
    00015 // ME driver utils provides some simple driver functions that can be used to
    00016 //  interface uncompiled ME (Titan 1.1) and compiled ME (Titan 2.0)
    00017 //
    00018 //=============================================================================
    00019
    00020 #include <iostream>
    00021 #include <vector>
    00022 #include <Mers/Utils/Lang/String.h>
    00023
    00024 #include <Mers/Models/Base/BaseModel.h>
    00025 #include <Mers/Models/Base/BaseModelVariable.h>
    00026 #include <Mers/Models/Base/BaseModelValue.h>
    00027 #include <Mers/Models/Base/BaseModelAssignment.h>
    00028 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00029
    00030 #include "Estimate.h"
    00031 #include "Trajectories.h"
    00032 #include "ModeEstimatorBase.h"
    00033
    00034
    00035 using Mers::Models::Base::BaseModel;
    00036 using Mers::Models::Base::BaseModelVariable;
    00037 using Mers::Models::Base::BaseModelValue;
    00038 using Mers::Models::Base::BaseModelAssignment;
    00039 using Mers::Models::Base::BaseModelAssignmentArray;
    00040 using Mers::Utils::Lang::String;
    00041 using std::istream;
    00042 using std::ostream;
    00043 using std::vector;
    00044 using std::cerr;
    00045 using std::endl;
    00046
    00047
    00048 namespace Mers {
    00049         namespace Estimation {
    00050                 namespace Discrete {
    00051
    00052                         // get_directed_assign returns true if successful belief entry, false otherwise
    00053                         bool    get_directed_assign(const char ch, std::istream &stream, std::ostream &outStream,
    00054                                                 ModeEstimatorBase * me, Mers::Models::Base::BaseModel &model, const bool addTraj = false,
    00055                                                 const bool echo = false);
    00056                         bool    read_trajectory(std::istream &stream, std::ostream &outStream, ModeEstimatorBase * me,
    00057                                                 Mers::Models::Base::BaseModel &model, const bool addTraj = false, const bool echo = false);
    00058                         bool    assign(const int var, const int val, ModeEstimatorBase * me,
    00059                                                 Mers::Models::Base::BaseModel &model, std::ostream &outStream);
    00060                         bool    assign(const Mers::Utils::Lang::String var, const Mers::Utils::Lang::String val,
    00061                                                 ModeEstimatorBase * me, Mers::Models::Base::BaseModel &model, std::ostream &outStream);
    00062                         bool    variableExists(const unsigned int var, const Mers::Models::Base::BaseModel &model);
    00063                         bool    variableExists(const Mers::Utils::Lang::String var, const Mers::Models::Base::BaseModel &model);
    00064                         bool    get_assignment_pairs(std::istream &stream, std::ostream &outStream, Mers::Utils::Lang::String &str_1,
    00065                                                 Mers::Utils::Lang::String &str_2, const bool echo = false);
    00066                         bool    read_pair_of_string(std::istream &stream, std::ostream &outStream, Mers::Utils::Lang::String &str_1,
    00067                                                 Mers::Utils::Lang::String &str_2, const bool echo = false);
    00068                         void    print_estimator_variables(const char ch, const ModeEstimatorBase * me, std::ostream &outStream);
    00069                         void    eat_space_or_tab(std::istream &stream);
    00070                         void    clear_stream(std::istream &stream);
    00071                         bool    isNumeric(const Mers::Utils::Lang::String str);
    00072
    00073
    00074                         bool
    00075                         get_directed_assign(const char ch, istream &stream, ostream &outStream, ModeEstimatorBase * me, BaseModel &model, const bool addTraj, const bool echo)
    00076                         {
    00077                                 bool successFlag = true;
    00078                                 String varStr, valStr;
    00079                                 switch(ch)
    00080                                 {
    00081                                         case 'S':
    00082                                         case 's':
    00083                                                 while(get_assignment_pairs(stream, outStream, varStr, valStr, echo))
    00084                                                 {
    00085                                                         if(isNumeric(varStr) && isNumeric(valStr))
    00086                                                         {
    00087                                                                 if(!me->setCurrentState(atoi(varStr.c_str()), atoi(valStr.c_str())))
    00088                                                                         successFlag = false;
    00089                                                         }
    00090                                                         else
    00091                                                         {
    00092                                                                 if(!me->setCurrentState(varStr, valStr))
    00093                                                                         successFlag = false;
    00094                                                         }
    00095                                                 }
    00096                                                 break;
    00097                                         case 'U':
    00098                                         case 'u':
    00099                                                 while(get_assignment_pairs(stream, outStream, varStr, valStr, echo))
    00100                                                 {
    00101                                                         if(isNumeric(varStr) && isNumeric(valStr))
    00102                                                         {
    00103                                                                 if(!me->command(atoi(varStr.c_str()), atoi(valStr.c_str())))
    00104                                                                         successFlag = false;
    00105                                                         }
    00106                                                         else
    00107                                                         {
    00108                                                                 if(!me->command(varStr, valStr))
    00109                                                                         successFlag = false;
    00110                                                         }
    00111                                                 }
    00112                                                 break;
    00113                                         case 'O':
    00114                                         case 'o':
    00115                                                 while(get_assignment_pairs(stream, outStream, varStr, valStr, echo))
    00116                                                 {
    00117                                                         if(isNumeric(varStr) && isNumeric(valStr))
    00118                                                         {
    00119                                                                 if(!me->observe(atoi(varStr.c_str()), atoi(valStr.c_str())))
    00120                                                                         successFlag = false;
    00121                                                         }
    00122                                                         else
    00123                                                         {
    00124                                                                 if(!me->observe(varStr, valStr))
    00125                                                                         successFlag = false;
    00126                                                         }
    00127                                                 }
    00128                                                 break;
    00129                                         case 'B':
    00130                                         case 'b':
    00131                                                 if(!read_trajectory(stream, outStream, me, model, addTraj, echo))
    00132                                                         successFlag = false;
    00133                                                 break;
    00134                                 }
    00135                                 return successFlag;
    00136                         }
    00137
    00138                         bool
    00139                         read_trajectory(istream &stream, ostream &outStream, ModeEstimatorBase * me, BaseModel &model, const bool addTraj, const bool echo)
    00140                         {
    00141                                 char char_p[300];
    00142                                 double probability = 0.0;
    00143                                 String varStr, valStr;
    00144                                 BaseModelVariable * bmVar = NULL;
    00145                                 BaseModelValue * bmVal = NULL;
    00146                                 bool success = true;
    00147                                 unsigned int i = 0;
    00148                                 vector<bool> assignedVal;
    00149                                 Estimate * est;
    00150                                 assignedVal.resize(model.getVariables()->getLength());
    00151                                 for(i=0; i<model.getVariables()->getLength(); i++)
    00152                                         assignedVal[i] = false;
    00153
    00154                                 eat_space_or_tab(stream);
    00155                                 if(stream.peek() == '\n')
    00156                                         return false;
    00157                                 stream >> char_p;                               // Get probability
    00158                                 if(echo)
    00159                                         outStream << char_p << " ";
    00160                                 if(!atof(char_p))
    00161                                 {
    00162                                         if(echo)
    00163                                         {
    00164                                                 stream.get(char_p, 300, '\n');
    00165                                                 outStream << char_p << endl;
    00166                                         }
    00167                                         else
    00168                                                 clear_stream(stream);
    00169                                         cerr << "ERROR: Expected probability value! Skipping." << endl;
    00170                                         return false;
    00171                                 }
    00172                                 probability = atof(char_p);
    00173                                 if(probability < 0.0 || probability > 1.0)
    00174                                 {
    00175                                         cerr << "ERROR: Probability should be between [0,1] (" << probability << ")" << endl;
    00176                                         clear_stream(stream);
    00177                                         return false;
    00178                                 }
    00179                                 // read in state vars...
    00180                                 BaseModelAssignmentArray * modeAssigns = new BaseModelAssignmentArray(true);
    00181                                 while(get_assignment_pairs(stream, outStream, varStr, valStr, echo))
    00182                                 {
    00183                                         if(isNumeric(varStr) && isNumeric(valStr))      // Get UIDs
    00184                                         {
    00185                                                 if(variableExists(atoi(varStr.c_str()),model))
    00186                                                 {
    00187                                                         bmVar = model.getVariable(atoi(varStr.c_str()));
    00188                                                         bmVal = bmVar->getValue(atoi(valStr.c_str()));
    00189                                                 }
    00190                                         }
    00191                                         else
    00192                                         {
    00193                                                 if(variableExists(varStr,model))
    00194                                                 {
    00195                                                         bmVar = model.getVariable(varStr.c_str());
    00196                                                         bmVal = bmVar->getValue(valStr.c_str());
    00197                                                 }
    00198                                         }
    00199                                         if(bmVar)
    00200                                         {
    00201                                                 if(!bmVal)
    00202                                                         outStream << "WARNING: Variable assignment value not found. Ignoring." << endl;
    00203                                                 else
    00204                                                 {
    00205                                                         if(assignedVal[bmVar->getUID()])
    00206                                                                 outStream << "WARNING: Variable " << bmVar->getName() << " already assigned a value. Ignoring." << endl;
    00207                                                         else
    00208                                                         {
    00209                                                                 modeAssigns->pushBack(new BaseModelAssignment(bmVar, bmVal));
    00210                                                                 assignedVal[bmVar->getUID()] = true;
    00211                                                         }
    00212                                                 }
    00213                                         }
    00214                                         bmVar = NULL;
    00215                                         bmVal = NULL;
    00216                                 }
    00217                                 // Check to see if all state variable have values
    00218                                 success = true;
    00219                                 for(i=0; i<model.getVariables()->getLength(); i++)
    00220                                         if(model.getVariable(i)->getVariableType() == BaseModelVariable::STATE && !assignedVal[i])      // assumes location in var array is the same as UID
    00221                                                 success = false;
    00222
    00223                                 if(!success)
    00224                                 {
    00225                                         cerr << "ERROR: Not all mode variables were assigned values!" << endl;
    00226                                         return false;
    00227                                 }
    00228                                 est = new Estimate(modeAssigns, probability);
    00229                                 if(addTraj)
    00230                                         success = me->addTrajectory(est);
    00231                                 else
    00232                                         success = me->setTrajectory(est);
    00233
    00234                                 return success;
    00235                         }
    00236
    00237
    00238                         bool
    00239                         assign(const int var, const int val, ModeEstimatorBase * me, BaseModel &model, ostream &outStream)
    00240                         {
    00241                                 bool successFlag = false;
    00242                                 if(!variableExists(var, model))
    00243                                         return false;
    00244                                 const BaseModelVariable * bmVar = model.getVariable(var);
    00245                                 BaseModelVariable::VariableType type = bmVar->getVariableType();
    00246                                 if(type == BaseModelVariable::CONTROL)
    00247                                         successFlag = me->command(var, val);
    00248                                 else if(type == BaseModelVariable::OBSERVABLE)
    00249                                         successFlag = me->observe(var, val);
    00250                                 else if(type == BaseModelVariable::STATE)
    00251                                         successFlag = me->setCurrentState(var, val);
    00252                                 else
    00253                                 {
    00254                                         outStream << "Unsupported variable type. Ignoring." << endl;
    00255                                         successFlag = false;
    00256                                 }
    00257                                 return successFlag;
    00258                         }
    00259
    00260                         bool
    00261                         assign(const String var, const String val, ModeEstimatorBase * me, BaseModel &model, ostream &outStream)
    00262                         {
    00263                                 bool successFlag = false;
    00264                                 if(isNumeric(var) && isNumeric(val))                    // Check for numeric
    00265                                         return assign(atoi(var.c_str()), atoi(val.c_str()), me, model, outStream);
    00266
    00267                                 if(!variableExists(var, model))
    00268                                         return false;
    00269                                 const BaseModelVariable * bmVar = model.getVariable(var.c_str());
    00270                                 BaseModelVariable::VariableType type = bmVar->getVariableType();
    00271                                 if(type == BaseModelVariable::CONTROL)
    00272                                         successFlag = me->command(var, val);
    00273                                 else if(type == BaseModelVariable::OBSERVABLE)
    00274                                         successFlag = me->observe(var, val);
    00275                                 else if(type == BaseModelVariable::STATE)
    00276                                         successFlag = me->setCurrentState(var, val);
    00277                                 else
    00278                                 {
    00279                                         outStream << "Unsupported variable type. Ignoring." << endl;
    00280                                         successFlag = false;
    00281                                 }
    00282                                 return successFlag;
    00283                         }
    00284
    00285
    00286                         bool
    00287                         variableExists(const unsigned int var, const BaseModel &model)
    00288                         {
    00289                                 if(var >= model.getVariables()->getLength())
    00290                                 {
    00291                                         cerr << "WARNING: Variable index out of range " << var << ". Ignoring." << endl;
    00292                                         return false;
    00293                                 }
    00294                                 const BaseModelVariable * bmVar = model.getVariable(var);
    00295                                 if(bmVar == NULL)
    00296                                 {
    00297                                         cerr << "WARNING: Unable to find variable " << var << ". Ignoring." << endl;
    00298                                         return false;
    00299                                 }
    00300                                 return true;
    00301                         }
    00302
    00303                         bool
    00304                         variableExists(const String var, const BaseModel &model)
    00305                         {
    00306                                 const BaseModelVariable * bmVar = model.getVariable(var.c_str());
    00307                                 if(bmVar == NULL)
    00308                                 {
    00309                                         cerr << "WARNING: Unable to find variable " << var << ". Ignoring." << endl;
    00310                                         return false;
    00311                                 }
    00312                                 return true;
    00313                         }
    00314
    00315                         bool
    00316                         get_assignment_pairs(istream &stream, ostream &outStream, String &str_1, String &str_2, const bool echo)
    00317                         {
    00318                                 char char_p[300];
    00319
    00320                                 if(stream.peek() == '\n')
    00321                                         return false;
    00322                                 else
    00323                                 {
    00324                                         if(stream.peek() == '/')        // Start of a comment
    00325                                         {
    00326                                                 if(echo)
    00327                                                 {
    00328                                                         stream.get(char_p, 300, '\n');
    00329                                                         outStream << char_p;
    00330                                                 }
    00331                                                 clear_stream(stream);
    00332                                                 return false;
    00333                                         }
    00334                                         if(!read_pair_of_string(stream, outStream, str_1, str_2, echo))
    00335                                         {
    00336                                                 if(echo)
    00337                                                         cerr << endl;
    00338                                                 cerr << "WARNING: Expected (var = val) pairs." << endl;
    00339                                                 return false;
    00340                                         }
    00341                                 }
    00342                                 return true;
    00343                         }
    00344
    00345                         bool
    00346                         read_pair_of_string(istream &stream, ostream &outStream, String &str_1, String &str_2, const bool echo)
    00347                         {
    00348                                 char char_p[300];
    00349                                 stream >> char_p;
    00350                                 if(echo)
    00351                                         outStream << char_p << " ";
    00352                                 str_1 = char_p;
    00353                                 eat_space_or_tab(stream);
    00354                                 if (stream.peek() == '\n')
    00355                                         return false;
    00356                                 stream >> char_p;
    00357                                 if(echo)
    00358                                         outStream << char_p << " ";
    00359                                 str_2 = char_p;
    00360                                 eat_space_or_tab(stream);
    00361                                 return true;
    00362                         }
    00363
    00364                         void
    00365                         print_estimator_variables(const char ch, const ModeEstimatorBase * me, ostream &outStream)
    00366                         {
    00367                                 switch(ch)
    00368                                 {
    00369                                 case 'a':
    00370                                 case 'A':
    00371                                         me->showAllVars(outStream);
    00372                                         break;
    00373                                 case 's':
    00374                                 case 'S':
    00375                                         me->showStateVars(outStream);
    00376                                         break;
    00377                                 case 'u':
    00378                                 case 'U':
    00379                                         me->showCommandVars(outStream);
    00380                                         break;
    00381                                 case 'o':
    00382                                 case 'O':
    00383                                         me->showObservationVars(outStream);
    00384                                         break;
    00385                                 case 'd':
    00386                                 case 'D':
    00387                                         me->showDissents(outStream);
    00388                                         break;
    00389                                 case 't':
    00390                                 case 'T':
    00391                                         me->showCompiledTransitions(outStream);
    00392                                         break;
    00393                                 case 'e':
    00394                                 case 'E':
    00395                                         me->showEnabled(outStream);
    00396                                         break;
    00397                                 case 'c':
    00398                                 case 'C':
    00399                                         me->showConflicts(outStream);
    00400                                         break;
    00401                                 case 'b':
    00402                                 case 'B':
    00403                                         me->showCurrentTrajectories(outStream);
    00404                                         break;
    00405                                 }
    00406                                 return;
    00407                         }
    00408
    00409                         void
    00410                         eat_space_or_tab(istream &stream)
    00411                         {
    00412                                 while(stream.peek() == ' ' || stream.peek() == '\t')
    00413                                 {
    00414                                         stream.ignore(1);
    00415                                 }
    00416                         }
    00417
    00418                         void
    00419                         clear_stream(istream &stream)
    00420                         {
    00421                                 while(stream.peek() != '\n')
    00422                                 {
    00423                                         stream.ignore(1);
    00424                                 }
    00425                                 stream.ignore(1);
    00426                         }
    00427
    00428                         bool
    00429                         isNumeric(const String str)
    00430                         {
    00431                                 if(atoi(str.c_str()))
    00432                                         return true;
    00433                                 else if(*str.c_str() == '0')
    00434                                         return true;
    00435                                 return false;
    00436                         }
    00437                 }
    00438         }
    00439 }