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

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/Configs/ATRV/DCATRVTest.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 <Mers/Estimation/Discrete/ModeEstimation/ModeEstimator.h>
    00015 #include <Mers/Estimation/Discrete/MEInterface.h>
    00016 #include <Mers/Estimation/Discrete/Trajectories.h>
    00017 #include <Mers/Estimation/Discrete/Io/Xml/Sax/TrajCH.h>
    00018 #include <Mers/Solvers/Entailment/GI/GoalInterpreter.h>
    00019 #include <Mers/Models/CCA/CCAModel.h>
    00020 #include <Mers/Testbeds/ATRV/Robot/RobotSetup.h>
    00021 #include <Mers/Testbeds/ATRV/Robot/RobotExecInterface.h>
    00022 #include <Mers/Testbeds/ATRV/Robot/RobotListener.h>
    00023 #include <Mers/Utils/Containers/DynamicArrayTemplate.h>
    00024 #include <Mers/Utils/Logging/logger.h>
    00025 #include <Mers/Utils/Lang/String.h>
    00026 #include <Mers/Utils/Io/Xml/Sax/XMLLoader.h>
    00027 #include <Mers/Executives/DeductiveController/Observation/DiscreteObservationConverter.h>
    00028 #include <Mers/Executives/DeductiveController/Command/CommandTranslator.h>
    00029 #include <Mers/Executives/DeductiveController/Command/TaskTransConfigParser.hpp>
    00030 #include <Mers/Executives/DeductiveController/Command/TaskTransConfigLexer.hpp>
    00031 #include <antlr/TokenStreamException.hpp>
    00032 #include <vector>
    00033
    00035
    00036 using Mers::Models::CCA::CCAModel;
    00037 using Mers::Models::Base::BaseModel;
    00038 using Mers::Estimation::Discrete::MEInterface;
    00039 using Mers::Estimation::Discrete::Trajectories;
    00040 using Mers::Estimation::Discrete::Io::Xml::Sax::TrajCH;
    00041 using Mers::Estimation::Discrete::ModeEstimation::ModeEstimator;
    00042 using Mers::Solvers::Entailment::GoalInterpreterInterface;
    00043 using Mers::Solvers::Entailment::GI::GoalInterpreter;
    00044 using Mers::Testbeds::ATRV::Robot::Robot;
    00045 using Mers::Testbeds::ATRV::Robot::RobotListener;
    00046 using Mers::Testbeds::ATRV::Robot::RobotSetup;
    00047 using Mers::Testbeds::ATRV::Robot::RobotExecInterface;
    00048 using Mers::Utils::Containers::DynamicArrayTemplate;
    00049 using Mers::Utils::Lang::String;
    00050 using Mers::Utils::Io::Xml::Sax::XMLLoader;
    00051 using Mers::Executives::DeductiveController::DCInterface;
    00052 using Mers::Executives::DeductiveController::Observation::DiscreteObservationConverter;
    00053 using Mers::Executives::DeductiveController::Command::CommandTranslator;
    00054 using Mers::Executives::DeductiveController::Command::CommandDispatcherEntry;
    00055 using Mers::Executives::DeductiveController::Command::TaskTransConfigParser;
    00056 using Mers::Executives::DeductiveController::Command::TaskTransConfigLexer;
    00057 using Mers::Interfaces::ExecutionInterface;
    00058 using std::vector;
    00059 using std::ifstream;
    00060 using std::cerr;
    00061 using std::endl;
    00062
    00064
    00065 void printUse( const char * progName )
    00066 {
    00067         printf("Use: %s <CCA file> <ME Init file> <Robot File> [-l<log4cplus file>] "
    00068                 "[-t<Task Translation File>] <Observation Discretization File>+\n"
    00069         "  <Task Translation File> : Used to configure a CommandTranslator, "
    00070         "translating from DC Tasks to Robot Tasks\n"
    00071         "  <Observation Discretization File> : Used to configure a "
    00072         "DiscreteObservationConverter, with one per robot, discretizing "
    00073         "observations from each robot to the DC.\n",
    00074                 progName);
    00075 }
    00076
    00078
    00079 bool requiredParameters(int argc, const char * progName) {
    00080         if( argc < 3 ){
    00081                 printUse( (progName == NULL ? "DCTest" : progName ) );
    00082                 return false;
    00083         } else {
    00084                 return true;
    00085         }
    00086 }
    00087
    00089
    00090 class Arguments
    00091 {
    00092 public:
    00094         Arguments() : ccaModel( NULL ), modelFileName( NULL ), trajFileName( NULL ),
    00095                 robotInitFileName( NULL ), robotSetup( NULL ), robotExecInterface( NULL ),
    00096                 log4cplusFileName( NULL ), taskTransFileName( NULL ),
    00097                 taskTransEntries( true ), discObsConvFileNames( true ) {}
    00098
    00100         ~Arguments() {
    00101                 if( robotSetup != NULL )
    00102                         delete robotSetup;
    00103                 if( robotExecInterface != NULL )
    00104                         delete robotExecInterface;
    00105         }
    00106
    00108         CCAModel * ccaModel;
    00109         const char * modelFileName;
    00110         const char * trajFileName;
    00111         const char * robotInitFileName;
    00112         RobotSetup * robotSetup;
    00113         RobotExecInterface * robotExecInterface;
    00114         const char * log4cplusFileName;
    00115         const char * taskTransFileName;
    00116         DynamicArrayTemplate< CommandDispatcherEntry > taskTransEntries;
    00119         DynamicArrayTemplate<String> discObsConvFileNames;
    00120         // These need to persist until after Robot is deleted.
    00121         vector< RobotListener * > robotListeners;
    00122 };
    00123
    00125
    00126 CCAModel *      load_MOF(char const * MOF_file_name);
    00127
    00129
    00140 bool loadTaskEntries( const String &fileName,
    00141         DynamicArrayTemplate< CommandDispatcherEntry > &out_entries ) {
    00142         try {
    00143                 ifstream ifs( fileName.c_str() );
    00144                 TaskTransConfigLexer lex(ifs);
    00145                 lex.setFilename( fileName.c_str() );
    00146                 TaskTransConfigParser parser(lex);
    00147                 parser.setFilename( fileName.c_str() );
    00148
    00149                 parser.loadConfigFile( out_entries );
    00150         } catch(ANTLR_USE_NAMESPACE(antlr)TokenStreamException * e) {
    00151                 cerr << "Problem with stream: " << e << endl;
    00152                 cerr << "For file " << fileName << endl;
    00153                 return false;
    00154         } catch(ANTLR_USE_NAMESPACE(antlr)RecognitionException * re) {
    00155                 cerr << "Bad input: " << re << endl;
    00156                 cerr << "For file " << fileName << endl;
    00157                 return false;
    00158         } catch(...) {
    00159                 cerr << "Unknown Task Translation Config File Exception." <<
    00160                         endl;
    00161                 cerr << "For file " << fileName << endl;
    00162                 return false;
    00163         }
    00164         return true;
    00165 }
    00166
    00168
    00169 Arguments * loadParameters(int argc, const char * argv[]) {
    00170         Arguments * args = new Arguments();
    00171         args->ccaModel = load_MOF(argv[0]);
    00172         args->modelFileName = argv[0];
    00173         args->trajFileName = argv[1];
    00174         args->robotInitFileName = argv[2];
    00175         args->robotSetup = new RobotSetup( args->robotInitFileName );
    00176         args->robotExecInterface = new RobotExecInterface( args->robotSetup );
    00177
    00178         int i = 3;
    00179         // load optional log4cplus file.
    00180         if( argc > i &&  argv[i][0] == '-' && argv[i][1] == 'l') {
    00181                 args->log4cplusFileName = argv[i++] + 2;
    00182         }
    00183
    00184         // load optional task translation config file.
    00185         if( argc > i &&  argv[i][0] == '-' && argv[i][1] == 't') {
    00186                 args->taskTransFileName = argv[i++] + 2;
    00187                 if( !loadTaskEntries( args->taskTransFileName, args->taskTransEntries ) ) {
    00188                         const char * loggerName =
    00189                                 "Mers.Executives.DeductiveController.Configs.ATRV.DCTest";
    00190                         LOG4CPLUS_WARN( Logger::getInstance(loggerName),
    00191                                 "Unable to load task translation entries from " <<
    00192                                 args->taskTransFileName );
    00193                         args->taskTransFileName = NULL;
    00194                 }
    00195         }
    00196
    00197         // Load any remaining discrete observation converter config files.
    00198         for( ; i < argc; ++i ) {
    00199                 args->discObsConvFileNames.pushBack( new String( argv[i] ) );
    00200         }
    00201         return args;
    00202 }
    00203
    00205
    00206 ModeEstimator * constructME(Arguments * args) {
    00207         Trajectories * trajs;
    00208         // Load the Trajectories.
    00209         if( FAILED( XMLLoader::load< Trajectories *, const BaseModel *,
    00210                 TrajCH>( args->trajFileName, trajs, args->ccaModel ) ) )
    00211         {
    00212                 trajs = NULL;
    00213         }
    00214
    00215         ModeEstimator * me = new ModeEstimator(*(args->ccaModel), trajs);
    00216
    00217         return me;
    00218 }
    00219
    00221
    00222 GoalInterpreterInterface * constructGI(Arguments * args) {
    00223         return new GoalInterpreter(args->ccaModel);
    00224 }
    00225
    00227
    00231 RobotSetup * getRobots( Arguments * args ) {
    00232         return args->robotSetup;
    00233 }
    00235
    00239 RobotExecInterface * getRobotExecInt( Arguments * args ) {
    00240         return args->robotExecInterface;
    00241 }
    00242
    00244
    00249 void constructDCOI( Arguments * args, DCInterface * dc ) {
    00250         const char * loggerName =
    00251                 "Mers.Executives.DeductiveController.Configs.ATRV.DCTest";
    00252         RobotSetup * rSetup = getRobots( args );
    00253
    00254         DynamicArrayTemplate<String> &docFileNames = args->discObsConvFileNames;
    00255         unsigned int numConfigs = docFileNames.size();
    00256         if( (unsigned int) rSetup->numRobots() < numConfigs )
    00257                 numConfigs = rSetup->numRobots();
    00258
    00259         if( docFileNames.size() < (unsigned int) rSetup->numRobots() )
    00260                 LOG4CPLUS_WARN(Logger::getInstance(loggerName), "There were fewer "
    00261                         "discretization config files than robots: " <<
    00262                         docFileNames.size() << " vs. " << rSetup->numRobots() );
    00263
    00264         if( docFileNames.size() > (unsigned int) rSetup->numRobots() )
    00265                 LOG4CPLUS_WARN(Logger::getInstance(loggerName), "There were more "
    00266                         "discretization config files than robots: " <<
    00267                         docFileNames.size() << " vs. " << rSetup->numRobots() );
    00268
    00269         // We have to create an observation interface for
    00270         // each robot, because robot listeners can only load
    00271         // observations from a single robot.
    00272         for( unsigned int i = 0; i < numConfigs; ++i ) {
    00273                 DiscreteObservationConverter * doConv =
    00274                         new DiscreteObservationConverter( docFileNames[i]->c_str() );
    00275                 doConv->connectTitanInterface( dc );
    00276
    00277                 Robot * robot = rSetup->getRobot( i );
    00278                 RobotListener * robotListener =
    00279                         new RobotListener( doConv, robot );
    00280
    00281                 args->robotListeners.push_back( robotListener );
    00282         }
    00283
    00284         if( args->robotListeners.size() > 0 ) {
    00285                 RobotExecInterface * rExecInt = getRobotExecInt( args );
    00286                 rExecInt->setListeners( &(args->robotListeners) );
    00287         }
    00288 }
    00289
    00291
    00292 void CreateChildren( Arguments * args, DCInterface * dc ) {
    00293         ExecutionInterface * execInt = getRobotExecInt( args );
    00294         if( args->taskTransFileName != NULL ) {
    00295                 execInt = new CommandTranslator( execInt, args->taskTransEntries );
    00296         }
    00297         dc->setExecutionInterface( execInt );
    00298         constructDCOI( args, dc );
    00299 }
    00300
    00302
    00303 #include <Mers/Executives/DeductiveController/DeductiveController.cpp>
    00304
    00306