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

# /Users/fpy/titan-1-5-MBARI/Mers/Planning/Reactive/CCA/rpInterface.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 // rpInterface class
    00014 //
    00015 //=============================================================================
    00016
    00017 #include <cstdio>
    00018 #include <cassert>
    00019
    00020 #include <Mers/Models/CCA/CCAVariables.h>
    00021 #include <Mers/Models/CCA/CCAVariable.h>
    00022 #include <Mers/Models/CCA/CCADomain.h>
    00023 #include <Mers/Models/CCA/CCAValue.h>
    00024
    00025 #include "reactivePlanner.h"
    00026 #include "rpCCAutomata.h"
    00027 #include "rpAutomaton.h"
    00028 #include "rpStates.h"
    00029 #include "rpState.h"
    00030 #include "rpInterface.h"
    00031
    00033
    00034 using Mers::Models::CCA::CCAVariables;
    00035 using Mers::Models::CCA::CCAAssignments;
    00036 using Mers::Models::CCA::CCADomain;
    00037 using Mers::Utils::Containers::intList;
    00038 using std::cerr;
    00039 using std::endl;
    00040
    00042 // Constructors, Copy Constructors, & Destructors
    00044
    00045 rpInterface::rpInterface(reactivePlanner &_reactivePlanner) {
    00046         _reactivePlanner_p = &_reactivePlanner;
    00047         const CCAVariables *                    _variables_p = _reactivePlanner_p->
    00048                                                                                            model_p()->variables_p();
    00049         CCAVariables::const_iterator    variables_const_it;
    00050         CCADomain::const_iterator               values_const_it;
    00051         unsigned int                            rpAutomaton_index;
    00052         unsigned int                            rpState_index;
    00053         _to_rp_index_conversionChart.initialize(0, _variables_p->getLength());
    00054         for (variables_const_it = _variables_p->begin();
    00055         variables_const_it != _variables_p->end();
    00056         variables_const_it++) {
    00057                 for (rpAutomaton_index = 0;
    00058                 rpAutomaton_index < _reactivePlanner_p->_rpCCAutomata.size();
    00059                 rpAutomaton_index++) {
    00060                         if (_reactivePlanner_p->_rpCCAutomata[rpAutomaton_index].
    00061                                 variable_p() == &*variables_const_it) {
    00062                                 break;
    00063                         }
    00064                 }
    00065                 if (rpAutomaton_index < _reactivePlanner_p->_rpCCAutomata.size()) {
    00066                         _to_rp_index_conversionChart[variables_const_it->getUID()].initialize(
    00067                                 rpAutomaton_index,
    00068                                 _reactivePlanner_p->_rpCCAutomata[rpAutomaton_index].
    00069                                 variable_p()->getDomain()->getLength());
    00070                         for(values_const_it = variables_const_it->getDomain()->begin();
    00071                         values_const_it != variables_const_it->getDomain()->end();
    00072                         values_const_it++) {
    00073                                 for (rpState_index = 0;
    00074                                 rpState_index < _reactivePlanner_p->
    00075                                         _rpCCAutomata[rpAutomaton_index].rpStates_p()->size();
    00076                                 rpState_index++) {
    00077                                         if (_reactivePlanner_p->_rpCCAutomata[rpAutomaton_index].
    00078                                                 rpStates_p(rpState_index)->value_p() ==
    00079                                                 &*values_const_it) {
    00080                                                 break;
    00081                                         }
    00082                                 }
    00083                                 _to_rp_index_conversionChart[
    00084                                         variables_const_it->getUID()][values_const_it->getUID()].initialize(
    00085                                         rpState_index);
    00086                         }
    00087                 }
    00088         }
    00089 }
    00090 rpInterface::~rpInterface() {
    00091         _reactivePlanner_p = NULL;
    00092 }
    00093 //------------------------------------------------------------------------------
    00094 // Public Function Members
    00095 //------------------------------------------------------------------------------
    00096 void rpInterface::update_current_state(const unsigned int variable_id,
    00097                                                                            const unsigned int value_id) const {
    00098         _reactivePlanner_p->update_current_state(
    00099                 _to_rp_index_conversionChart[variable_id].index(),
    00100                 _to_rp_index_conversionChart[variable_id][value_id].index());
    00101 }
    00102 void rpInterface::update_goal_state(const unsigned int variable_id,
    00103                                                                         const unsigned int value_id) const {
    00104         _reactivePlanner_p->update_goal_state(
    00105                 _to_rp_index_conversionChart[variable_id].index(),
    00106                 _to_rp_index_conversionChart[variable_id][value_id].index());
    00107 }
    00108
    00109 void rpInterface::update_goal_states(const intList &goal_intList)  const
    00110 {
    00111         unsigned int               variable_id;
    00112         intList::const_iterator    intList_const_it;
    00113         for (intList_const_it = goal_intList.begin();
    00114              intList_const_it != goal_intList.end(); intList_const_it++)
    00115         {
    00116                 variable_id = *intList_const_it;
    00117                 intList_const_it++;
    00118                 if (intList_const_it == goal_intList.end())
    00119                 {
    00120                         cerr << "Error: odd length goal list given to rpInterface.update_goal_states()!"
    00121                              << endl;
    00122                         break;
    00123                 } else
    00124                 {
    00125                         update_goal_state(variable_id, *intList_const_it);
    00126                 }
    00127         }
    00128 }
    00129
    00130 const intList *const rpInterface::generate_next_command_intList_p() {
    00131         _command_intList.clear();
    00132         reactivePlanner::rpResultType   result;
    00133         CCAAssignments::const_iterator          assignments_const_it;
    00134         const CCAAssignments *                          command_assignments_p;
    00135         command_assignments_p =
    00136                 _reactivePlanner_p->generate_next_command_assignments_p(result);
    00137         _command_intList.push_back(result);
    00138         if (command_assignments_p != NULL) {
    00139                 for (assignments_const_it = command_assignments_p->begin();
    00140                 assignments_const_it != command_assignments_p->end();
    00141                 assignments_const_it++) {
    00142                         _command_intList.push_back
    00143                                 (assignments_const_it->getVariable()->getUID());
    00144                         _command_intList.push_back(
    00145                                 assignments_const_it->getValue()->getUID());
    00146                 }
    00147         }
    00148         return &_command_intList;
    00149 }
    00150 const intList *const rpInterface::reachable_state_intList_p() {
    00151         _reachable_state_intList.clear();
    00152         if (_reactivePlanner_p->_status_update_required) {
    00153                 _reactivePlanner_p->update_status();
    00154                 _reactivePlanner_p->_status_update_required = false;
    00155         }
    00156         unsigned int i, j;
    00157         for (i = 0; i <_reactivePlanner_p->_rpCCAutomata.size(); i++) {
    00158                 for (j = 0;
    00159                 j <_reactivePlanner_p->_rpCCAutomata[i].rpStates_p()->size();
    00160                 j++) {
    00161                         if (_reactivePlanner_p->_rpCCAutomata[i].rpStates_p(j)->
    00162                                 reachability() == rpState::ALWAYS) {
    00163                                 _reachable_state_intList.push_back(
    00164                                         _reactivePlanner_p->_rpCCAutomata[i].variable_p()->getUID());
    00165                                 _reachable_state_intList.push_back(
    00166                                         _reactivePlanner_p->_rpCCAutomata[i].rpStates_p(j)->
    00167                                         value_p()->getUID());
    00168                         }
    00169                 }
    00170         }
    00171         return &_reachable_state_intList;
    00172 }
    00173 //------------------------------------------------------------------------------
    00174 // Public Operators
    00175 //------------------------------------------------------------------------------
    00176 std::ostream &operator<<(std::ostream &_ostream,
    00177                                                  const rpInterface &_rpInterface) {
    00178         if (_rpInterface._reactivePlanner_p != NULL) {
    00179                 if (_rpInterface._reactivePlanner_p->model_p() != NULL) {
    00180                         CCADomain::const_iterator               values_const_it;
    00181                         CCAVariables::const_iterator    variables_const_it =
    00182                                 _rpInterface._reactivePlanner_p->model_p()->variables_p()->
    00183                                 begin();
    00184                         while (variables_const_it !=
    00185                                 _rpInterface._reactivePlanner_p->model_p()->variables_p()->
    00186                                 end()) {
    00187                                 _ostream << "Variable: ";
    00188                                 _ostream << variables_const_it->getName();
    00189                                 _ostream << "(" << variables_const_it->getUID() << ")" << std::endl;
    00190                                 _ostream << "Values:";
    00191                                 values_const_it = variables_const_it->getDomain()->begin();
    00192                                 while (values_const_it !=
    00193                                         variables_const_it->getDomain()->end()) {
    00194                                         _ostream << "\t" << values_const_it->getName();
    00195                                         _ostream << "(" << values_const_it->getUID() << ")";
    00196                                         if (++values_const_it !=
    00197                                                 variables_const_it->getDomain()->end()) {
    00198                                                 _ostream << std::endl;
    00199                                         }
    00200                                 }
    00201                                 if (++variables_const_it != _rpInterface.
    00202                                         _reactivePlanner_p->model_p()->variables_p()->end()) {
    00203                                         _ostream << std::endl;
    00204                                 }
    00205                         }
    00206                 }
    00207         }
    00208         return _ostream;
    00209 }
    00210 //------------------------------------------------------------------------------
    00211 // Private Function Members
    00212 //------------------------------------------------------------------------------
    00213 rpInterface::indexArray::indexArray() {
    00214         _size           = 0;
    00215         _indexArray     = NULL;
    00216 }
    00217 rpInterface::indexArray::~indexArray() {
    00218         if (_indexArray != NULL) {
    00219                 delete [] _indexArray;
    00220         }
    00221 }
    00222 const unsigned int rpInterface::indexArray::index() const {
    00223         return _index;
    00224 }
    00225 const unsigned int rpInterface::indexArray::size() const {
    00226         return _size;
    00227 }
    00228 void rpInterface::indexArray::
    00229 initialize(const unsigned int index_, const unsigned int sub_indexArray_size) {
    00230         _index = index_;
    00231         if (_indexArray == NULL && sub_indexArray_size > 0) {
    00232                 _size           = sub_indexArray_size;
    00233                 _indexArray = new indexArray[sub_indexArray_size];
    00234         }
    00235 }
    00236 rpInterface::indexArray &
    00237 rpInterface::indexArray::operator [](const unsigned int index_) {
    00238         if (_indexArray != NULL && index_ < _size) {
    00239                 return _indexArray[index_];
    00240         } else {
    00241                 return *this;
    00242         }
    00243 }
    00244 const rpInterface::indexArray &
    00245 rpInterface::indexArray::operator [](const unsigned int index_) const {
    00246         if (_indexArray != NULL && index_ < _size) {
    00247                 return _indexArray[index_];
    00248         } else {
    00249                 return *this;
    00250         }
    00251 }
    00252