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

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/Command/CommandDispatcher.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 //
    00010
    00011 #include "CommandDispatcher.h"
    00012 #include <Mers/Models/Base/BaseModel.h>
    00013 #include <Mers/Interfaces/ExecutionInterfaceBase.h>
    00014 //#include <Mers/Interfaces/Task.h>
    00015 #include <Mers/Utils/Containers/IntegerArray.h>
    00016 #include <Mers/Utils/Containers/DynamicArrayTemplate.h>
    00017 #include "CommandDispatcherEntry.h"
    00018
    00020
    00021 using namespace Mers::Executives::DeductiveController::Command;
    00022 using Mers::Executives::DeductiveController::Rtapi::CommandCallback;
    00023 using Mers::Utils::Lang::BaseModelObject;
    00024 using Mers::Utils::Containers::DynamicArrayTemplate;
    00025 using Mers::Utils::Containers::ObjectArray;
    00026 using Mers::Utils::Containers::DynamicArray;
    00027 using Mers::Utils::Containers::IntegerArray;
    00028 using Mers::Models::Base::BaseModelVariableArray;
    00029 using Mers::Models::Base::BaseModelVariable;
    00030 using Mers::Models::Base::BaseModel;
    00031 using Mers::Models::Base::BaseModelValueArray;
    00032 using Mers::Models::Base::BaseModelValue;
    00033 using Mers::Interfaces::ExecutionInterfaceBase;
    00034 using std::ostream;
    00035 using std::cout;
    00036 using std::endl;
    00037
    00039
    00040 namespace Mers {
    00041         namespace Executives {
    00042                 namespace DeductiveController {
    00043                         namespace Command {
    00044
    00045                                 class DispatcherPair : public BaseModelObject
    00046                                 {
    00047                                 public:
    00054                                         DispatcherPair(ExecutionInterfaceBase * connection,
    00055                                                 ExecutionInterfaceBase::TaskIdentifier task)
    00056                                                 : BaseModelObject(BaseModelObject::oDispatcherPair),
    00057                                                         mConnection(connection), mTask(task)
    00058                                         {}
    00059
    00063                                         ~DispatcherPair()
    00064                                         {
    00065                                                 mConnection->deleteTask(mTask);
    00066                                         }
    00067
    00068                                         void dispatch()
    00069                                         {
    00070                                                 if(mConnection != NULL)
    00071                                                         mConnection->start(mTask);
    00072                                         }
    00073
    00074 #ifndef EMBEDDED
    00075
    00080                                         virtual void print(ostream &out) const
    00081                                         {
    00082                                                 out << "Dispatcher Pair";
    00083                                         }
    00084 #endif
    00085
    00086                                 protected:
    00088                                         ExecutionInterfaceBase * mConnection;
    00089
    00091                                         ExecutionInterfaceBase::TaskIdentifier mTask;
    00092                                 };
    00093                         }
    00094                 }
    00095         }
    00096 }
    00097
    00099
    00100 CommandDispatcher::CommandDispatcher() : CommandCallback(),
    00101         mCommandMap(true), mDispatchLists(true), mModelVariables(NULL)
    00102 {
    00103 }
    00104
    00106
    00107 CommandDispatcher::~CommandDispatcher()
    00108 {
    00109         mCommandMap.makeEmpty();
    00110         mDispatchLists.makeEmpty();
    00111 }
    00112
    00114
    00115 void CommandDispatcher::setModel(BaseModel const * model)
    00116 {
    00117         // We need to map all variable/value combinations from the model to
    00118         // the command map and then create mDispatchLists space for each of
    00119         // them.
    00120
    00121         mCommandMap.makeEmpty();
    00122         mDispatchLists.makeEmpty();
    00123
    00124         unsigned int uid = 0;
    00125         mModelVariables = model->getVariables();
    00126         BaseModelVariableArray const &vars = *mModelVariables;
    00127         for(unsigned int x = 0; x < vars.getLength(); x++){
    00128                 BaseModelVariable const &var = *(vars[x]);
    00129                 BaseModelValueArray const &vals = *(var.getDomain());
    00130                 IntegerArray * mapping = new IntegerArray();
    00131                 mapping->resize(vals.getLength());
    00132                 mCommandMap.pushBack(mapping);
    00133
    00134                 for(unsigned int y = 0; y < mapping->getLength(); y++){
    00135                         (*mapping)[y] = uid++;
    00136
    00137                         ObjectArray * dispatcherList = new ObjectArray(true);
    00138                         mDispatchLists.pushBack(dispatcherList);
    00139                 }
    00140         }
    00141 }
    00142
    00144
    00145 extern FILE * yyCommandDispatcherConfigParserin;
    00146 int yyCommandDispatcherConfigParserparse(void * entries);
    00147
    00148 int CommandDispatcher::addConnection(ExecutionInterfaceBase * connection,
    00149         char const * fileName)
    00150 {
    00151         DynamicArrayTemplate<CommandDispatcherEntry> entries(true);
    00152
    00153         yyCommandDispatcherConfigParserin = fopen(fileName, "r");
    00154         if(yyCommandDispatcherConfigParserin == NULL){
    00155                 perror(fileName);
    00156                 return 1;
    00157         }
    00158
    00159         if(yyCommandDispatcherConfigParserparse(&entries) != 0){
    00160                 fprintf(stderr, "Error parsing file: %s\n", fileName);
    00161                 fclose(yyCommandDispatcherConfigParserin);
    00162                 return 1;
    00163         }
    00164
    00165         for(unsigned int x = 0; x < entries.getLength(); x++){
    00166                 CommandDispatcherEntry &entry = (*entries[x]);
    00167
    00168                 BaseModelVariable const * var = mModelVariables->find(entry.variable);
    00169                 if(var == NULL){
    00170                         fprintf(stderr, "Error, unable to find the variable '%s'\n"
    00171                                 "from the file '%s'\n", entry.variable, fileName);
    00172                         fclose(yyCommandDispatcherConfigParserin);
    00173                         return 1;
    00174                 }
    00175                 unsigned int varUID = var->getUID();
    00176
    00177                 BaseModelValue const * val = var->getDomain()->find(entry.value);
    00178                 if(var == NULL){
    00179                         fprintf(stderr, "Error, unable to find the value '%s'"
    00180                                 " of the variable '%s'\nfrom the file '%s'\n",
    00181                                 entry.value, entry.variable, fileName);
    00182                         fclose(yyCommandDispatcherConfigParserin);
    00183                         return 1;
    00184                 }
    00185                 unsigned int valUID = val->getUID();
    00186
    00187                 unsigned int index = getIndex(varUID, valUID);
    00188                 if(mDispatchLists[index]->getObjectType() !=
    00189                         BaseModelObject::BMDynamicArray)
    00190                 {
    00191                         fprintf(stderr, "Error, unexpected object '%s'.  Expecting '%s'.\n",
    00192                                 BaseModelObject::objectTypeToString
    00193                                 (mDispatchLists[index]->getObjectType()),
    00194                                 BaseModelObject::objectTypeToString(BaseModelObject::BMDynamicArray));
    00195                         fclose(yyCommandDispatcherConfigParserin);
    00196                         return 1;
    00197                 }
    00198                 DynamicArray * dispDynArray = (DynamicArray *) mDispatchLists[index];
    00199                 if(dispDynArray->getDynamicArrayType() !=
    00200                         DynamicArray::oObjectArray)
    00201                 {
    00202                         fprintf(stderr, "Error, unexpected object '%s'.  Expecting '%s'.\n",
    00203                                 DynamicArray::dynamicArrayTypeToString
    00204                                 (dispDynArray->getDynamicArrayType()),
    00205                                 DynamicArray::dynamicArrayTypeToString(DynamicArray::oObjectArray));
    00206                         fclose(yyCommandDispatcherConfigParserin);
    00207                         return 1;
    00208                 }
    00209                 ObjectArray &dispatcherList = *((ObjectArray *) dispDynArray);
    00210
    00211                 ExecutionInterfaceBase::TaskIdentifier task =
    00212                         connection->createTask(entry.function, *entry.parameters);
    00213                 DispatcherPair * pair = new DispatcherPair(connection, task);
    00214
    00215                 dispatcherList.pushBack(pair);
    00216         }
    00217
    00218         fclose(yyCommandDispatcherConfigParserin);
    00219         return 0;
    00220 }
    00221
    00223
    00224 void CommandDispatcher::issueCommand(unsigned int commandVariable,
    00225         unsigned int commandValue)
    00226 {
    00227         unsigned int index = getIndex(commandVariable, commandValue);
    00228         if(mDispatchLists[index]->getObjectType() !=
    00229                 BaseModelObject::BMDynamicArray)
    00230         {
    00231                 fprintf(stderr, "Error, unexpected object '%s'.  Expecting '%s'.\n",
    00232                         BaseModelObject::objectTypeToString
    00233                         (mDispatchLists[index]->getObjectType()),
    00234                         BaseModelObject::objectTypeToString(BaseModelObject::BMDynamicArray));
    00235                 exit(1);
    00236         }
    00237         DynamicArray * dispDynArray = (DynamicArray *) mDispatchLists[index];
    00238         if(dispDynArray->getDynamicArrayType() !=
    00239                 DynamicArray::oObjectArray)
    00240         {
    00241                 fprintf(stderr, "Error, unexpected object '%s'.  Expecting '%s'.\n",
    00242                         DynamicArray::dynamicArrayTypeToString
    00243                         (dispDynArray->getDynamicArrayType()),
    00244                         DynamicArray::dynamicArrayTypeToString(DynamicArray::oObjectArray));
    00245                 exit(1);
    00246         }
    00247         ObjectArray &dispatcherList = *((ObjectArray *) dispDynArray);
    00248
    00249         unsigned int dispatcherListLength = dispatcherList.getLength();
    00250         for(unsigned int x = 0; x < dispatcherListLength; x++){
    00251                 if(dispatcherList[x]->getObjectType() !=
    00252                         BaseModelObject::oDispatcherPair)
    00253                 {
    00254                         fprintf(stderr, "Error, unexpected object '%s'.  Expecting '%s'.\n",
    00255                                 BaseModelObject::objectTypeToString
    00256                                 (mDispatchLists[index]->getObjectType()),
    00257                                 BaseModelObject::objectTypeToString(BaseModelObject::oDispatcherPair));
    00258                         exit(1);
    00259                 }
    00260                 DispatcherPair * dispatchTask = (DispatcherPair *) dispatcherList[x];
    00261
    00262                 //cout << "Dispatching: " << *dispatchTask << endl;
    00263                 dispatchTask->dispatch();
    00264         }
    00265 }
    00266
    00268
    00269 #ifndef EMBEDDED
    00270
    00271 void CommandDispatcher::print(ostream &out) const
    00272 {
    00273         out << "Command Dispatcher";
    00274 }
    00275
    00276 #endif
    00277
    00279
    00280 unsigned int CommandDispatcher::getIndex(unsigned int varUID, unsigned int valUID)
    00281 {
    00282         if(mCommandMap.getLength() <= varUID)
    00283         {
    00284                 fprintf(stderr, "Error, command variable UID out of range:\n"
    00285                         "%d not in [0-%d]\n", varUID, mCommandMap.getLength() - 1);
    00286                 exit(1);
    00287         }else if(mCommandMap[varUID]->getObjectType() !=
    00288                 BaseModelObject::BMDynamicArray)
    00289         {
    00290                 fprintf(stderr, "Error, unexpected object '%s'.  Expecting '%s'.\n",
    00291                         BaseModelObject::objectTypeToString
    00292                         (mCommandMap[varUID]->getObjectType()),
    00293                         BaseModelObject::objectTypeToString(BaseModelObject::BMDynamicArray));
    00294                 exit(1);
    00295         }
    00296         DynamicArray * dispDynArray = (DynamicArray *) mCommandMap[varUID];
    00297         if(dispDynArray->getDynamicArrayType() !=
    00298                 DynamicArray::IntegerArray){
    00299                 fprintf(stderr, "Error, invalid indexing object:\n"
    00300                         "found '%s' but expected '%s'\n",
    00301                         DynamicArray::dynamicArrayTypeToString
    00302                         (dispDynArray->getDynamicArrayType()),
    00303                         DynamicArray::dynamicArrayTypeToString(DynamicArray::IntegerArray));
    00304                         exit(1);
    00305         }
    00306
    00307         IntegerArray &indices = *((IntegerArray *) mCommandMap[varUID]);
    00308         if(indices.getLength() <= valUID)
    00309         {
    00310                 fprintf(stderr, "Error, command value UID out of range:\n"
    00311                         "%d not in [0-%d]\n", valUID, indices.getLength() - 1);
    00312                 exit(1);
    00313         }
    00314         return indices[valUID];
    00315 }
    00316
    00318
    00319