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

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/DeductiveController.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 // Organization:        Model-based Embedded and Robotic Systems Group
    00010 //                                      Massachusetts Institute of Technology
    00011 //
    00013
    00014 #include <cstdio>
    00015 #include <cassert>
    00016 #include <iostream>
    00017 #include <string>
    00018 #include <Mers/Utils/Containers/intList.h>
    00019 #include <Mers/Utils/Lang/String.h>
    00020 #include <Mers/Utils/Lang/Sleep.h>
    00021 #include <Mers/Utils/Logging/logger.h>
    00022 #include <Mers/Models/CCA/CCAVariable.h>
    00023 #include <Mers/Models/CCA/CCAVariables.h>
    00024 #include <Mers/Models/CCA/CCAModel.h>
    00025 #include <Mers/Models/CCA/Io/Mof/MOFReader.h>
    00026 #include <Mers/Interfaces/ExecutionInterface.h>
    00027 #include <Mers/Estimation/Discrete/ME_connection.h>
    00028
    00029 #include <Mers/Reconfiguration/Discrete/MR_connection.h>
    00030 #include "DCInterface.h"
    00031 #include <Mers/Utils/Io/Xml/Sax/XMLLoader.h>
    00032
    00034
    00035 using namespace Mers::Executives::DeductiveController;
    00036 using Mers::Models::CCA::CCAVariables;
    00037 using Mers::Models::CCA::Io::Mof::MOFReader;
    00038 using Mers::Models::CCA::CCAVariable;
    00039 using Mers::Models::CCA::CCADomain;
    00040 using Mers::Models::Base::BaseModelVariable;
    00041 using Mers::Models::Base::BaseModelValue;
    00042 using Mers::Interfaces::ObservationInterface;
    00043 using Mers::Interfaces::ExecutionInterface;
    00044 using Mers::Utils::Containers::SharedPointer;
    00045 using Mers::Utils::Lang::String;
    00046 using Mers::Utils::Io::Xml::Sax::XMLLoader;
    00047 using Mers::Utils::Lang::sleep;
    00048 using Mers::Estimation::Discrete::ME_connection;
    00049 using Mers::Reconfiguration::Discrete::MR_connection;
    00050 using namespace std;
    00051
    00053
    00054 CCAModel *      load_MOF(char const * MOF_file_name);
    00055 void    run_Titan_shell(DCInterface *pDCInterface, CCAModel const *model_p, ME_connection * me);
    00056 void    print_Titan_shell_help(char ch);
    00057 void    printVariables(CCAVariables const * variable_p, char ch, ME_connection const * me);
    00058 void    eat_space_or_tab(istream &stream);
    00059 void    clear_stream(istream &stream);
    00060 bool    read_pair_of_int(istream &stream, int &int_1, int &int_2);
    00061
    00067 int
    00068 main(int argc, const char* argv[])
    00069 {
    00070         argc--;
    00071         argv++;
    00072
    00073         try {
    00074         log4cplus::BasicConfigurator::doConfigure();
    00075         XMLLoader::initialize();
    00076
    00077         if(!requiredParameters(argc, (argc >= 0 ? argv[-1] : NULL))) {
    00078                 return 1;
    00079         }
    00080
    00081         Arguments * args = loadParameters(argc, argv);
    00082
    00083         if( args->log4cplusFileName != NULL )
    00084                 log4cplus::PropertyConfigurator::doConfigure( args->log4cplusFileName );
    00085
    00086         cout << "Using " << args->modelFileName << " model..." << endl << endl;
    00087
    00088         // Mode Estimator instantiation
    00089         ME_connection * me_interface_p = constructME( args );
    00090         MR_connection * mr_interface_p = constructMR( args );
    00091
    00092         //mr_interface_p->setDebug(true);
    00093         SharedPointer< BaseModel > bModel( args->ccaModel );
    00094
    00095         DCInterface *   pDCInterface;
    00096         DCInterface dcInt( me_interface_p, mr_interface_p, bModel );
    00097         pDCInterface = &dcInt;
    00098
    00099         CreateChildren( args, pDCInterface );
    00100
    00101         cout << endl << "Running demo..." << endl << endl;
    00102         run_Titan_shell(pDCInterface, args->ccaModel, me_interface_p);
    00103         cout << endl << args->modelFileName << "... demo done." << endl;
    00104
    00105         sleep(1); // Give dispatchers time to finish all threads.
    00106
    00107         } catch( ::std::exception &exc ) {
    00108                 cout << exc.what() << endl;
    00109         } catch( ::std::exception * exc ) {
    00110                 cout << exc->what() << endl;
    00111         } catch( int i ) {
    00112                 cout << "Exception " << i << endl;
    00113         } catch(...) {
    00114                 cout << "An exception was thrown." << endl;
    00115         }
    00116         return 0;
    00117 }
    00118
    00119 CCAModel *
    00120 load_MOF(char const * MOF_file_name)
    00121 {
    00122         if (MOF_file_name == "")
    00123         {
    00124                 cerr << endl << "No MOF file specified." << endl;
    00125                 exit(-1);
    00126         }
    00127
    00128         CCAModel *model_p = new CCAModel();
    00129
    00130         if(MOFReader::loadMOF(MOF_file_name, *model_p))
    00131         {
    00132                 delete model_p;
    00133                 cerr << endl << "Could not load " << MOF_file_name << "." << endl;
    00134                 exit(-1);
    00135         }
    00136         return model_p;
    00137 }
    00138
    00140
    00141 void submitObservation( ME_connection * me, int oVar, int oVal )
    00142 {
    00143         me->observe( oVar, oVal );
    00144 }
    00145
    00147
    00148 void submitCommand( ME_connection * me, int cVar, int cVal )
    00149 {
    00150         me->progress( cVar, cVal );
    00151 }
    00152
    00154
    00155 void
    00156 run_Titan_shell(DCInterface *pDCInterface, CCAModel const *model_p, ME_connection * me)
    00157 {
    00158         int             var_index, val_index;
    00159         char    char_p[300];
    00160         bool    quit_shell = false;
    00161
    00162         cout << endl << "TITAN Shell: Enter 'h' for help.";
    00163         cout << endl << endl;
    00164
    00165         while (!quit_shell)
    00166         {
    00167                 cout << "> " << flush;
    00168                 eat_space_or_tab(cin);
    00169                 cin >> char_p[0];
    00170
    00171                 // 'c' or 'C' corresponds to queue command.
    00172                 if (char_p[0] == 'c' || char_p[0] == 'C')
    00173                 {
    00174                         if (read_pair_of_int(cin, var_index, val_index) == false)
    00175                         {
    00176                                 cout << "Error: 'c' must be followed by pair of integers." << endl;
    00177                         }
    00178                         else
    00179                         {
    00180                                 submitCommand( me, var_index, val_index );
    00181
    00182                                 cout << endl;
    00183                         }
    00184                 }
    00185                 // 'e' or 'E' corresponds to queue find mode estimate.
    00186                 else if (char_p[0] == 'e' || char_p[0] == 'E')
    00187                 {
    00188                         eat_space_or_tab(cin);
    00189                         if (cin.peek() == '\n')
    00190                         {
    00191                                 pDCInterface->requestFullState();
    00192                                 sleep(1);
    00193                                 cout << endl;
    00194                         }
    00195                         else
    00196                         {
    00197                                 cin >> char_p;
    00198                                 if (char_p[0] == '-' && char_p[1] == 'f')
    00199                                 {
    00200                                         pDCInterface->requestFullState();
    00201                                         sleep(1);
    00202                                         cout << endl;
    00203                                 }
    00204                                 else
    00205                                 {
    00206                                         pDCInterface->requestFullState();
    00207                                         sleep(1);
    00208                                         cout << endl;
    00209                                 }
    00210                         }
    00211                 }
    00212                 // 'g' or 'G' corresponds to queue goal.
    00213                 else if (char_p[0] == 'g' || char_p[0] == 'G')
    00214                 {
    00215                         eat_space_or_tab(cin);
    00216
    00217                         char_p[0] = cin.peek();
    00218                         if( char_p[0] == '-' ) {
    00219                                 cin.ignore();
    00220                                 char_p[0] = cin.get();
    00221                                 if( char_p[0] == 'c' ) {
    00222                                         pDCInterface->stop();
    00223                                 }
    00224
    00225                                 eat_space_or_tab(cin);
    00226                         } else {
    00227                                 if (read_pair_of_int(cin, var_index, val_index) == false)
    00228                                 {
    00229                                         cout << "Error: 'g' must be followed by pair of integers." << endl;
    00230                                 }
    00231                                 else
    00232                                 {
    00233                                         do{
    00234                                                 const BaseModelVariable * var = model_p->getVariable(var_index);
    00235                                                 const BaseModelValue * val = var->getValue(val_index);
    00236                                                 String valName(val->getName());
    00237                                                 vector<const String *> params;
    00238                                                 params.push_back(&valName);
    00239
    00240                                                 ExecutionInterface::TaskIdentifier taskID =
    00241                                                         pDCInterface->createTask(var->getName(), params);
    00242                                                 pDCInterface->start(taskID);
    00243                                                 // We should eventually stop this task, but I'm not sure when.
    00244                                                 pDCInterface->deleteTask(taskID);
    00245                                         }while(read_pair_of_int(cin, var_index, val_index));
    00246                                         cout << endl;
    00247                                 }
    00248                         }
    00249                 }
    00250                 // 'h' or 'H' or '?' corresponds to display help.
    00251                 else if (char_p[0] == 'h' || char_p[0] == 'H' || char_p[0] == '?')
    00252                 {
    00253                         eat_space_or_tab(cin);
    00254                         if (cin.peek() == '\n')
    00255                         {
    00256                                 print_Titan_shell_help('h');
    00257                                 cout << endl;
    00258                         }
    00259                         else
    00260                         {
    00261                                 cin >> char_p;
    00262                                 if (char_p[0] != '\0')
    00263                                 {
    00264                                         print_Titan_shell_help(char_p[0]);
    00265                                         cout << endl;
    00266                                 }
    00267                                 else
    00268                                 {
    00269                                         print_Titan_shell_help('h');
    00270                                         cout << endl;
    00271                                 }
    00272                         }
    00273                 }
    00274                 // 's' or 'S' corresponds to queue observation.
    00275                 else if (char_p[0] == 's' || char_p[0] == 'S')
    00276                 {
    00277                         if (read_pair_of_int(cin, var_index, val_index) == false)
    00278                         {
    00279                                 cout << "Error: 's' must be followed by pair of integers." << endl;
    00280                         }
    00281                         else
    00282                         {
    00283                                 do{
    00284                                         pDCInterface->setInitialState(var_index, val_index);
    00285                                 }while(read_pair_of_int(cin, var_index, val_index));
    00286                                 cout << endl;
    00287                         }
    00288                 }
    00289                 // 'o' or 'O' corresponds to queue observation.
    00290                 else if (char_p[0] == 'o' || char_p[0] == 'O')
    00291                 {
    00292                         if (read_pair_of_int(cin, var_index, val_index) == false)
    00293                         {
    00294                                 cout << "Error: 'o' must be followed by pair of integers." << endl;
    00295                         }
    00296                         else
    00297                         {
    00298                                 do{
    00299                                         submitObservation( me, var_index, val_index );
    00300                                 }while(read_pair_of_int(cin, var_index, val_index));
    00301                                 cout << endl;
    00302                         }
    00303                 }
    00304                 // 'q' or 'Q' corresponds to display quit.
    00305                 else if (char_p[0] == 'q' || char_p[0] == 'Q') // quit
    00306                 {
    00307                         quit_shell = true;
    00308                 }
    00309                 // 'r' or 'R' corresponds to queue observation.
    00310                 else if (char_p[0] == 'r' || char_p[0] == 'R')
    00311                 {
    00312                         // pDCInterface->requestMR();
    00313                         pDCInterface->step( true );
    00314                         sleep(1);
    00315                         cout << endl;
    00316                 }
    00317                 // 'v' or 'V' corresponds to display all variables.
    00318                 else if (char_p[0] == 'v' || char_p[0] == 'V')
    00319                 {
    00320                         eat_space_or_tab(cin);
    00321                         if (cin.peek() == '\n')
    00322                         {
    00323                                 printVariables(model_p->variables_p(), 'a', me);
    00324                                 cout << endl;
    00325                         }
    00326                         else
    00327                         {
    00328                                 cin >> char_p;
    00329                                 if (char_p[0] == '-' && char_p[1] != '\0')
    00330                                 {
    00331                                         printVariables(model_p->variables_p(), char_p[1], me);
    00332                                         cout << endl;
    00333                                 }
    00334                                 else
    00335                                 {
    00336                                         printVariables(model_p->variables_p(), 'a', me);
    00337                                         cout << endl;
    00338                                 }
    00339                         }
    00340                 }
    00341                 clear_stream(cin);
    00342         }
    00343 }
    00344
    00345 void
    00346 print_Titan_shell_help(char ch)
    00347 {
    00348         if (ch == 'c')
    00349         {
    00350                 cout << "NAME" << endl;
    00351                 cout << "\tc - queues a command to Mode Estimation" << endl << endl;
    00352                 cout << "SYNOPSIS" << endl;
    00353                 cout << "\tc (VARIABLE_INDEX = VALUE_INDEX)" << endl << endl;
    00354                 cout << "DESCRIPTION" << endl;
    00355                 cout << "\tQueues the command VARIABLE_INDEX = VALUE_INDEX to Mode Estimation."
    00356                          << endl;
    00357         }
    00358         else if (ch == 'e')
    00359         {
    00360                 cout << "NAME" << endl;
    00361                 cout << "\te - queues find_mode_estimate to Mode Estimation" << endl << endl;
    00362                 cout << "SYNOPSIS" << endl;
    00363                 cout << "\te [OPTION]" << endl << endl;
    00364                 cout << "DESCRIPTION" << endl;
    00365                 cout << "\tQueues find_mode_estimate() to Mode Estimation." << endl << endl;
    00366                 cout << "\t-f\tQueues reprot_full_mode_estimate() to Mode Estimation." << endl;
    00367         }
    00368         else if (ch == 'g')
    00369         {
    00370                 cout << "NAME" << endl;
    00371                 cout << "\tg - issues a task (goal) to Mode Reconfiguration" << endl << endl;
    00372                 cout << "SYNOPSIS" << endl;
    00373                 cout << "\tg VARIABLE_INDEX VALUE_INDEX" << endl << endl;
    00374                 cout << "DESCRIPTION" << endl;
    00375                 cout << "\tIssues the task VARIABLE_INDEX(VALUE_INDEX) (the goal (VARIABLE_INDEX = VALUE_INDEX)) to MR."
    00376                         << endl << endl;
    00377                 cout << "SYNOPSIS" << endl;
    00378                 cout << "\tg -c" << endl << endl;
    00379                 cout << "DESCRIPTION" << endl;
    00380                 cout << "\tClears all currently active tasks (goals)." << endl;
    00381         }
    00382         else if (ch == 'h')
    00383         {
    00384                 cout << "  s : sets a mode variable in the leading trajectory in ME" << endl;
    00385                 cout << "  c : queues a command to ME" << endl;
    00386                 cout << "  o : queues an observation to ME" << endl;
    00387                 cout << "  e : queues find_mode_estimate to ME" << endl;
    00388                 cout << "  g : issues a task (goal) for MR" << endl;
    00389                 cout << "  h : displays this help" << endl;
    00390                 cout << "  q : quit" << endl;
    00391                 cout << "  r : queues a reconfiguration request to MR" << endl;
    00392                 cout << "  v : displays variables" << endl;
    00393                 cout << endl;
    00394         }
    00395         else if (ch == 's')
    00396         {
    00397                 cout << "NAME" << endl;
    00398                 cout << "\ts - sets a mode variable in the leading trajectory in Mode Estimation" << endl << endl;
    00399                 cout << "SYNOPSIS" << endl;
    00400                 cout << "\ts (VARIABLE_INDEX = VALUE_INDEX)" << endl << endl;
    00401                 cout << "DESCRIPTION" << endl;
    00402                 cout << "\tSets the mode VARIABLE_INDEX = VALUE_INDEX of the leading trajectory in Mode Estimation."
    00403                          << endl;
    00404                 cout << "\t The trajectory probability is set to 1 and all other trajectories are eliminated." << endl;
    00405         }
    00406         else if (ch == 'o')
    00407         {
    00408                 cout << "NAME" << endl;
    00409                 cout << "\to - queues an observation to Mode Estimation" << endl << endl;
    00410                 cout << "SYNOPSIS" << endl;
    00411                 cout << "\to (VARIABLE_INDEX = VALUE_INDEX)" << endl << endl;
    00412                 cout << "DESCRIPTION" << endl;
    00413                 cout << "\tQueues the observation VARIABLE_INDEX = VALUE_INDEX to Mode Estimation."
    00414                          << endl;
    00415         }
    00416         else if (ch == 'q')
    00417         {
    00418                 cout << "NAME" << endl;
    00419                 cout << "\tq - quit Titan Interface test" << endl << endl;
    00420                 cout << "SYNOPSIS" << endl;
    00421                 cout << "\tq" << endl << endl;
    00422                 cout << "DESCRIPTION" << endl;
    00423                 cout << "\tQuits out of Titan Interface test."
    00424                          << endl;
    00425         }
    00426         else if (ch == 'r')
    00427         {
    00428                 cout << "NAME" << endl;
    00429                 cout << "\tr - queues a reconfiguration request to Mode Reconfiguration" << endl << endl;
    00430                 cout << "SYNOPSIS" << endl;
    00431                 cout << "\tr" << endl << endl;
    00432                 cout << "DESCRIPTION" << endl;
    00433                 cout << "\tQueues a reconfiguration request to Mode Reconfiguration."
    00434                          << endl;
    00435         }
    00436         else if (ch == 'v')
    00437         {
    00438                 cout << "NAME" << endl;
    00439                 cout << "\tv - displays the model variables and the corresponding indicies"
    00440                          << endl << endl;
    00441                 cout << "SYNOPSIS" << endl;
    00442                 cout << "\tv [OPTION]" << endl << endl;
    00443                 cout << "DESCRIPTION" << endl;
    00444                 cout << "\tDisplays the model variables and the corresponding indicies" << endl;
    00445                 cout << "\tand domain." << endl << endl;
    00446                 cout << "\t-c\tdisplays only control variables." << endl;
    00447                 cout << "\t-d\tdisplays only dependent variables." << endl;
    00448                 cout << "\t-o\tdisplays only observable variables." << endl;
    00449                 cout << "\t-s\tdisplays only state variables." << endl;
    00450         }
    00451 }
    00452
    00454
    00455 void printVariable(CCAVariable const * var)
    00456 {
    00457         cout << var->getName() << '[' << var->getUID() << "]: {";
    00458         CCADomain const &dom = *(var->getDomain());
    00459         for(unsigned int x = 0; x < dom.getLength(); x++){
    00460                 if(x != 0)
    00461                         cout << ", ";
    00462                 cout << dom[x]->getName() << '[' << dom[x]->getUID() << ']';
    00463         }
    00464         cout << "}" << endl;
    00465 }
    00466
    00468
    00469 void printVariables( CCAVariables const * pVariables,
    00470         CCAVariable::VariableType varType, bool allVars = false )
    00471 {
    00472         CCAVariables::const_iterator cIter;
    00473
    00474         for (cIter = pVariables->begin(); cIter != pVariables->end(); cIter++)
    00475         {
    00476                 if ( allVars || cIter->getVariableType() == varType )
    00477                 {
    00478                         printVariable(&(*cIter));
    00479                 }
    00480         }
    00481 }
    00482
    00484
    00485 void
    00486 printVariables(CCAVariables const *variables_p, char ch, ME_connection const * me)
    00487 {
    00488         if (ch == 'c')
    00489         {
    00490                 if( !me->printCommandVars( cout ) )
    00491                         printVariables( variables_p, CCAVariable::CONTROL );
    00492         }
    00493         else if (ch == 'd')
    00494         {
    00495                 if( !me->printDependentVars( cout ) )
    00496                         printVariables( variables_p, CCAVariable::DEPENDENT );
    00497         }
    00498         else if (ch == 'o')
    00499         {
    00500                 if( !me->printObservationVars( cout ) )
    00501                         printVariables( variables_p, CCAVariable::OBSERVABLE );
    00502         }
    00503         else if (ch == 's')
    00504         {
    00505                 if( !me->printStateVars( cout ) )
    00506                         printVariables( variables_p, CCAVariable::STATE );
    00507         }
    00508         else
    00509         {
    00510                 if( !me->printAllVars( cout ) )
    00511                         printVariables( variables_p, CCAVariable::STATE, true );
    00512         }
    00513 }
    00514
    00516
    00517 void
    00518 eat_space_or_tab(istream &stream)
    00519 {
    00520         while(stream.peek() == ' ' || stream.peek() == '\t')
    00521         {
    00522                 stream.ignore(1);
    00523         }
    00524 }
    00525
    00526 void
    00527 clear_stream(istream &stream)
    00528 {
    00529         while(stream.peek() != '\n')
    00530         {
    00531                 stream.ignore(1);
    00532         }
    00533         stream.ignore(1);
    00534 }
    00535
    00536 bool
    00537 read_pair_of_int(istream &stream, int &int_1, int &int_2)
    00538 {
    00539         while((stream.peek() < '0' || stream.peek() > '9') && stream.peek() != '\n')
    00540         {
    00541                 stream.ignore(1);
    00542         }
    00543         if (stream.peek() == '\n')
    00544         {
    00545                 return false;
    00546         }
    00547         stream >> int_1;
    00548         while((stream.peek() < '0' || stream.peek() > '9') && stream.peek() != '\n')
    00549         {
    00550                 stream.ignore(1);
    00551         }
    00552         if (stream.peek() == '\n')
    00553         {
    00554                 return false;
    00555         }
    00556         stream >> int_2;
    00557         return true;
    00558 }