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

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/Rtapi/MR/MR/test.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 "../rtapi/livapi_rt.h"
    00015 #include <Mers/Executives/DeductiveController/Rtapi/MR/MR_rtapi.h>
    00016 #include <Mers/Executives/DeductiveController/Rtapi/MR/Stub/MR_callback_stub.h>
    00017 #include <Mers/Models/CCA/CCAModel.h>
    00018 #include <Mers/Solvers/Entailment/GI/GoalInterpreter.h>
    00019 #include <Mers/Reconfiguration/Discrete/mrInterface.h>
    00020 #include <Mers/Models/CCA/HardCodedModels.h>
    00021 #include <Mers/Models/CCA/Io/Mof/MOFReader.h>
    00022 #include <Mers/Utils/Lang/Sleep.h>
    00023
    00024 //#include "../rtapi/userfcns_debug.h"
    00025
    00027
    00028 using Mers::Executives::DeductiveController::Rtapi::MR::MR_rtapi;
    00029 using Mers::Solvers::Entailment::GI::GoalInterpreter;
    00030 using Mers::Executives::DeductiveController::Rtapi::MR::Stub::MR_callback_stub;
    00031 using Mers::Models::CCA::CCAModel;
    00032 using Mers::Models::CCA::Io::Mof::MOFReader;
    00033 using Mers::Utils::Lang::sleep;
    00034 using std::endl;
    00035 using std::cerr;
    00036 using std::cout;
    00037
    00039
    00040 void test_titanPropulsion_interface(MR_rtapi &mr_rtapi)
    00041 {
    00042         printf("Testing the Titan Propulsion Module\n");
    00043
    00044         //mr_rtapi.queue_state_update(1, 1);
    00045         //mr_rtapi.queue_goal(2,1);
    00046         //mr_rtapi.queue_request_MR();
    00047         mr_rtapi.queue_state_update(0, 4);
    00048         mr_rtapi.queue_state_update(5, 4);
    00049         mr_rtapi.queue_state_update(10, 3);
    00050         mr_rtapi.queue_state_update(16, 2);
    00051
    00052         // give it the final goal
    00053         // the goal is that the driver is off and the valves are open and
    00054         // and the thruster is on
    00055         mr_rtapi.queue_goal(0, 3);
    00056         mr_rtapi.queue_goal(5, 3);
    00057         mr_rtapi.queue_goal(10, 2);
    00058         mr_rtapi.queue_goal(16, 2);
    00059
    00060         mr_rtapi.queue_request_MR();
    00061         //command (31, 0)
    00062
    00063         mr_rtapi.queue_state_update(16, 1);
    00064         mr_rtapi.queue_request_MR();
    00065         //command (29, 0)
    00066
    00067         mr_rtapi.queue_state_update(0, 3);
    00068         mr_rtapi.queue_request_MR();
    00069         // command should be (30, 0) indicating that valve1 should be
    00070
    00071         mr_rtapi.queue_state_update(5, 3);
    00072         mr_rtapi.queue_request_MR();
    00073         // command should be (28, 0) indicating that thruster should be
    00074
    00075         mr_rtapi.queue_state_update(10, 2);
    00076         mr_rtapi.queue_request_MR();
    00077         // command should be (31, 1) indicating that DCIU should be
    00078
    00079         mr_rtapi.queue_state_update(16, 2);
    00080         mr_rtapi.queue_request_MR();
    00081         // No command is required
    00082
    00083         // our new states indicate the driver is unknown and the thruster is off
    00084         //
    00085         // our desired goal of keeping the driver off and the valve closed can
    00086         // no longer be reached
    00087         // so RequestMR will return an error that the goal cannot be reached
    00088         // reversably
    00089         mr_rtapi.queue_state_update(10, 3);
    00090         mr_rtapi.queue_state_update(16, 0);
    00091         mr_rtapi.queue_request_MR();
    00092         // Failed
    00093
    00094         return;
    00095 }
    00096
    00097 void test_MRP_interface(MR_rtapi &mr_rtapi)
    00098 {
    00099         printf("***Testing MR interface***\n");
    00100
    00101         mr_rtapi.queue_state_update(1, 1);
    00102         mr_rtapi.queue_state_update(2, 0);
    00103         //mr_rtapi.queue_state_update(3, 1);
    00104         mr_rtapi.queue_goal(1,1);
    00105         mr_rtapi.queue_goal(2,1);
    00106         mr_rtapi.queue_request_MR();
    00107         mr_rtapi.queue_state_update(1, 0);
    00108         mr_rtapi.queue_state_update(2, 0);
    00109         //mr_rtapi.queue_state_update(3, 1);
    00110         //mr_rtapi.queue_goal(1,1);
    00111         //mr_rtapi.queue_goal(2,1);
    00112         mr_rtapi.queue_request_MR();
    00113         mr_rtapi.queue_state_update(1, 0);
    00114         mr_rtapi.queue_state_update(2, 1);
    00115         //mr_rtapi.queue_state_update(3, 0);
    00116         //mr_rtapi.queue_goal(1,1);
    00117         //mr_rtapi.queue_goal(2,1);
    00118         mr_rtapi.queue_request_MR();
    00119         mr_rtapi.queue_state_update(1, 1);
    00120         mr_rtapi.queue_state_update(2, 1);
    00121         //mr_rtapi.queue_state_update(3, 0);
    00122         //mr_rtapi.queue_goal(1,1);
    00123         //mr_rtapi.queue_goal(2,1);
    00124         mr_rtapi.queue_request_MR();
    00125         mr_rtapi.queue_state_update(1,3);
    00126         mr_rtapi.queue_state_update(2,1);
    00127         //mr_rtapi.queue_state_update(3, 0);
    00128         //mr_rtapi.queue_goal(1,1);
    00129         //mr_rtapi.queue_goal(2,1);
    00130         mr_rtapi.queue_request_MR();
    00131
    00132         mr_rtapi.queue_state_update(1, 1);
    00133         mr_rtapi.queue_state_update(2, 0);
    00134         mr_rtapi.queue_goal(3, 1);
    00135         mr_rtapi.queue_request_MR();
    00136         return;
    00137 }
    00138
    00139 void test_RP_interface(MR_rtapi &mr_rtapi)
    00140 {
    00141         printf("***Testing MR interface***\n");
    00142
    00143         mr_rtapi.queue_state_update(1,1);
    00144         mr_rtapi.queue_state_update(2,0);
    00145         mr_rtapi.queue_goal(1,1);
    00146         mr_rtapi.queue_goal(2,1);
    00147         mr_rtapi.queue_request_MR();
    00148         mr_rtapi.queue_state_update(1,0);
    00149         mr_rtapi.queue_state_update(2,0);
    00150         mr_rtapi.queue_goal(1,1);
    00151         mr_rtapi.queue_goal(2,1);
    00152         mr_rtapi.queue_request_MR();
    00153         mr_rtapi.queue_state_update(1,0);
    00154         mr_rtapi.queue_state_update(2,1);
    00155         mr_rtapi.queue_goal(1,1);
    00156         mr_rtapi.queue_goal(2,1);
    00157         mr_rtapi.queue_request_MR();
    00158         mr_rtapi.queue_state_update(1,1);
    00159         mr_rtapi.queue_state_update(2,1);
    00160         mr_rtapi.queue_goal(1,1);
    00161         mr_rtapi.queue_goal(2,1);
    00162         mr_rtapi.queue_request_MR();
    00163         mr_rtapi.queue_state_update(1,3);
    00164         mr_rtapi.queue_state_update(2,1);
    00165         mr_rtapi.queue_goal(1,1);
    00166         mr_rtapi.queue_goal(2,1);
    00167         mr_rtapi.queue_request_MR();
    00168         return;
    00169 }
    00170
    00171 int main()
    00172 {
    00173
    00174         //-------------------------
    00175         // Creating Model
    00176         //-------------------------
    00177         const char filename[] = "../Model/MOFReader/propulsion.mof";
    00178
    00179         CCAModel my_model("Test Model");
    00180
    00181         //createValveDriverModel(my_model);
    00182         if(MOFReader::loadMOF(filename, my_model))
    00183         {
    00184                 cerr << "Failed to load file " << filename << endl
    00185                         << "Aborting" << endl;
    00186                 return 1;
    00187         }
    00188         cout << "Loaded Model: " << filename << endl;
    00189
    00190         //std::cout << my_model << std::endl;
    00191
    00192         /*
    00193         cout << "Making a reactive planner... ";
    00194
    00195           reactivePlanner * rp = new reactivePlanner(my_model);
    00196           if(!rp){
    00197           cout << "Failed." << endl;
    00198           return 1;
    00199           }
    00200
    00201                 cout << "Done." << endl << "Making rpInterface... ";
    00202
    00203                   rpInterface * rpi = new rpInterface(*rp);
    00204                   if(!rpi){
    00205                   cout << "Failed." << endl;
    00206                   return 2;
    00207                   }
    00208
    00209                         cout << "Done." << endl << "Making Goal Interpreter... ";
    00210
    00211                           GoalInterpreter * gi = new GoalInterpreter(&my_model);
    00212                           if(!gi){
    00213                           cout << "Failed." << endl;
    00214                           return 3;
    00215                           }
    00216
    00217                                 cout << "Done." << endl;
    00218
    00219                                   delete rp;
    00220                                   delete rpi;
    00221                                   delete gi;
    00222
    00223                                         cout << "Test done, creating mode Reconfiguration" << endl;
    00224         */
    00225
    00226         //-------------------
    00227         // Create rpCCAutomata for Reactive Planning
    00228         //--------------------
    00229         GoalInterpreter * gi = new GoalInterpreter(&my_model);
    00230         modeReconfiguration my_modeReconfiguration(my_model, gi);
    00231         //      std::cout << my_rpCCAutomata << std::endl;
    00232
    00233         //cout << "Creating mrInterface" << endl;
    00234
    00235         //-----------------------
    00236         // Create mrInterface
    00237         //-------------------------
    00238         mrInterface my_mrInterface(my_modeReconfiguration);
    00239
    00240         //cout << "Printing the Model" << endl;
    00241         std::cout << my_mrInterface << std::endl;
    00242         //cout << "Done" << endl;
    00243
    00244         MR_callback_stub *mr_cbs;
    00245         MR_rtapi *mr_rtapi;
    00246
    00247         //      mr = new MR();
    00248         mr_cbs = new MR_callback_stub();
    00249         mr_rtapi = new MR_rtapi(&my_mrInterface, mr_cbs);
    00250
    00251         //test_MRP_interface(*mr_rtapi);
    00252         test_titanPropulsion_interface(*mr_rtapi);
    00253
    00254         sleep(1);
    00255
    00256         delete mr_rtapi; // this will wait for the dispatcher thread to exit
    00257         delete mr_cbs;
    00258
    00259
    00260         /*
    00261         Tracker * syst;
    00262         ReportTransitionsDebug * rtd;
    00263         L2_rtapi * rtapi;
    00264
    00265           syst = new Tracker();
    00266
    00267                 if(!syst)
    00268                 {
    00269                 perror("failed to create a Tracker object\n");
    00270                 return 1;
    00271                 }
    00272
    00273                   rtd = new ReportTransitionsDebug(syst);
    00274                   if(!rtd)
    00275                   {
    00276                   delete syst;
    00277                   perror("failed to create a reporter object\n");
    00278                   return 1;
    00279                   }
    00280                   rtapi = new L2_rtapi(mr, syst, *rtd);
    00281                   if(!rtapi)
    00282                   {
    00283                   delete syst;
    00284                   delete rtd;
    00285                   perror("failed to create a real time interface\n");
    00286                   return 1;
    00287                   }
    00288                   testAPI(*rtapi);
    00289                   delete rtapi;
    00290                   delete rtd;
    00291                   delete syst;
    00292         */
    00293
    00294
    00295         return 0;
    00296 }
    00297