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

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/DCInterface.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 "DCInterface.h"
    00012
    00013 #include "DiscreteValue.h"
    00014 #include "DiscreteVariable.h"
    00015 #include "Task.h"
    00016 #include "TaskFactory.h"
    00017
    00018 #include <cstdio>
    00019 #include <cassert>
    00020 #include <cerrno>
    00021 #include <iostream>
    00022 #include <Mers/Estimation/Discrete/ME_connection.h>
    00023 #include <Mers/Reconfiguration/Discrete/MR_connection.h>
    00024 #include <Mers/Executives/DeductiveController/Rtapi/ME/ME_rtapi.h>
    00025 #include <Mers/Executives/DeductiveController/Rtapi/MR/MR_rtapi.h>
    00026 #include <Mers/Executives/DeductiveController/Rtapi/RTAPICallback.h>
    00027 #include <Mers/Models/Base/BaseModel.h>
    00028
    00030
    00031 using namespace Mers::Executives::DeductiveController;
    00032 using Mers::Estimation::Discrete::ME_connection;
    00033 using Mers::Reconfiguration::Discrete::MR_connection;
    00034 using Mers::Executives::DeductiveController::Rtapi::ME::ME_rtapi;
    00035 using Mers::Executives::DeductiveController::Rtapi::MR::MR_rtapi;
    00036 using Mers::Executives::DeductiveController::Rtapi::CommandCallback;
    00037 using Mers::Utils::Containers::intList;
    00038 using Mers::Utils::Containers::IntegerArray;
    00039 using Mers::Utils::Containers::DynamicArrayTemplate;
    00040 using Mers::Utils::Lang::String;
    00041 using Mers::Interfaces::ExecutionInterfaceBase;
    00042 using Mers::Interfaces::ObservationTransformationInterface;
    00043 using Mers::Models::Base::BaseModel;
    00044 using Mers::Models::Base::BaseModelVariable;
    00045 using Mers::Models::Base::BaseModelVariableArray;
    00046 using Mers::Models::Base::BaseModelValue;
    00047 using Mers::Models::Base::BaseModelDomain;
    00048 using Mers::Models::Base::BaseModelAssignment;
    00049 using std::vector;
    00050 using std::map;
    00051 using std::set;
    00052 using log4cplus::Logger;
    00053
    00055
    00056 Logger DCInterface::m_Logger(Logger::getInstance("Mers.Executives."
    00057         "DeductiveController.DCInterface"));
    00058
    00060 // Public Constructors, Copy Constructors, & Destructors
    00062
    00063 DCInterface::DCInterface(ME_connection *me_connection_p,
    00064         MR_connection *mr_connection_p, BaseModel * model, bool initialEstimate)
    00065         : RTAPICallback(), ExecutionInterfaceBase( NULL ),
    00066                 ObservationTransformationInterface(), mExecInterface(NULL), mTasks(),
    00067                 mBaseOffset(), mModel(model), mDVars(), mDVals(), mMrFailed(false),
    00068                 mActiveGoals(), m_MRIdle(true), m_MEIdle(true), m_recentlyFailedTasks(),
    00069                 m_recentlyCompletedTasks()
    00070 {
    00071         mMeRtapi                        = new ME_rtapi(me_connection_p, this);
    00072         mMrRtapi                        = new MR_rtapi(mr_connection_p, this);
    00073
    00074         init(model);
    00075
    00076         if( initialEstimate )
    00077                 requestFullState();
    00078 }
    00079
    00081
    00082 DCInterface::~DCInterface()
    00083 {
    00084         LOG4CPLUS_TRACE_STR(m_Logger, "Deleting ME RTAPI");
    00085         if(mMeRtapi != NULL)
    00086                 delete mMeRtapi;
    00087
    00088         LOG4CPLUS_TRACE_STR(m_Logger, "Deleting MR RTAPI");
    00089         if(mMrRtapi != NULL)
    00090                 delete mMrRtapi;
    00091
    00092         LOG4CPLUS_TRACE_STR(m_Logger, "De-referencing the execution interface.");
    00093         //if(mExecInterface != NULL)
    00094         //      mExecInterface->removeReference();
    00095
    00096         LOG4CPLUS_TRACE_STR(m_Logger, "Deleting their tasks.");
    00097         map<int, TaskIdentifier>::iterator iMap2;
    00098         for(iMap2 = mTasks.begin(); iMap2 != mTasks.end(); ++iMap2) {
    00099                 TaskIdentifier taskID = (*iMap2).second;
    00100                 mExecInterface->deleteTask(taskID);
    00101         }
    00102
    00103         LOG4CPLUS_TRACE_STR(m_Logger, "Deleting internal variables.");
    00104         vector<DiscreteVariable *>::iterator iVec1;
    00105         for(iVec1 = mDVars.begin(); iVec1 != mDVars.end(); ++iVec1) {
    00106                 DiscreteVariable * var = (*iVec1);
    00107                 delete var;
    00108         }
    00109
    00110         LOG4CPLUS_TRACE_STR(m_Logger, "Deleting internal values.");
    00111         vector<DiscreteValue *>::iterator iVec2;
    00112         for(iVec2 = mDVals.begin(); iVec2 != mDVals.end(); ++iVec2) {
    00113                 DiscreteValue * val = (*iVec2);
    00114                 delete val;
    00115         }
    00116 }
    00117
    00119 // Public Function Members
    00121
    00122 void DCInterface::setExecutionInterface(ExecutionInterfaceBase * execInterface)
    00123 {
    00124         //execInterface->addReference();
    00125         if(mExecInterface != NULL) {
    00126                 //mExecInterface->removeReference();
    00127
    00128                 map<int, TaskIdentifier>::iterator iMap;
    00129                 for(iMap = mTasks.begin(); iMap != mTasks.end(); ++iMap) {
    00130                         TaskIdentifier taskID = (*iMap).second;
    00131                         mExecInterface->deleteTask(taskID);
    00132                 }
    00133                 mTasks.clear();
    00134         }
    00135         mExecInterface = execInterface;
    00136
    00137         // Get the variables.
    00138         BaseModelVariableArray &vars = *mModel->getVariables();
    00139
    00140         // Create all of our tasks.
    00141         for(unsigned int i = 0; i < vars.getLength(); ++i) {
    00142                 BaseModelVariable * var = vars[i];
    00143                 BaseModelDomain &dom = *var->getDomain();
    00144
    00145                 if(var->getVariableType() == BaseModelVariable::Control) {
    00146                         for(unsigned int j = 0; j < dom.getLength(); ++j) {
    00147                                 BaseModelValue * val = dom[j];
    00148
    00149                                 int index = getAbsoluteIndex(var->getUID(), val->getUID());
    00150
    00151                                 String valName(val->getName());
    00152
    00153                                 vector<const String *> parameters;
    00154                                 parameters.push_back(&valName);
    00155
    00156                                 TaskIdentifier taskID =
    00157                                         mExecInterface->createTask(var->getName(), parameters);
    00158
    00159                                 map<int, TaskIdentifier>::value_type entry(index, taskID);
    00160                                 mTasks.insert(entry);
    00161                         }
    00162
    00163                 }
    00164         }
    00165 }
    00166
    00168
    00169 void DCInterface::addObservationInterface(ObservationInterface * obsInterface)
    00170 {
    00171         // Extract all the observations
    00172         BaseModelVariableArray &vars = *mModel->getVariables();
    00173
    00174         // Extract all of our possible observations.
    00175         unsigned int index = 0;
    00176         for(unsigned int i = 0; i < vars.getLength(); ++i) {
    00177                 BaseModelVariable * var = vars[i];
    00178                 BaseModelDomain &dom = *var->getDomain();
    00179
    00180                 if(var->getVariableType() == BaseModelVariable::Observable) {
    00181                         // Get this variable
    00182                         DiscreteVariableIdentifier dVarID =
    00183                                 obsInterface->getDiscreteVariable(var->getName());
    00184
    00185                         // See if it's valid
    00186                         if(obsInterface->isValid(dVarID)) {
    00187                                 // Get each value.
    00188                                 for(unsigned int j = 0; j < dom.getLength(); ++j, ++index) {
    00189                                         BaseModelValue * val = dom[j];
    00190
    00191                                         // Get a value
    00192                                         ValueIdentifier dValID =
    00193                                                 obsInterface->getValue(dVarID, val->getName());
    00194
    00195                                         // See if it's valid
    00196                                         if(obsInterface->isValid(dValID)) {
    00197                                                 // Insert it.
    00198                                                 t_ValueMap::value_type entry(dValID,
    00199                                                         t_VarValPair(var->getUID(), val->getUID()));
    00200                                                 mValueIDMap.insert(entry);
    00201                                         } else {
    00202                                                 // This is probably worth warning about.
    00203                                                 LOG4CPLUS_WARN(m_Logger, "DCInterface: Warning: Variable " <<
    00204                                                         var->getName() <<
    00205                                                         " was found but the observer did not have the domain element " <<
    00206                                                         val->getName());
    00207                                         }
    00208                                 }
    00209                         } // Otherwise this interface doesn't provide observations for
    00210                                 // this variable.
    00211                 }
    00212         }
    00213 }
    00214
    00216 // Functions for DC Callback
    00217
    00218 void
    00219 DCInterface::post_mode_estimate(intList const &updated_mode_estimate)
    00220 {
    00221         // Updated mode estimate should be pair of variable and value indicies.
    00222         // Thus, the number of int in updatated mode estimate should be even.
    00223         assert(updated_mode_estimate.size()%2 == 0);
    00224
    00225         int var_index, val_index;
    00226
    00227         // We have to update MR before DC Callback so that if titan
    00228         // callback sends a message to MR, the new state is queued first
    00229
    00230         // Update Mode Reconfiguration with the newly updated mode estimate.
    00231         for (intList::const_iterator const_it = updated_mode_estimate.begin();
    00232         const_it != updated_mode_estimate.end(); ++const_it)
    00233         {
    00234                 var_index = *const_it;
    00235                 ++const_it;
    00236                 val_index = *const_it;
    00237
    00238                 // Update MR
    00239                 mMrRtapi->queue_state_update(var_index, val_index);
    00240
    00241                 // Update our listeners
    00242                 mCurrentState[var_index] = getDiscreteValue(var_index, val_index);
    00243                 updateListeners(mCurrentState[var_index]);
    00244         }
    00245
    00246         LOG4CPLUS_DEBUG(m_Logger, "DCInterface: Received Estimate: " <<
    00247                 updated_mode_estimate);
    00248
    00249         // ME is now off the hook.
    00250         m_MEIdle = true;
    00251 }
    00252
    00254
    00255 void
    00256 DCInterface::post_full_state(intList const &full_mode_estimate)
    00257 {
    00258         post_mode_estimate(full_mode_estimate);
    00259 }
    00260
    00262
    00263 void DCInterface::post_observation(unsigned int varUID, unsigned int valUID)
    00264 {
    00265         // Update our listeners
    00266         updateListeners(getDiscreteValue(varUID, valUID));
    00267 }
    00268
    00270
    00271 void
    00272 DCInterface::process_next_control_cmd(intList const &commands)
    00273 {
    00274         // MR is now Idle, having issued a command.
    00275         m_MEIdle = false;
    00276         m_MRIdle = true;
    00277
    00278         // Issue this command to our execution interface.
    00279         if(commands.front() == 0)
    00280         {
    00281                 // No command was necessary.
    00282                 LOG4CPLUS_DEBUG_STR(m_Logger, "DCI: MR said no command was necessary.");
    00283
    00284                 // Push another find mode estimate to keep our estimates going.
    00285                 mMeRtapi->queue_find_mode_estimate();
    00286         }else if(commands.front() == 2){
    00287                 // Goals could not be satisfied.
    00288                 LOG4CPLUS_DEBUG_STR(m_Logger, "DCI: MR said the goal was unachievable.");
    00289
    00290                 mMrFailed = true;
    00291
    00292                 // Push another find mode estimate to keep our estimates going.
    00293                 mMeRtapi->queue_find_mode_estimate();
    00294         }else if(commands.front() == 1){
    00295                 intList::const_iterator it = commands.begin();
    00296                 int var, val;
    00297                 it++;
    00298                 if(it == commands.end()) {
    00299                         // We need to still take action, but we cannot issue a command
    00300                         // to take action.
    00301                         LOG4CPLUS_DEBUG_STR(m_Logger,
    00302                                 "DCI: MR said it is waiting for an observation.");
    00303
    00304                         // Push another find mode estimate to keep our estimates going.
    00305                         mMeRtapi->queue_find_mode_estimate();
    00306                 } else {
    00307                         // We need to take action, and the action has been specified.
    00308                         var = *it;
    00309                         it++;
    00310                         if(it == commands.end()) {
    00311                                 // Invalid command - we needed 2 values.
    00312                                 LOG4CPLUS_ERROR_STR(m_Logger,
    00313                                         "DCI: MR failed to provide a value with its command.");
    00314
    00315                                 // Push another find mode estimate to keep our estimates going.
    00316                                 mMeRtapi->queue_find_mode_estimate();
    00317                         } else {
    00318                                 // Take the action: var(val)
    00319                                 val = *it;
    00320
    00321                                 LOG4CPLUS_DEBUG(m_Logger, "DCI: MR said to execute [" << var <<
    00322                                         ", " << val << "], executing this command.");
    00323
    00324                                 // Inform ME of this action.
    00325                                 mMeRtapi->queue_start_command_and_time(var, val);
    00326
    00327                                 // Tell our ExecutionInterfaceBase to take this action.
    00328                                 if(mExecInterface != NULL) {
    00329                                         int index = getAbsoluteIndex(var, val);
    00330                                         map<int, TaskIdentifier>::iterator iMap = mTasks.find(index);
    00331                                         if(iMap != mTasks.end()) {
    00332                                                 mExecInterface->start((*iMap).second);
    00333                                         }
    00334                                 }
    00335                         }
    00336                 }
    00337         }else{
    00338                 LOG4CPLUS_ERROR(m_Logger, "DCI: Invalid execute_command flag '" <<
    00339                         commands.front() << "' from MR.");
    00340         }
    00341 }
    00342
    00344 // Interface Functions for ExecutionInterfaceBase
    00346
    00347 bool DCInterface::start( TaskIdentifier taskID )
    00348 {
    00349         // Add this as a goal for MR.
    00350
    00351         LOG4CPLUS_TRACE_STR(m_Logger, "DCInterface::start(): Begin");
    00352
    00353         // Start by seeing if we undertand this task.
    00354         t_TaskSharedPointer task( ExecutionInterfaceBase::getTask( taskID ) );
    00355
    00356         Task * dcTask = dynamic_cast<Task *>(task.get());
    00357
    00358         // For some reason, the LOG4 code below results in a core dump on
    00359         // my machine.  For now, the easiest "solution" is just to disable
    00360         // it.
    00361         if( dcTask == NULL ) {}
    00362         /*
    00363                         LOG4CPLUS_ERROR( m_Logger, "DCInterface::start(): Task was not a DC task: "
    00364                         << task->toString() );
    00365         */
    00366
    00367         if( !task ) {
    00368                 LOG4CPLUS_TRACE(m_Logger, "DCInterface::start(): The task was not found: "
    00369                         << taskID );
    00370
    00371                 // @Hack Pass through unknown tasks to our child execution interface.
    00372                 // They might have created this task.
    00373                 if( mExecInterface != NULL )
    00374                         return mExecInterface->start( taskID );
    00375
    00376                 return false;
    00377         }
    00378
    00379         // Make sure that this task is not already running.
    00380         set<t_TaskSharedPointer>::iterator iSet = mActiveGoals.find(task);
    00381         if(iSet != mActiveGoals.end()) {
    00382                 LOG4CPLUS_TRACE(m_Logger, "DCInterface::start(): The task was "
    00383                         "already active: " << taskID );
    00384                 return false;
    00385         }
    00386
    00387         // Add the task to the list of active goals.
    00388         mActiveGoals.insert(task);
    00389
    00390         LOG4CPLUS_TRACE(m_Logger, "DCInterface::start(): There are now "
    00391                 << mActiveGoals.size() << " active goals." );
    00392
    00393         LOG4CPLUS_TRACE_STR(m_Logger, "DCInterface::start(): End");
    00394         return true;
    00395 }
    00396
    00398
    00399 bool DCInterface::stop()
    00400 {
    00401         // Remove all goals from MR.
    00402         mActiveGoals.clear();
    00403
    00404         // @Hack Pass through the stop command to our child execution interface.
    00405         // They might have some of our tasks.
    00406         if( mExecInterface != NULL )
    00407                 return mExecInterface->stop();
    00408
    00409         return true;
    00410 }
    00411
    00413
    00414 bool DCInterface::stop( TaskIdentifier taskID )
    00415 {
    00416         // Remove this goal from MR.
    00417
    00418         // Start by seeing if we undertand this task.
    00419         t_TaskSharedPointer task = ExecutionInterfaceBase::getTask( taskID );
    00420
    00421         Task * dcTask = dynamic_cast<Task *>(task.get());
    00422         if( dcTask == NULL )
    00423                 LOG4CPLUS_ERROR( m_Logger, "DCInterface::stop(): Task was not a DC task: "
    00424                         << task->toString() );
    00425
    00426         if( !task ) {
    00427                 // @Hack Pass through unknown tasks to our child execution interface.
    00428                 // They might have created this task.
    00429                 if( mExecInterface != NULL )
    00430                         return mExecInterface->stop( taskID );
    00431
    00432                 return false;
    00433         }
    00434
    00435         // See that the task is active.
    00436         set<t_TaskSharedPointer>::iterator iSet = mActiveGoals.find(task);
    00437         if(iSet != mActiveGoals.end()) {
    00438                 mActiveGoals.erase(iSet);
    00439         }
    00440
    00441         return true;
    00442 }
    00443
    00445
    00446 int DCInterface::step( bool block )
    00447 {
    00448         LOG4CPLUS_TRACE_STR(m_Logger, "DCInterface::step(): Begin");
    00449
    00450         // Check for all tasks that were completed.
    00451         isDone();
    00452
    00453         // Clear the list of failed tasks and completed tasks.
    00454         m_recentlyFailedTasks.clear();
    00455         m_recentlyCompletedTasks.clear();
    00456
    00457         // Do an MR request, first queueing the active goals, but only if it's idle.
    00458         if(m_MRIdle && m_MEIdle) {
    00459                 m_MRIdle = false;
    00460
    00461                 LOG4CPLUS_TRACE(m_Logger, "DCInterface::step(): Starting up MR with "
    00462                         << mActiveGoals.size() << " goals." );
    00463
    00464                 set<t_TaskSharedPointer>::iterator iSet;
    00465                 for(iSet = mActiveGoals.begin(); iSet != mActiveGoals.end(); ++iSet) {
    00466                         LOG4CPLUS_TRACE_STR(m_Logger, "DCInterface::step(): Casting Task." );
    00467                         Task * task = dynamic_cast<Task *>(iSet->get());
    00468
    00469                         if( task != NULL ) {
    00470                                 LOG4CPLUS_TRACE(m_Logger, "DCInterface::step(): Sending task " <<
    00471                                         (*iSet)->toString() );
    00472                                 mMrRtapi->queue_goal(task->getVariable()->getUID(),
    00473                                         task->getValue()->getUID());
    00474                                 LOG4CPLUS_DEBUG(m_Logger, "DCInterface: Added goal (" <<
    00475                                         task->getVariable()->getName() << '[' <<
    00476                                         task->getVariable()->getUID() << "] = " <<
    00477                                         task->getValue()->getName() << '[' << task->getValue()->getUID() <<
    00478                                         "])");
    00479                         } else {
    00480                                 LOG4CPLUS_ERROR_STR(m_Logger, "DCInterface::step(): Task was NULL." );
    00481                                 if( iSet->get() != NULL )
    00482                                         LOG4CPLUS_ERROR(m_Logger, "DCInterface::step(): Is not a DCTask: " <<
    00483                                                 (*iSet)->toString() );
    00484                         }
    00485                 }
    00486
    00487                 LOG4CPLUS_TRACE_STR(m_Logger, "DCInterface::step(): Sending MR Request");
    00488
    00489                 mMrRtapi->queue_request_MR();
    00490         }
    00491
    00492         if(mMrFailed)
    00493         {
    00494                 // MR failed, indicate this.
    00495                 mMrFailed = false;
    00496                 // Some of currently active goals are failed tasks.
    00497                 // For now assume all are failed tasks.
    00498                 // To Do:  MR needs to figure out the proper subset of tasks that failed.
    00499                 set<t_TaskSharedPointer>::iterator iSet;
    00500                 for(iSet = mActiveGoals.begin(); iSet != mActiveGoals.end(); ++iSet)
    00501                 {
    00502                         m_recentlyFailedTasks.insert(ExecutionInterfaceBase::getTaskID(*iSet));
    00503                 }
    00504                 return 1;
    00505         }
    00506
    00507         LOG4CPLUS_TRACE_STR(m_Logger, "DCInterface::step(): Stepping Child EI");
    00508
    00509         // Let our next process decide the status.
    00510         if( mExecInterface != NULL )
    00511                 return mExecInterface->step( block );
    00512
    00513         LOG4CPLUS_TRACE_STR(m_Logger, "DCInterface::step(): End");
    00514
    00515         // Normal step taken.
    00516         return 0;
    00517 }
    00518
    00520
    00521 bool DCInterface::isDone()
    00522 {
    00523         // Check if currently active task is done.
    00524         set<t_TaskSharedPointer>::iterator iSet;
    00525         iSet = mActiveGoals.begin();
    00526         while (iSet != mActiveGoals.end())
    00527         {
    00528                 t_TaskSharedPointer task(*iSet);
    00529                 ++iSet;
    00530                 isDone(task);
    00531         }
    00532
    00533         // Consider our next process to decide if done.
    00534         if( mExecInterface != NULL )
    00535                 return mExecInterface->isDone() && mActiveGoals.empty();
    00536
    00537         // Return true if we have no MR goals right now.
    00538         return mActiveGoals.empty();
    00539 }
    00540
    00542
    00543 bool DCInterface::isDone( TaskIdentifier taskID )
    00544 {
    00545         // Check the task against the current state.  Return true if it matches and
    00546         // then remove it from the list of active tasks.
    00547
    00548
    00549         // Start by seeing if we undertand this task.
    00550         t_DCTaskSharedPointer task =
    00551                 ::Mers::Utils::Containers::dynamic_pointer_cast
    00552                 < Task, ::Mers::Interfaces::Task >
    00553                 ( ExecutionInterfaceBase::getTask( taskID ) );
    00554
    00555         if( !task ) {
    00556                 // @Hack Pass through unknown tasks to our child execution interface.
    00557                 // They might have created this task.
    00558                 if( mExecInterface != NULL )
    00559                         return mExecInterface->isDone( taskID );
    00560
    00561                 return false;
    00562         }
    00563
    00564         return isDone(task);
    00565 }
    00566
    00568
    00569 bool DCInterface::isDone(t_DCTaskSharedPointer task)
    00570 {
    00571         // Make sure the task is currently active, otherwise it's done.
    00572         set<t_TaskSharedPointer>::iterator iSet = mActiveGoals.find(task);
    00573         if(iSet == mActiveGoals.end())
    00574                 return true;
    00575
    00576         // We know it's active
    00577         // Check to see if it's achieved.
    00578         DiscreteValue * dValue = getDiscreteValue(task->getVariable()->getUID(),
    00579                 task->getValue()->getUID());
    00580
    00581         // See if we can even check if we're done.
    00582         if(dValue == NULL)
    00583                 return false;
    00584
    00585         // If achieved, remove the reference to it and return true.
    00586         if(isCurrentState(dValue)) {
    00587                 // If the task is done, store it in the list of completed tasks.
    00588                 m_recentlyCompletedTasks.insert(ExecutionInterfaceBase::getTaskID(task));
    00589                 mActiveGoals.erase(iSet);
    00590                 return true;
    00591         }
    00592         // Not done yet.
    00593         return false;
    00594 }
    00595
    00597
    00598 String DCInterface::getActiveTasksString() const
    00599 {
    00600         // Return a list of all active goals.
    00601         String str;
    00602
    00603         set<t_TaskSharedPointer>::const_iterator iSet;
    00604         for(iSet = mActiveGoals.begin(); iSet != mActiveGoals.end(); ++iSet) {
    00605                 t_TaskSharedPointer task = *iSet;
    00606
    00607                 str += task->toString().c_str();
    00608                 str += '\n';
    00609         }
    00610         return str;
    00611 }
    00612
    00614
    00616 DCInterface::TaskIdentifier DCInterface::createTask( const String &taskName )
    00617 {
    00618         // Get our normal identifier.
    00619         TaskIdentifier taskID( ExecutionInterfaceBase::createTask( taskName ) );
    00620         // Return it if it's valid or we don't have a child.
    00621         if( mExecInterface == NULL || ExecutionInterfaceBase::isValid( taskID ) )
    00622                 return taskID;
    00623
    00624         // Otherwise, try our child.
    00625         return mExecInterface->createTask( taskName );
    00626 }
    00627
    00629
    00631 DCInterface::TaskIdentifier DCInterface::createTask( const String &taskName,
    00632         vector<const String *> const &parameters )
    00633 {
    00634         // Get our normal identifier.
    00635         TaskIdentifier taskID
    00636                 ( ExecutionInterfaceBase::createTask( taskName, parameters ) );
    00637         // Return it if it's valid or we don't have a child.
    00638         if( mExecInterface == NULL || ExecutionInterfaceBase::isValid( taskID ) )
    00639                 return taskID;
    00640
    00641         // Otherwise, try our child.
    00642         return mExecInterface->createTask( taskName, parameters );
    00643 }
    00644
    00646
    00647 ExecutionInterfaceBase::t_TaskSharedPointer
    00648 DCInterface::getTask(TaskIdentifier task) const
    00649 {
    00650         t_TaskSharedPointer aTask = ExecutionInterfaceBase::getTask(task);
    00651         if( aTask ) {
    00652                 LOG4CPLUS_TRACE( m_Logger, "Found task " << aTask->toString() );
    00653                 return aTask;
    00654         } else {
    00655           // Ok, we didn't understand how to process this task.  If there
    00656           // is a "lower level" execution interface, try to pass this task
    00657           // down to the lower level interface for processing.  Otherwise,
    00658           // return NULL and let the parent figure out how to deal with
    00659           // the problem.
    00660                 LOG4CPLUS_TRACE( m_Logger, "Unable to find task " << task->toString() );
    00661                 if (mExecInterface != NULL) {
    00662                         return mExecInterface->getTask(task);
    00663                 } else {
    00664                         // This is equivalent to returning NULL.
    00665                         return t_TaskSharedPointer();
    00666                 }
    00667         }
    00668 }
    00669
    00671
    00672 set<DCInterface::TaskIdentifier>
    00673 DCInterface::getRecentlyFailedTasks()
    00674 {
    00675         set<DCInterface::TaskIdentifier> recentlyFailedTasks = m_recentlyFailedTasks;
    00676         if (mExecInterface != NULL)
    00677         {
    00678                 // Union this set with that of mExecInterface
    00679                 set<TaskIdentifier> otherRecentlyFailedTasks =
    00680                         mExecInterface->getRecentlyFailedTasks();
    00681                 recentlyFailedTasks.insert(otherRecentlyFailedTasks.begin(),otherRecentlyFailedTasks.end());
    00682         }
    00683         return recentlyFailedTasks;
    00684 }
    00685
    00687
    00688 set<DCInterface::TaskIdentifier>
    00689 DCInterface::getRecentlyCompletedTasks()
    00690 {
    00691         // Check for all tasks that were completed.
    00692         isDone();
    00693
    00694         set<DCInterface::TaskIdentifier> recentlyCompletedTasks = m_recentlyCompletedTasks;
    00695         if (mExecInterface != NULL)
    00696         {
    00697                 // Union this set with that of mExecInterface
    00698                 set<DCInterface::TaskIdentifier> otherRecentlyCompletedTasks =
    00699                         mExecInterface->getRecentlyCompletedTasks();
    00700                 recentlyCompletedTasks.insert(otherRecentlyCompletedTasks.begin(),otherRecentlyCompletedTasks.end());
    00701         }
    00702         return recentlyCompletedTasks;
    00703 }
    00704
    00706
    00707
    00709 // Interface Functions for ObservationTransformationInterface
    00711
    00712 void DCInterface::update(ValueIdentifier value)
    00713 {
    00714         // We've received an update from below - update ME
    00715
    00716         // See if we understand.
    00717         t_ValueMap::iterator itVM = mValueIDMap.find(value);
    00718
    00719         if(itVM != mValueIDMap.end()) {
    00720                 t_VarValPair pVals = (*itVM).second;
    00721
    00722                 int varID = pVals.first;
    00723                 int valID = pVals.second;
    00724                 mCurrentState[varID] = getDiscreteValue(varID, valID);
    00725                 updateListeners(mCurrentState[varID]);
    00726
    00727                 mMeRtapi->queue_observations_and_time(varID, valID);
    00728         }
    00729 }
    00730
    00732
    00733 void DCInterface::update(ContinuousVariableIdentifier varID, double value)
    00734 {
    00735         (void) varID;
    00736         (void) value;
    00737         // We've received an update from below - ignore since we're discrete
    00738
    00739         // Note: Should we record this and pass it to our parent?
    00740 }
    00741
    00743
    00744 DCInterface::DiscreteVariableIdentifier DCInterface::getDiscreteVariable(const char * varName)
    00745 {
    00746         // Retrieve the variable with the matching name.  Note that we
    00747         // override this because we don't want to keep 2 lists of all the
    00748         // variables and all the values.
    00749         for(unsigned int i = 0; i < mDVars.size(); ++i) {
    00750                 DiscreteVariable * var = mDVars[i];
    00751
    00752                 if(strcmp(var->getName(), varName) == 0)
    00753                         return var;
    00754         }
    00755         return NULL;
    00756 }
    00757
    00759
    00760 DCInterface::ValueIdentifier DCInterface::getValue(DiscreteVariableIdentifier varID, const char * valueName)
    00761 {
    00762         // Retrieve the value with the matching name.  Note that we
    00763         // override this because we don't want to keep 2 lists of all the
    00764         // variables and all the values.
    00765         DiscreteVariable * var = (DiscreteVariable *) varID;
    00766
    00767         // Note that our values are partially sorted.
    00768         const BaseModelVariable * bmVar = var->getVariable();
    00769         int base = mBaseOffset[bmVar->getUID()];
    00770         int length = base + bmVar->getDomain()->size();
    00771         for(int i = base; i < length; ++i) {
    00772                 DiscreteValue * value = mDVals[i];
    00773
    00774                 LOG4CPLUS_TRACE(m_Logger, "Loaded Value " << (void *) value <<
    00775                         " with name " << value->getName());
    00776
    00777                 if(strcmp(value->getName(), valueName) == 0) {
    00778                         return value;
    00779                 }
    00780         }
    00781         return NULL;
    00782 }
    00783
    00785
    00786 bool DCInterface::isValid(DiscreteVariableIdentifier varID)
    00787 {
    00788         // We should check that varID is one of our variable identifiers.
    00789         return varID != NULL;
    00790 }
    00791
    00793
    00794 bool DCInterface::isValid(ValueIdentifier valueID)
    00795 {
    00796         // We should check that valueID is one of our value identifiers.
    00797         return valueID != NULL;
    00798 }
    00799
    00801
    00802 DCInterface::ValueIdentifier DCInterface::getCurrentState(DiscreteVariableIdentifier varID)
    00803 {
    00804         // Retrieve the current state from ME for the variable varID.
    00805         DiscreteVariable * var = (DiscreteVariable *) varID;
    00806         return mCurrentState[var->getVariable()->getUID()];
    00807 }
    00808
    00810
    00811 double DCInterface::getCurrentState(ContinuousVariableIdentifier varID)
    00812 {
    00813         (void) varID;
    00814         // Note that we're discrete, so we cannot have a valid
    00815         // ContinuousVariableIdentifier.  Just return 0 for now
    00816
    00817         // maybe we should pass through values from our child?
    00818         return 0;
    00819 }
    00820
    00822
    00823 Mers::Utils::Lang::String
    00824 DCInterface::getCurrentStateStr() const {
    00825   // Each variable/value assignment is turned into a string and placed
    00826   // into a vector.
    00827   std::vector<Mers::Utils::Lang::String> assignments;
    00828   for (std::vector<DiscreteVariable*>::const_iterator it = mDVars.begin();
    00829            it != mDVars.end(); it++) {
    00830         Mers::Utils::Lang::String s((*it)->getName());
    00831         s.append(" = ");
    00832         s.append(mCurrentState[(*it)->getVariable()->getUID()]->getName());
    00833         assignments.push_back(s);
    00834   }
    00835
    00836   // Alphabetize the assignments.
    00837   sort(assignments.begin(), assignments.end());
    00838
    00839   // Turn the individual assignment strings into one big string.
    00840   std::ostringstream os;
    00841   for (std::vector<Mers::Utils::Lang::String>::const_iterator it = assignments.begin();
    00842            it != assignments.end(); it++) {
    00843         os << (*it) << std::endl;
    00844   }
    00845
    00846   return Mers::Utils::Lang::String(os.str());
    00847 }
    00848
    00850 // Interface Functions for Mode Estimation
    00852
    00853 void DCInterface::setInitialState(unsigned int state_index,
    00854         unsigned int value_index)
    00855 {
    00856         mMeRtapi->queue_initial_state(state_index, value_index);
    00857 }
    00858
    00860
    00861 void DCInterface::requestAbort()
    00862 {
    00863         mMeRtapi->queue_abort_command();
    00864 }
    00865
    00867
    00868 void DCInterface::requestFullState()
    00869 {
    00870         m_MEIdle = false;
    00871         mMeRtapi->queue_report_full_state();
    00872 }
    00873
    00875
    00876 int DCInterface::loadInitialState(char const * initFileName,
    00877         BaseModel const * model)
    00878 {
    00879         FILE * initFile;
    00880         char variable[500];
    00881         char value[500];
    00882         unsigned int end, lastNonSpace;
    00883
    00884         // Open our file.
    00885         initFile = fopen(initFileName, "r");
    00886         // Make sure we found it.
    00887         if(!initFile){
    00888                 const char * errorMsg = strerror( errno );
    00889                 LOG4CPLUS_ERROR( m_Logger, "Error opening the file " << initFileName <<
    00890                         ": " << errorMsg);
    00891                 return 1;
    00892         }
    00893
    00894         // Parse the file until we reach the end.
    00895         while(!feof(initFile)){
    00896                 // Reset the end of current word to the beginning.
    00897                 end = 0;
    00898                 // Reset the location of the last non-space to the start.
    00899                 lastNonSpace = 1;
    00900                 // Read until we get a non-space character.
    00901                 do{
    00902                         // Get a character.
    00903                         variable[0] = getc(initFile);
    00904                         // If we reached the end, quit.
    00905                         if(variable[0] == EOF){
    00906                                 break;
    00907                         }
    00908                         // See what we found.
    00909                         if(variable[0] == '/'){
    00910                                 // A comment character was found.  Get the next character.
    00911                                 variable[1] = getc(initFile);
    00912                                 // If end of file - stop looking.
    00913                                 if(variable[1] == EOF){
    00914                                         break;
    00915                                 }
    00916                                 // See if the next character is also a comment character.
    00917                                 if(variable[1] == '/'){
    00918                                         // The rest of this line is a comment - skip over it.
    00919                                         do{
    00920                                                 // Read a character.
    00921                                                 variable[0] = getc(initFile);
    00922                                                 // Stop when we reach the end of the line or file.
    00923                                         }while(variable[0] != '\n' && variable[0] != EOF);
    00924                                         // If we reached the end of the file, stop reading.
    00925                                         if(variable[0] == EOF){
    00926                                                 break;
    00927                                         }
    00928                                 }else{
    00929                                         // This isn't a comment - keep it for a token.  Note that
    00930                                         // we've now read 2 characters.
    00931                                         ++end;
    00932                                         // If the last character is not a space, increments the
    00933                                         // non-space count as well.
    00934                                         if(variable[1] != ' ' && variable[1] != '\t' && variable[1] != '\n')
    00935                                                 lastNonSpace = 2;
    00936                                 }
    00937                         }
    00938                         // Continue while we only have spaces.
    00939                 }while(variable[0] == ' ' || variable[0] == '\t' ||
    00940                                          variable[0] == '\n');
    00941                 // If we exited due to an EOF character, exit this level as well.
    00942                 if(variable[0] == EOF)
    00943                         break;
    00944                 // We just read a character - add one to end.
    00945                 ++end;
    00946
    00947                 // Keep reading until we find an '=' sign, recording the
    00948                 // variable name.  Note that we skip over '\n' characters.
    00949                 do{
    00950                         // Skip any '\n' characters.
    00951                         do{
    00952                                 // Keep reading characers as long as we're reading '\n' characters.
    00953                                 variable[end] = getc(initFile);
    00954                         }while(variable[end] == '\n');
    00955
    00956                         // if we found the end of the file, generate an error.
    00957                         if(variable[end] == EOF){
    00958                                 variable[end] = '\0';
    00959
    00960                                 LOG4CPLUS_ERROR_STR(m_Logger, "Unexpected end of file");
    00961                                 LOG4CPLUS_ERROR(m_Logger, "Expected '=' after the variable '" <<
    00962                                         variable << "'");
    00963
    00964                                 // Close the file we were loading and return an error.
    00965                                 fclose(initFile);
    00966                                 return 2;
    00967                         }
    00968                         // If we read a non-space, make sure we're aware of where the
    00969                         // end of the word is now located.
    00970                         if(variable[end] != ' ' && variable[end] != '\t' && variable[end] != '=')
    00971                                 lastNonSpace = end;
    00972                         // Continue until we find the '=' sign.
    00973                 }while(variable[end++] != '=');
    00974                 // End our variable name after the last real character.
    00975                 variable[lastNonSpace + 1] = '\0';
    00976
    00977                 // Skip over any spaces until the start of the value name.
    00978                 do{
    00979                         // Get a character.
    00980                         value[0] = getc(initFile);
    00981                         // If we found the end of the file, generate an error.
    00982                         if(value[0] == EOF){
    00983                                 LOG4CPLUS_ERROR_STR(m_Logger, "Unexpected end of file");
    00984                                 LOG4CPLUS_ERROR(m_Logger, "Expected a value for the variable '" <<
    00985                                         variable << "'");
    00986
    00987                                 // Close the file we were loading and return an error.
    00988                                 fclose(initFile);
    00989                                 return 2;
    00990                         }
    00991                         // Continue while we see white space.
    00992                 }while(value[0] == ' ' || value[0] == '\t' ||
    00993                                          value[0] == '\n');
    00994
    00995                 // We now have one character
    00996                 end = 1;
    00997                 // And one non-space character.
    00998                 lastNonSpace = end;
    00999                 // Read the value until the end of the line.
    01000                 do{
    01001                         // Get a character.
    01002                         value[end] = getc(initFile);
    01003                         // If we found the end of the file, stop.
    01004                         if(value[end] == EOF){
    01005                                 break;
    01006                         }
    01007                         // If we found a non-whitespace character, record it.
    01008                         if(value[end] != ' ' && value[end] != '\t' &&
    01009         value[end] != '=' && value[end] != '\n')
    01010                                 lastNonSpace = end;
    01011                         // Stop when we find the end of the file.
    01012                 }while(value[end++] != '\n');
    01013                 // If we stopped for the end of the file or line, clear the
    01014                 // character to NULL.
    01015                 value[lastNonSpace + 1] = '\0';
    01016
    01017                 // Find the assignment specified by this variable/value pair.
    01018                 BaseModelAssignment * asg =
    01019                         model->getAssignment(variable, value, BaseModelAssignment::Positive);
    01020
    01021                 // If the variable is NULL, generate an error.
    01022                 if(asg == NULL || asg->getVariable() == NULL){
    01023                         LOG4CPLUS_ERROR(m_Logger, "The variable '" << variable
    01024                                 << "' was not found in the model");
    01025
    01026                         // Close the file we were loading and return an error.
    01027                         fclose(initFile);
    01028                         return 3;
    01029                 }
    01030                 // If the value is NULL, generate an error.
    01031                 if(asg->getValue() == NULL){
    01032                         LOG4CPLUS_ERROR(m_Logger, "The value '" << value
    01033                                 << "' was not in the domain of the variable "
    01034                                 << variable);
    01035
    01036                         // Close the file we were loading and return an error.
    01037                         fclose(initFile);
    01038                         return 3;
    01039                 }
    01040                 // Add this variable/value pair to the initial state.
    01041                 setInitialState(asg->getVariable()->getUID(), asg->getValue()->getUID());
    01042         }
    01043
    01044         // Request a full state update with this initial state.
    01045         requestFullState();
    01046
    01047         // Close the file we were loading.
    01048         fclose(initFile);
    01049   return 0;
    01050 }
    01051
    01053 // Interface Functions for Mode Reconfiguration
    01055
    01056 void DCInterface::requestMR(void)
    01057 {
    01058         mMrRtapi->queue_request_MR();
    01059 }
    01060
    01062 // Other protected functions
    01064
    01065 DiscreteValue * DCInterface::getDiscreteValue(int varIndex, int valIndex)
    01066 {
    01067                 return mDVals[getAbsoluteIndex(varIndex, valIndex)];
    01068 }
    01069
    01071
    01072 int DCInterface::getAbsoluteIndex(int varIndex, int valIndex)
    01073 {
    01074         return mBaseOffset[varIndex] + valIndex;
    01075 }
    01076
    01078
    01079 DCInterface::DCInterface(BaseModel * model)
    01080         : RTAPICallback(), ExecutionInterfaceBase( NULL ),
    01081                 ObservationTransformationInterface(), mExecInterface(NULL), mTasks(),
    01082                 mBaseOffset(), mModel(NULL), mDVars(), mDVals(), mMrFailed(false),
    01083                 mActiveGoals(), mCurrentState(),
    01084                 mMeRtapi(NULL), mMrRtapi(NULL), m_MRIdle(true), m_MEIdle(true)
    01085 {
    01086         init(model);
    01087 }
    01088
    01090
    01091 void DCInterface::init(BaseModel * model)
    01092 {
    01093         TaskFactory * factory = new TaskFactory(model);
    01094         setTaskFactory(factory);
    01095
    01096         // Setup observation information.
    01097         BaseModelVariableArray &vars = *model->getVariables();
    01098
    01099         unsigned int numVars = vars.getLength();
    01100         unsigned int numVals = 0;
    01101
    01102         // Count the number of assignments.
    01103         for(unsigned int i = 0; i < numVars; ++i) {
    01104                 BaseModelVariable * var = vars[i];
    01105
    01106                 // Keep a running index - assume that the UID's are sorted and compact
    01107                 mBaseOffset.pushBack(numVals);
    01108                 // Count the number of values in this next domain.
    01109                 numVals += var->getDomain()->getLength();
    01110         }
    01111
    01112         mDVars.resize(numVars);
    01113         mDVals.resize(numVals);
    01114         mCurrentState.resize(numVars);
    01115
    01116         // Create all of our observations.
    01117         unsigned int index = 0;
    01118         for(unsigned int i = 0; i < vars.getLength(); ++i) {
    01119                 BaseModelVariable * var = vars[i];
    01120                 BaseModelDomain &dom = *var->getDomain();
    01121
    01122                 mDVars[i] = new DiscreteVariable(var);
    01123
    01124                 for(unsigned int j = 0; j < dom.getLength(); ++j, ++index) {
    01125                         BaseModelValue * val = dom[j];
    01126
    01127                         mDVals[index] = new DiscreteValue(mDVars[i], val);
    01128                 }
    01129
    01130                 mCurrentState[i] = mDVals[index - 1];
    01131         }
    01132 }