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

# /Users/fpy/titan-1-5-MBARI/Mers/Planning/Reactive/CCA/reactivePlanner.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 // reactivePlanner class
    00014 //
    00015 //=============================================================================
    00016
    00017 #include "reactivePlanner.h"
    00018
    00019 #include <cstdio>
    00020 #include <cassert>
    00021
    00022 #include "rpCCAutomata.h"
    00023 #include "rpAutomaton.h"
    00024 #include "rpStates.h"
    00025 #include "rpTransition.h"
    00026
    00028
    00029 using Mers::Models::CCA::CCAModel;
    00030 using Mers::Models::CCA::CCAAssignments;
    00031
    00033 // Constructors, Copy Constructors, & Destructors
    00035
    00036 reactivePlanner::reactivePlanner() {
    00037         _model_p                                = NULL;
    00038         _initialized                    = false;
    00039         _status_update_required = true;
    00040 }
    00041 reactivePlanner::reactivePlanner(const CCAModel &_model) :
    00042 _rpCCAutomata(*_model.ccAutomata_p()) {
    00043         _model_p                                = &_model;
    00044         _initialized                    = false;
    00045         _status_update_required = true;
    00046 }
    00047 /*reactivePlanner::reactivePlanner(const reactivePlanner &other) {
    00048         if (this != &other) {
    00049                 copy(other);
    00050         }
    00051 }*/
    00052 reactivePlanner::~reactivePlanner() {
    00053         destroy();
    00054 }
    00055 //------------------------------------------------------------------------------
    00056 // Public Function Members
    00057 //------------------------------------------------------------------------------
    00058 const CCAModel *const reactivePlanner::model_p() const {
    00059         return _model_p;
    00060 }
    00061 const bool
    00062 reactivePlanner::update_current_state(const unsigned int rpAutomaton_index,
    00063                                                                           const unsigned int rpState_index) {
    00064         if (rpAutomaton_index < _rpCCAutomata.size()) {
    00065                 if (rpState_index <
    00066                         _rpCCAutomata[rpAutomaton_index].rpStates_p()->size()) {
    00067                         _rpCCAutomata[rpAutomaton_index].set_current_rpState(
    00068                                 *_rpCCAutomata[rpAutomaton_index].rpStates_p(rpState_index));
    00069                         if (_rpCCAutomata[rpAutomaton_index].current_rpState_p()->
    00070                                 reachability() == rpState::NEVER) {
    00071                                 _status_update_required = true;
    00072                         }
    00073                         bool initialize = true;
    00074                         if (_initialized == false) {
    00075                                 for (unsigned int i = 0; i < _rpCCAutomata.size(); i++) {
    00076                                         if (_rpCCAutomata[i].current_rpState_p() == NULL) {
    00077                                                 initialize = false;
    00078                                         }
    00079                                 }
    00080                                 if (initialize == true) {
    00081                                         update_status(initialize);
    00082                                         _initialized = true;
    00083                                 }
    00084                         }
    00085                         return true;
    00086                 }
    00087         }
    00088         return false;
    00089 }
    00090 const bool
    00091 reactivePlanner::update_goal_state(const unsigned int rpAutomaton_index,
    00092                                                                    const unsigned int rpState_index) {
    00093         if (rpAutomaton_index < _rpCCAutomata.size()) {
    00094                 if (rpState_index <
    00095                         _rpCCAutomata[rpAutomaton_index].rpStates_p()->size()) {
    00096                         _rpCCAutomata[rpAutomaton_index].set_goal_rpState(
    00097                                 *_rpCCAutomata[rpAutomaton_index].rpStates_p(rpState_index));
    00098                         return true;
    00099                 }
    00100         }
    00101         return false;
    00102 }
    00103 const CCAAssignments *const
    00104 reactivePlanner::generate_next_command_assignments_p(rpResultType &result) {
    00105         unsigned int i;
    00106         if (_status_update_required) {
    00107                 update_status();
    00108                 _status_update_required = false;
    00109         }
    00110         for (i = 0; i < _rpCCAutomata.size(); i++) {
    00111                 if (_rpCCAutomata[i].goal_rpState_p()->
    00112                         reachability() != rpState::ALWAYS) {
    00113                         result = FAILED;
    00114                         return NULL;
    00115                 }
    00116         }
    00117         for (i = 0; i < _rpCCAutomata.size(); i++) {
    00118                 if (_rpCCAutomata[i].goal_rpState_p() !=
    00119                         _rpCCAutomata[i].current_rpState_p()) {
    00120                         result = COMMAND;
    00121                         return generate_next_commands_for_subgoal(
    00122                                 *_rpCCAutomata[i].goal_rpState_p());
    00123                 }
    00124         }
    00125         result = ACHIEVED;
    00126         return NULL;
    00127 }
    00128 //------------------------------------------------------------------------------
    00129 // Public Operators
    00130 //------------------------------------------------------------------------------
    00131 std::ostream &
    00132 operator <<(std::ostream &_ostream, const reactivePlanner &_reactivePlanner) {
    00133         _ostream << "reactivePlanner:" << std::endl;
    00134         for (unsigned int i = 0; i < _reactivePlanner._rpCCAutomata.size(); i++) {
    00135                 _ostream << _reactivePlanner._rpCCAutomata[i] << std::endl;
    00136         }
    00137         return _ostream;
    00138 }
    00139 //------------------------------------------------------------------------------
    00140 // Private Function Members
    00141 //------------------------------------------------------------------------------
    00142 const CCAAssignments *const
    00143 reactivePlanner::generate_next_commands_for_subgoal(const rpState &
    00144                                                                                                    subgoal_rpState) {
    00145         const rpTransition *rpTransition_p =
    00146                 subgoal_rpState.rpAutomaton_p()->rpTransition_p(subgoal_rpState);
    00147         if (rpTransition_p == NULL) {
    00148                 return NULL;
    00149         } else {
    00150                 const rpState *new_subgoal_rpState_p =
    00151                         rpTransition_p->unsatisfied_stateCondition_p();
    00152                 if (new_subgoal_rpState_p == NULL) {
    00153                         return rpTransition_p->controlConditions_p();
    00154                 } else {
    00155                         return generate_next_commands_for_subgoal(*new_subgoal_rpState_p);
    00156                 }
    00157         }
    00158 }
    00159 void reactivePlanner::update_status(const bool initializing) {
    00160         // Update reactivePlanner status if any rpAutomaton exists
    00161         if (_rpCCAutomata.size() > 0) {
    00162                 // Update reactivePlanner in decreasing order.
    00163                 // Note that the cause > effect. Since the status of the cause has to be
    00164                 // updated before the effect, the statuss update must be performed in
    00165                 // decreasing order.
    00166                 bool predecessor_status_changed = false;
    00167                 for (unsigned int i = _rpCCAutomata.size(); i > 0; i--) {
    00168                         if (_rpCCAutomata[i-1].update_status(predecessor_status_changed,
    00169                                                                                                  initializing) ||
    00170                                 initializing) {
    00171                                 predecessor_status_changed = true;
    00172                                 _rpCCAutomata[i-1].update_policies();
    00173                         }
    00174                 }
    00175         }
    00176 }
    00177 void reactivePlanner::destroy() {
    00178         _model_p = NULL;
    00179 }
    00180 void reactivePlanner::copy(const reactivePlanner &other) {
    00181         _model_p                                = other._model_p;
    00182         _rpCCAutomata                   = other._rpCCAutomata;
    00183         _initialized                    = other._initialized;
    00184         _status_update_required = other._status_update_required;
    00185 }