planner.cpp

   1 #include "planner.h"
   2 #include "node.h"
   3 #include <iostream>
   4 using namespace std;
   5 using namespace __gnu_cxx;
   6
   7 Planner::Planner(std::vector<Action> actions, literals init, literals goal){
   8
   9         _start = new Node(Action("start",literals(), init));
  10         Node* finish = new Node(Action("finish",goal,literals()));
  11
  12         for(vector<Action>::iterator action = actions.begin(); action != actions.end(); ++action){
  13                 const literals& effects = action->effects();
  14                 for (literals::const_iterator effect = effects.begin(); effect != effects.end(); ++effect){
  15                         cerr << "Adding effect: " << *effect << endl;
  16                         _actions[*effect] = *action;
  17                 }
  18         }
  19         makePlan(finish);
  20 }
  21
  22 Planner::~Planner(){
  23         for (hash_map<string,Node*>::iterator node = _addedNodes.begin(); node != _addedNodes.end(); ++node){
  24                 cerr << "Deleting node " << node->second->action().executable() << endl;
  25                 delete node->second;
  26         }
  27 }
  28
  29
  30 void Planner::makePlan(Node* node){
  31         cerr << "Fetching preconditions for action: " << node->action().executable() << ".. ";
  32         const literals& preconds = node->action().preconditions();
  33         cerr << "done" << endl;
  34
  35         addNode(node);
  36
  37         if (preconds.size() == 0){
  38                 cerr << "Found no preconds" << endl;
  39                 _start->addChild(node);
  40         }else{
  41                 for (literals::const_iterator precond = preconds.begin(); precond != preconds.end(); ++precond){
  42                         cerr << "Looking for: " << *precond << endl;
  43                         hash_map<string,Node*>::iterator addedNode = _addedNodes.find(*precond);
  44                         if(addedNode != _addedNodes.end()){
  45                                 cerr << "Using already added node" << endl;
  46                                 addedNode->second->addChild(node);
  47                         }else {
  48                                 hash_map<string, Action>::iterator action = _actions.find(*precond);
  49                                 if (action != _actions.end()){
  50                                         cerr << "Adding new node" << endl;
  51                                         Node* newnode = new Node(action->second);
  52                                         newnode->addChild(node);
  53                                         makePlan(newnode);
  54                                 }else{
  55                                         cerr << "Action with effect: " << *precond << " not found!" << endl;
  56                                 }
  57                         }
  58                 }
  59         }
  60 }
  61
  62 void Planner::addNode(Node* node){
  63         const literals& effects = node->action().effects();
  64
  65         for (literals::const_iterator effect = effects.begin(); effect != effects.end(); ++effect){
  66                 cout << "Adding node for effect: " << *effect << endl;
  67                 _addedNodes[*effect] = node;
  68         }
  69 }
  70
  71
  72 void Planner::execute(){
  73         _start->execute(literals());
  74 }