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

# /Users/fpy/titan-1-5-MBARI/Mers/Planning/Reactive/CCA/test.cpp

    00001 //=============================================================================
    00002 //
    00003 // Copyright (c) 2001           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:                      Seung H. Chung
    00010 // Organization:        Model-based Embedded and Robotic Systems Group
    00011 //                                      Massachusetts Institute of Technology
    00012 //
    00013 //
    00014 //
    00015 //=============================================================================
    00016
    00017 #ifdef _MSC_VER
    00018 #pragma warning( disable : 4786 )
    00019 #endif
    00020
    00021 #include <cstdlib>
    00022 #include <cassert>
    00023 #include <iostream>
    00024 #include <Mers/Utils/Containers/intList.h>
    00025 #include <Mers/Models/CCA/CCAModel.h>
    00026 #include <Mers/Models/CCA/Io/Mof/MOFReader.h>
    00027 #include "reactivePlanner.h"
    00028 #include "rpInterface.h"
    00029 using namespace std;
    00030 using namespace Mers::Models::CCA;
    00031 using namespace Mers::Models::CCA::Io::Mof;
    00032 using namespace Mers::Utils::Containers;
    00033 using namespace Mers::Models::Base;
    00034 using namespace Mers::Utils::Lang;
    00035
    00036 CCAModel *      load_MOF(String MOF_file_name);
    00037 void    run_ReactivePlanner_shell(rpInterface *rp_interface_p, CCAModel const *model_p);
    00038 void    print_ReactivePlanner_shell_help(char ch);
    00039 void    print_variables(CCAVariables const * variable_p, char ch);
    00040 void    eat_space_or_tab(istream &stream);
    00041 void    clear_stream(istream &stream);
    00042 bool    read_pair_of_int(istream &stream, int &int_1, int &int_2);
    00043 void    valve_driver_and_valve_test();
    00044
    00045 int
    00046 main(int argc, char** argv)
    00047 {
    00048         argc--;
    00049         argv++;
    00050
    00051         CCAModel *model_p;
    00052         //String MOF_file_name = "../../Model/MOFReader/propulsion.mof";
    00053         String MOF_file_name = "../../../../Mers/Models/CCA/Io/Mof/switch.mof";
    00054
    00055         if (argc > 0)  // mof test file given...
    00056         {
    00057                 model_p = load_MOF(String(argv[0]));
    00058                 MOF_file_name = String(argv[0]);
    00059         }
    00060         else
    00061         {
    00062                 model_p = load_MOF(MOF_file_name);
    00063         }
    00064         cout << "Using " << MOF_file_name << " model..." << endl << endl;
    00065
    00066         reactivePlanner *       reactive_planner_p      = new reactivePlanner(*model_p);
    00067         rpInterface     *               rp_interface_p          = new rpInterface(*reactive_planner_p);
    00068
    00069         cout << endl << "Running demo..." << endl << endl;
    00070         run_ReactivePlanner_shell(rp_interface_p, model_p);
    00071         cout << endl << MOF_file_name << "... demo done." << endl;
    00072
    00073         assert(rp_interface_p != NULL);
    00074         delete rp_interface_p;
    00075
    00076         assert(reactive_planner_p != NULL);
    00077         delete reactive_planner_p;
    00078
    00079         assert(model_p != NULL);
    00080         delete model_p;
    00081
    00082         exit(0);
    00083         return 0;
    00084 }
    00085
    00086 CCAModel *
    00087 load_MOF(String MOF_file_name)
    00088 {
    00089         CCAModel *model_p = new CCAModel();
    00090         if (MOF_file_name == "")
    00091         {
    00092                 delete model_p;
    00093                 cerr << endl << "No MOF file specified." << endl;
    00094                 exit(-1);
    00095         }
    00096         else if(MOFReader::loadMOF(MOF_file_name.c_str(), *model_p))
    00097         {
    00098                 delete model_p;
    00099                 cerr << endl << "Could not load " << MOF_file_name << "." << endl;
    00100                 exit(-1);
    00101         }
    00102         return model_p;
    00103 }
    00104
    00105 void
    00106 run_ReactivePlanner_shell(rpInterface *rp_interface_p, CCAModel const *model_p)
    00107 {
    00108         int             var_index, val_index;
    00109         char    char_p[300];
    00110         bool    quit_shell = false;
    00111
    00112         cout << endl << "REACTIVE PLANNER Shell: Enter 'h' for help.";
    00113         cout << endl << endl;
    00114
    00115         while (!quit_shell)
    00116         {
    00117                 cout << "> " << flush;
    00118                 eat_space_or_tab(cin);
    00119                 cin.get(char_p, 2);
    00120
    00121                 // 'c' or 'C' corresponds to update current state.
    00122                 if (char_p[0] == 'c' || char_p[0] == 'C')
    00123                 {
    00124                         if (read_pair_of_int(cin, var_index, val_index) == false)
    00125                         {
    00126                                 cout << "Error: 'c' must be followed by pair of integers." << endl;
    00127                         }
    00128                         else
    00129                         {
    00130                                 rp_interface_p->update_current_state(var_index, val_index);
    00131                         }
    00132                 }
    00133                 // 'g' or 'G' corresponds to update goal state.
    00134                 else if (char_p[0] == 'g' || char_p[0] == 'G')
    00135                 {
    00136                         if (read_pair_of_int(cin, var_index, val_index) == false)
    00137                         {
    00138                                 cout << "Error: 'g' must be followed by pair of integers." << endl;
    00139                         }
    00140                         else
    00141                         {
    00142                                 rp_interface_p->update_goal_state(var_index, val_index);
    00143                         }
    00144                 }
    00145                 // 'h' or 'H' or '?' corresponds to display help.
    00146                 else if (char_p[0] == 'h' || char_p[0] == 'H' || char_p[0] == '?')
    00147                 {
    00148                         eat_space_or_tab(cin);
    00149                         if (cin.peek() == '\n')
    00150                         {
    00151                                 print_ReactivePlanner_shell_help('h');
    00152                         }
    00153                         else
    00154                         {
    00155                                 cin >> char_p;
    00156                                 if (char_p[0] != '\0')
    00157                                 {
    00158                                         print_ReactivePlanner_shell_help(char_p[0]);
    00159                                 }
    00160                                 else
    00161                                 {
    00162                                         print_ReactivePlanner_shell_help('h');
    00163                                 }
    00164                         }
    00165                 }
    00166                 // 'n' or 'N' corresponds to generate next commands.
    00167                 else if (char_p[0] == 'n' || char_p[0] == 'N')
    00168                 {
    00169                         cout << "Command: " << *rp_interface_p->generate_next_command_intList_p();
    00170                         cout << endl;
    00171                 }
    00172                 // 'q' or 'Q' corresponds to quit.
    00173                 else if (char_p[0] == 'q' || char_p[0] == 'Q') // quit
    00174                 {
    00175                         quit_shell = true;
    00176                 }
    00177                 // 'r' or 'R' corresponds to reachable states.
    00178                 else if (char_p[0] == 'r' || char_p[0] == 'R')
    00179                 {
    00180                         cout << "Reachable Modes: " << *rp_interface_p->reachable_state_intList_p();
    00181                         cout << endl;
    00182                 }
    00183                 // 'v' or 'V' corresponds to display all variables.
    00184                 else if (char_p[0] == 'v' || char_p[0] == 'V')
    00185                 {
    00186                         eat_space_or_tab(cin);
    00187                         if (cin.peek() == '\n')
    00188                         {
    00189                                 print_variables(model_p->variables_p(), 'a');
    00190                                 cout << endl;
    00191                         }
    00192                         else
    00193                         {
    00194                                 cin >> char_p;
    00195                                 if (char_p[0] == '-' && char_p[1] != '\0')
    00196                                 {
    00197                                         print_variables(model_p->variables_p(), char_p[1]);
    00198                                         cout << endl;
    00199                                 }
    00200                                 else
    00201                                 {
    00202                                         print_variables(model_p->variables_p(), 'a');
    00203                                         cout << endl;
    00204                                 }
    00205                         }
    00206                 }
    00207                 clear_stream(cin);
    00208         }
    00209 }
    00210
    00211 void
    00212 print_ReactivePlanner_shell_help(char ch)
    00213 {
    00214         if (ch == 'c')
    00215         {
    00216                 cout << "NAME" << endl;
    00217                 cout << "\tc - assert current state" << endl << endl;
    00218                 cout << "SYNOPSIS" << endl;
    00219                 cout << "\tc (VARIABLE_INDEX = VALUE_INDEX)" << endl << endl;
    00220                 cout << "DESCRIPTION" << endl;
    00221                 cout << "\tAssert state VARIABLE_INDEX = VALUE_INDEX as the current state."
    00222                          << endl;
    00223         }
    00224         else if (ch == 'g')
    00225         {
    00226                 cout << "NAME" << endl;
    00227                 cout << "\tg - assert goal state" << endl << endl;
    00228                 cout << "SYNOPSIS" << endl;
    00229                 cout << "\tg (VARIABLE_INDEX = VALUE_INDEX)" << endl << endl;
    00230                 cout << "DESCRIPTION" << endl;
    00231                 cout << "\tAsserts goal VARIABLE_INDEX = VALUE_INDEX as the goal state."
    00232                          << endl;
    00233         }
    00234         else if (ch == 'h')
    00235         {
    00236                 cout << "  c : assert current state" << endl;
    00237                 cout << "  g : assert goal state" << endl;
    00238                 cout << "  h : displays this help" << endl;
    00239                 cout << "  n : generate next commands" << endl;
    00240                 cout << "  q : quit" << endl;
    00241                 cout << "  r : display reachable states" << endl;
    00242                 cout << "  v : display variables" << endl;
    00243                 cout << endl;
    00244         }
    00245         else if (ch == 'n')
    00246         {
    00247                 cout << "NAME" << endl;
    00248                 cout << "\tn - generate next commands" << endl << endl;
    00249                 cout << "SYNOPSIS" << endl;
    00250                 cout << "\tn" << endl << endl;
    00251                 cout << "DESCRIPTION" << endl;
    00252                 cout << "\tGenerates next commands from current and goal states."
    00253                          << endl;
    00254         }
    00255         else if (ch == 'q')
    00256         {
    00257                 cout << "NAME" << endl;
    00258                 cout << "\tq - quit Titan Interface test" << endl << endl;
    00259                 cout << "SYNOPSIS" << endl;
    00260                 cout << "\tq" << endl << endl;
    00261                 cout << "DESCRIPTION" << endl;
    00262                 cout << "\tQuits out of Titan Interface test."
    00263                          << endl;
    00264         }
    00265         else if (ch == 'r')
    00266         {
    00267                 cout << "NAME" << endl;
    00268                 cout << "\tr - display reachable states" << endl << endl;
    00269                 cout << "SYNOPSIS" << endl;
    00270                 cout << "\tr" << endl << endl;
    00271                 cout << "DESCRIPTION" << endl;
    00272                 cout << "\tDisplays reachable states."
    00273                          << endl;
    00274         }
    00275         else if (ch == 'v')
    00276         {
    00277                 cout << "NAME" << endl;
    00278                 cout << "\tv - displays the model variables and the corresponding indicies"
    00279                          << endl << endl;
    00280                 cout << "SYNOPSIS" << endl;
    00281                 cout << "\tv [OPTION]" << endl << endl;
    00282                 cout << "DESCRIPTION" << endl;
    00283                 cout << "\tDisplays the model variables and the corresponding indicies" << endl;
    00284                 cout << "\tand domain." << endl << endl;
    00285                 cout << "\t-c\tdisplays only control variables." << endl;
    00286                 cout << "\t-d\tdisplays only dependent variables." << endl;
    00287                 cout << "\t-o\tdisplays only observable variables." << endl;
    00288                 cout << "\t-s\tdisplays only state variables." << endl;
    00289         }
    00290 }
    00291
    00292 void
    00293 print_variables(CCAVariables const *variables_p, char ch)
    00294 {
    00295         CCAVariables::const_iterator const_it;
    00296
    00297         if (ch == 'c')
    00298         {
    00299                 for (const_it = variables_p->begin(); const_it != variables_p->end(); const_it++)
    00300                 {
    00301                         if (const_it->getVariableType() == CCAVariable::CONTROL)
    00302                         {
    00303                                 cout << *const_it << endl;
    00304                         }
    00305                 }
    00306         }
    00307         else if (ch == 'd')
    00308         {
    00309                 for (const_it = variables_p->begin(); const_it != variables_p->end(); const_it++)
    00310                 {
    00311                         if (const_it->getVariableType() == CCAVariable::DEPENDENT)
    00312                         {
    00313                                 cout << *const_it << endl;
    00314                         }
    00315                 }
    00316         }
    00317         else if (ch == 'o')
    00318         {
    00319                 for (const_it = variables_p->begin(); const_it != variables_p->end(); const_it++)
    00320                 {
    00321                         if (const_it->getVariableType() == CCAVariable::OBSERVABLE)
    00322                         {
    00323                                 cout << *const_it << endl;
    00324                         }
    00325                 }
    00326         }
    00327         else if (ch == 's')
    00328         {
    00329                 for (const_it = variables_p->begin(); const_it != variables_p->end(); const_it++)
    00330                 {
    00331                         if (const_it->getVariableType() == CCAVariable::STATE)
    00332                         {
    00333                                 cout << *const_it << endl;
    00334                         }
    00335                 }
    00336         }
    00337         else
    00338         {
    00339                 cout << *variables_p << endl;
    00340         }
    00341 }
    00342
    00343 void
    00344 eat_space_or_tab(istream &stream)
    00345 {
    00346         while(stream.peek() == ' ' || stream.peek() == '\t')
    00347         {
    00348                 stream.ignore(1);
    00349         }
    00350 }
    00351
    00352 void
    00353 clear_stream(istream &stream)
    00354 {
    00355         while(stream.peek() != '\n')
    00356         {
    00357                 stream.ignore(1);
    00358         }
    00359         stream.ignore(1);
    00360 }
    00361
    00362 bool
    00363 read_pair_of_int(istream &stream, int &int_1, int &int_2)
    00364 {
    00365         while(stream.peek() < '0' || stream.peek() > '9')
    00366         {
    00367                 stream.ignore(1);
    00368         }
    00369         if (stream.peek() == '\n')
    00370         {
    00371                 return false;
    00372         }
    00373         stream >> int_1;
    00374         while(stream.peek() < '0' || stream.peek() > '9')
    00375         {
    00376                 stream.ignore(1);
    00377         }
    00378         if (stream.peek() == '\n')
    00379         {
    00380                 return false;
    00381         }
    00382         stream >> int_2;
    00383         return true;
    00384 }
    00385
    00386 void
    00387 valve_driver_and_valve_test()
    00388 {
    00389         cout << "This is a valve driver and valve test program for the Reactive Planner." << std::endl;
    00390
    00391         CCAValues               temp_values(true);
    00392         //CCAAssignment temp_assignment;
    00393 //      const assignments * temp_assignments_p;
    00394 //      rpCCAutomata::rpResultType result;
    00395         CCAClause               temp_c_clause(CCAClause::CONJUNCTIVE);
    00396         CCAClause               temp_d_clause(CCAClause::DISJUNCTIVE);
    00397         CCASentence     temp_constraint(CCASentence::CNF);
    00398
    00399         CCAModel my_model("Test Model");
    00400
    00401         // Domain
    00402         temp_values.insert_value(CCAValue("turn on"));
    00403         temp_values.insert_value(CCAValue("turn off"));
    00404         temp_values.insert_value(CCAValue("reset"));
    00405         temp_values.insert_value(CCAValue("close valve"));
    00406         temp_values.insert_value(CCAValue("open valve"));
    00407         my_model.insert_domain("Driver-CMD", temp_values);
    00408         temp_values.makeEmpty();
    00409
    00410         // Domain
    00411         temp_values.insert_value(CCAValue("on"));
    00412         temp_values.insert_value(CCAValue("off"));
    00413         temp_values.insert_value(CCAValue("resettable"));
    00414         temp_values.insert_value(CCAValue("failed"));
    00415         my_model.insert_domain("Driver", temp_values);
    00416         temp_values.makeEmpty();
    00417
    00418
    00419         // Domain
    00420         temp_values.insert_value(CCAValue("open"));
    00421         temp_values.insert_value(CCAValue("closed"));
    00422         temp_values.insert_value(CCAValue("stuck-open"));
    00423         temp_values.insert_value(CCAValue("stuck-closed"));
    00424         my_model.insert_domain("Valve", temp_values);
    00425         temp_values.makeEmpty();
    00426
    00427         // Variables
    00428         my_model.insert_variable("Command.CMD1", CCAVariable::CONTROL, "Driver-CMD");
    00429         my_model.insert_variable("Driver.D1", CCAVariable::STATE, "Driver", 1);
    00430         my_model.insert_variable("Valve.V1", CCAVariable::STATE, "Valve", 0);
    00431
    00432
    00433         // Transitions
    00434         CCAAssignment * temp_assignment =
    00435                 my_model.getAssignment("Command.CMD1", "turn on", CCAAssignment::POSITIVE);
    00436         temp_c_clause.pushBack(temp_assignment);
    00437         my_model.insert_transition("Driver.D1", "off", "on", temp_c_clause, 1);
    00438         temp_c_clause.makeEmpty();
    00439         temp_assignment =
    00440                 my_model.getAssignment("Command.CMD1", "turn off", CCAAssignment::POSITIVE);
    00441         temp_c_clause.pushBack(temp_assignment);
    00442         my_model.insert_transition("Driver.D1", "on", "off", temp_c_clause, 1);
    00443         temp_c_clause.makeEmpty();
    00444         temp_assignment =
    00445                 my_model.getAssignment("Command.CMD1", "reset", CCAAssignment::POSITIVE);
    00446         temp_c_clause.pushBack(temp_assignment);
    00447         my_model.insert_transition("Driver.D1", "resettable", "on", temp_c_clause, 1);
    00448         temp_c_clause.makeEmpty();
    00449         my_model.insert_transition("Driver.D1", "on", "resettable", temp_c_clause, 1);
    00450         my_model.insert_transition("Driver.D1", "on", "failed", temp_c_clause, 1);
    00451         my_model.insert_transition("Driver.D1", "off", "failed", temp_c_clause, 1);
    00452         my_model.insert_transition("Driver.D1", "resettable", "failed", temp_c_clause, 1);
    00453
    00454         temp_assignment = my_model.getAssignment("Command.CMD1",
    00455                 "close valve", CCAAssignment::POSITIVE);
    00456         temp_c_clause.pushBack(temp_assignment);
    00457         temp_assignment = my_model.getAssignment("Driver.D1",
    00458                 "on", CCAAssignment::POSITIVE);
    00459         temp_c_clause.pushBack(temp_assignment);
    00460         my_model.insert_transition("Valve.V1", "open", "closed", temp_c_clause, 1);
    00461         temp_c_clause.makeEmpty();
    00462         temp_assignment = my_model.getAssignment("Command.CMD1",
    00463                 "open valve", CCAAssignment::POSITIVE);
    00464         temp_c_clause.pushBack(temp_assignment);
    00465         temp_assignment = my_model.getAssignment("Driver.D1",
    00466                 "on", CCAAssignment::POSITIVE);
    00467         temp_c_clause.pushBack(temp_assignment);
    00468         my_model.insert_transition("Valve.V1", "closed", "open", temp_c_clause, 1);
    00469         temp_c_clause.makeEmpty();
    00470         my_model.insert_transition("Valve.V1", "open", "stuck-open", temp_c_clause, 1);
    00471         my_model.insert_transition("Valve.V1", "closed", "stuck-closed", temp_c_clause, 1);
    00472
    00473         // Set state type
    00474         my_model.set_state_type("Driver.D1", "on", CCAState::NOMINAL);
    00475         my_model.set_state_type("Driver.D1", "off", CCAState::NOMINAL);
    00476         my_model.set_state_type("Driver.D1", "resettable", CCAState::FAILURE);
    00477         my_model.set_state_type("Driver.D1", "failed", CCAState::FAILURE);
    00478
    00479         my_model.set_state_type("Valve.V1", "open", CCAState::NOMINAL);
    00480         my_model.set_state_type("Valve.V1", "closed", CCAState::NOMINAL);
    00481         my_model.set_state_type("Valve.V1", "stuck-open", CCAState::FAILURE);
    00482         my_model.set_state_type("Valve.V1", "stuck-closed", CCAState::FAILURE);
    00483
    00484 //      cout << my_model;
    00485
    00486         reactivePlanner my_reactivePlanner(my_model);
    00487 //      std::cout << my_rpCCAutomata << endl;
    00488
    00489         rpInterface my_rpInterface(my_reactivePlanner);
    00490         std::cout << my_rpInterface << std::endl;
    00491
    00492 /*      my_rpCCAutomata.set_current_rpState(0,0);
    00493         my_rpCCAutomata.set_current_rpState(1,1);
    00494         my_rpCCAutomata.update_status(true);
    00495
    00496         std::cout << my_rpCCAutomata << endl;
    00497
    00498         my_rpCCAutomata.set_goal_rpState(0,1);
    00499         my_rpCCAutomata.set_goal_rpState(1,1);
    00500
    00501         temp_assignments_p = my_rpCCAutomata.generate_next_command_assignments_p(result);
    00502         std::cout << result << endl;
    00503         if (temp_assignments_p != NULL) {
    00504                 cout << *temp_assignments_p << endl;
    00505         }
    00506
    00507         my_rpCCAutomata.set_current_rpState(0,0);
    00508         my_rpCCAutomata.set_current_rpState(1,0);
    00509         temp_assignments_p = my_rpCCAutomata.generate_next_command_assignments_p(result);
    00510         std::cout << result << endl;
    00511         if (temp_assignments_p != NULL) {
    00512                 cout << *temp_assignments_p << endl;
    00513         }
    00514
    00515         my_rpCCAutomata.set_current_rpState(0,1);
    00516         my_rpCCAutomata.set_current_rpState(1,0);
    00517         temp_assignments_p = my_rpCCAutomata.generate_next_command_assignments_p(result);
    00518         std::cout << result << endl;
    00519         if (temp_assignments_p != NULL) {
    00520                 cout << *temp_assignments_p << endl;
    00521         }
    00522
    00523         my_rpCCAutomata.set_current_rpState(0,1);
    00524         my_rpCCAutomata.set_current_rpState(1,1);
    00525         temp_assignments_p = my_rpCCAutomata.generate_next_command_assignments_p(result);
    00526         std::cout << result << endl;
    00527         if (temp_assignments_p != NULL) {
    00528                 cout << *temp_assignments_p << endl;
    00529         }
    00530         */
    00531
    00532         my_rpInterface.update_goal_state(1,1);
    00533         my_rpInterface.update_goal_state(2,1);
    00534
    00535         my_rpInterface.update_current_state(1,1);
    00536         my_rpInterface.update_current_state(2,0);
    00537         std::cout << *my_rpInterface.reachable_state_intList_p() << std::endl;
    00538         std::cout << *my_rpInterface.generate_next_command_intList_p() << std::endl;
    00539         my_rpInterface.update_current_state(1,0);
    00540         my_rpInterface.update_current_state(2,0);
    00541         std::cout << *my_rpInterface.generate_next_command_intList_p() << std::endl;
    00542         my_rpInterface.update_current_state(1,0);
    00543         my_rpInterface.update_current_state(2,1);
    00544         std::cout << *my_rpInterface.generate_next_command_intList_p() << std::endl;
    00545         my_rpInterface.update_current_state(1,1);
    00546         my_rpInterface.update_current_state(2,1);
    00547         std::cout << *my_rpInterface.generate_next_command_intList_p() << std::endl;
    00548         my_rpInterface.update_current_state(1,3);
    00549         my_rpInterface.update_current_state(2,1);
    00550         std::cout << *my_rpInterface.reachable_state_intList_p() << std::endl;
    00551         std::cout << *my_rpInterface.generate_next_command_intList_p() << std::endl;
    00552 }
    00553