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

# /Users/fpy/titan-1-5-MBARI/Mers/Reconfiguration/Discrete/modeReconfiguration.cpp

    00001 //=============================================================================
    00002 //
    00003 // Copyright (c) 2001           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Author:                      Seung H. Chung
    00010 // Organization:        Model-based Embedded and Robotic Systems Group
    00011 //                                      Massachusetts Institute of Technology
    00012 //
    00013 // modeReconfiguration class
    00014 //
    00015 //=============================================================================
    00016 #include <cstdlib>
    00017 #include <cstdio>
    00018 #include <cassert>
    00019 #include <Mers/Utils/Containers/intList.h>
    00020 #include <Mers/Planning/Reactive/CCA/reactivePlanner.h>
    00021 #include <Mers/Solvers/Entailment/GoalInterpreterInterface.h>
    00022 #include <Mers/Planning/Reactive/CCA/rpInterface.h>
    00023
    00024 #include "modeReconfiguration.h"
    00025
    00027
    00028 using Mers::Solvers::Entailment::GoalInterpreterInterface;
    00029 using Mers::Models::CCA::CCAModel;
    00030 using Mers::Utils::Containers::intList;
    00031 using Mers::Utils::Lang::String;
    00032 using std::cout;
    00033 using std::endl;
    00034 using std::list;
    00035
    00037 // Public Constructors, Copy Constructors, & Destructors
    00039
    00040 modeReconfiguration::modeReconfiguration(CCAModel const & _model,
    00041                                          GoalInterpreterInterface * giInterface)
    00042 : _reactivePlanner(new reactivePlanner(_model)),
    00043   _rpInterface(new rpInterface(*_reactivePlanner)),
    00044   goalInterpreter(giInterface), debug(false)
    00045 {
    00048 }
    00049
    00050 modeReconfiguration::~modeReconfiguration()
    00051 {
    00052         if (goalInterpreter)
    00053                 delete goalInterpreter;
    00054         if (_rpInterface)
    00055                 delete _rpInterface;
    00056         if (_reactivePlanner)
    00057                 delete _reactivePlanner;
    00058 }
    00059
    00060 //------------------------------------------------------------------------------
    00061 // Public Function Members
    00062 //------------------------------------------------------------------------------
    00063
    00064 void
    00065 modeReconfiguration::setDebug(bool debugState)
    00066 {
    00067         debug = debugState;
    00068         goalInterpreter->setDebug(debugState);
    00069 }
    00070
    00071
    00072 bool
    00073 modeReconfiguration::getDebug()  const
    00074 {
    00075         return debug;
    00076 }
    00077
    00078
    00079 bool
    00080 modeReconfiguration::update_current_state(unsigned int variable_id,
    00081                                           unsigned int value_id)
    00082 {
    00083         bool res;
    00084         res = goalInterpreter->updateCurrentState(variable_id, value_id);
    00085         _rpInterface->update_current_state(variable_id, value_id);
    00086         return res;
    00087 }
    00088
    00089
    00090 bool
    00091 modeReconfiguration::update_current_state(String variable_name,
    00092                                           String value_name)
    00093 {
    00094         bool res;
    00095         unsigned int varUID, valUID;
    00096
    00097         res = goalInterpreter->updateCurrentState(variable_name, value_name);
    00098         if (!res)
    00099                 return false;
    00100         if(FAILED(goalInterpreter->variableAndValueToUID(variable_name, value_name, varUID, valUID)))
    00101                 return false;
    00102         _rpInterface->update_current_state(varUID, valUID);
    00103         return true;
    00104 }
    00105
    00106
    00107 bool
    00108 modeReconfiguration::update_current_state(intList const * updates)
    00109 {
    00110         assert(updates);
    00111         assert(!(updates->size() % 2));
    00112         bool anyFailed = false;
    00113         intList::const_iterator iIter;
    00114         for (iIter = updates->begin(); iIter != updates->end(); iIter++)
    00115         {
    00116                 unsigned int var = (unsigned int)(*iIter);
    00117                 iIter++;
    00118                 unsigned int val = (unsigned int)(*iIter);
    00119                 if (!update_current_state(var, val))
    00120                         anyFailed = true;
    00121         }
    00122         return !anyFailed;
    00123 }
    00124
    00125
    00126 bool
    00127 modeReconfiguration::update_current_state(list<String> const & updates)
    00128 {
    00129         assert(!(updates.size() % 2));
    00130         bool anyFailed = false;
    00131         list<String>::const_iterator iIter;
    00132         for (iIter = updates.begin(); iIter != updates.end(); iIter++)
    00133         {
    00134                 String var = (*iIter);
    00135                 iIter++;
    00136                 String val = (*iIter);
    00137                 if (!update_current_state(var, val))
    00138                         anyFailed = true;
    00139         }
    00140         return !anyFailed;
    00141 }
    00142
    00143
    00144 bool
    00145 modeReconfiguration::update_goal(unsigned int variable_id,
    00146                                  unsigned int value_id)
    00147 {
    00148         return goalInterpreter->updateGoal(variable_id, value_id);
    00149 }
    00150
    00151
    00152 bool
    00153 modeReconfiguration::update_goal(String variable_name,
    00154                                  String value_name)
    00155 {
    00156         return goalInterpreter->updateGoal(variable_name, value_name);
    00157 }
    00158
    00159
    00160 void
    00161 modeReconfiguration::clear_goal()
    00162 {
    00163         // _rpInterface->update_goal_state(variable_id, value_id) should be removed
    00164         // once _giInterface.update_goal(variable_id, value_id) is implemented.
    00165
    00166         goalInterpreter->clearGoal();
    00167         // ???
    00168         //_rpInterface->clear_goal_state();
    00169 }
    00170
    00171
    00172 intList const * const
    00173 modeReconfiguration::generate_reachable_states_intList_p()
    00174 {
    00175         intList const * reachables = _rpInterface->reachable_state_intList_p();
    00176         return reachables;
    00177 }
    00178
    00179
    00180 intList const * const
    00181 modeReconfiguration::generate_next_command_intList_p()
    00182 {
    00183         // Reactive Planner will need an interface function that returns reachable
    00184         // states. Considering how the GI needs RP's information, considering GI
    00185         // as a submodule of RP may make more sense. For now, implement as below.
    00186         //
    00187         // _rpInterface->reachable_states() should return an intList that corresponds
    00188         // to the reachable states and _giInterface.goal_states() should return
    00189         // an intList that corresponds to the goal states that the Goal Interpreter
    00190         // generated.
    00191         if (debug)
    00192                 cout << "asking for reachable states from RP." << endl;
    00193         intList const * reachables = _rpInterface->reachable_state_intList_p();
    00194         assert(reachables);
    00195         if (debug)
    00196         {
    00197                 cout << "got reachable states from RP." << endl;
    00198                 cout << "Reachable states from RP: " << *reachables << endl;
    00199         }
    00200         goalInterpreter->updateReachableStates(reachables);
    00201         if (debug)
    00202                 cout << "updating reachable states in GI." << endl;
    00203         /*# This delete causes the program to exit!!! ???? */
    00204         //delete reachables;
    00205         if (debug)
    00206                 cout << "updated reachable states in GI." << endl;
    00207         intList const * gs = goalInterpreter->generateGoalState();
    00208         if (debug)
    00209                 cout << "got goal state from GI." << endl;
    00210         if (!gs)  /*# GI could not find it - should we return null or use an empty goal set? */
    00211         {
    00212                 //      gs = new intList();
    00213                 // Send a signal for unreachable := 1.
    00214                 intList * res = new intList();
    00215                 res->push_back(2);
    00216                 return res;
    00217         }
    00218         if (debug)
    00219                 cout << "updating rp with goal state." << endl;
    00220         _rpInterface->update_goal_states(*gs);
    00221         if (debug)
    00222                 cout << "updated rp with goal state." << endl;
    00223         intList const * res = _rpInterface->generate_next_command_intList_p();
    00224         if (debug)
    00225                 cout << "got command from rp." << endl;
    00226         delete gs;
    00227         return res;
    00228 }
    00229
    00230
    00231 list<String>
    00232 modeReconfiguration::generate_next_command_stringlist()
    00233 {
    00234         intList const * il = generate_next_command_intList_p();
    00235
    00236         list<String> res;
    00237         if (il->empty())  // no result?
    00238                 return res;
    00239         intList::const_iterator iIter = il->begin();
    00240         if (il->size() % 2)
    00241         {
    00242                 int type = (*iIter);
    00243                 iIter++;
    00244                 switch (type)
    00245                 {
    00246                         case reactivePlanner::ACHIEVED :
    00247                                 res.push_back("ACHIEVED");
    00248                                 break;
    00249                         case reactivePlanner::COMMAND :
    00250                                 res.push_back("COMMAND");
    00251                                 break;
    00252                         case reactivePlanner::FAILED :
    00253                                 res.push_back("FAILED");
    00254                                 break;
    00255                         default :
    00256                                 res.push_back("UNKNOWN");
    00257                 }
    00258         }
    00259         if (il->size() < 3)
    00260                 return res;
    00261         unsigned int var = (unsigned int)(*iIter);
    00262         iIter++;
    00263         unsigned int val = (unsigned int)(*iIter);
    00264         iIter++;
    00265
    00266         String varString, valString;
    00267         if(FAILED(goalInterpreter->variableAndValueToString(var, val, varString, valString)))
    00268                 return res;
    00269         res.push_back(varString);
    00270         res.push_back(valString);
    00271         return res;
    00272 }
    00273
    00274
    00275 void
    00276 modeReconfiguration::showVariables()  const
    00277 {
    00278         goalInterpreter->showVariables();
    00279 }
    00280
    00281
    00282 //------------------------------------------------------------------------------
    00283 // Public Operators
    00284 //------------------------------------------------------------------------------
    00285
    00286 std::ostream &
    00287 operator<<(std::ostream & _ostream, modeReconfiguration const & _modeReconfiguration)
    00288 {
    00289         _ostream << *(_modeReconfiguration._rpInterface);
    00290         return _ostream;
    00291 }
    00292
    00293
    00294 //------------------------------------------------------------------------------
    00295 // Private Function Members
    00296 //------------------------------------------------------------------------------
    00297
    00298
    00299