From 318003cdb8615b39ef71761f55ac2159caabbf47 Mon Sep 17 00:00:00 2001 From: Michael Andreen Date: Thu, 2 Jun 2005 17:59:44 +0000 Subject: [PATCH] major bugfix --- action.cpp | 14 +++---- node.cpp | 13 +++--- node.h | 1 + planner.cpp | 116 +++++++++++++++++++++++++++++----------------------- 4 files changed, 79 insertions(+), 65 deletions(-) diff --git a/action.cpp b/action.cpp index 52c1686..51e11e0 100644 --- a/action.cpp +++ b/action.cpp @@ -39,17 +39,15 @@ int Action::execute() const{ pid_t proc = fork(); if (proc == -1) return proc; + int retval; if (proc == 0) { //execl("/bin/sh", "-c", _executable.c_str(), (char*) NULL); - int val = system(_executable.c_str()); - exit(val); + retval = system(_executable.c_str()); + _exit(WEXITSTATUS(retval)); } - int retval; - - waitpid(proc,&retval,0); - - cout << "Done executing: " << _name << endl; - return retval; + waitpid(proc,&retval,NULL); + cout << "Done executing: " << _name << ", returnvalue: " << WEXITSTATUS(retval) << endl; + return WEXITSTATUS(retval); } const string& Action::name() const{ diff --git a/node.cpp b/node.cpp index b6793f6..24da8b1 100644 --- a/node.cpp +++ b/node.cpp @@ -42,17 +42,14 @@ bool Node::satisfyCondition(std::string effect){ bool Node::satisfyConditions(const Literals& effects){ for (Literals::const_iterator effect = effects.begin(); effect != effects.end(); ++effect){ - cerr << "Satisfied effect: " << *effect << endl; _preconditions.erase(_preconditions.find(*effect)); } - cerr << "Number of preconditions left: " << _preconditions.size() << endl; return _preconditions.size() == 0 && !_executed; } void Node::execute(){ - cerr << "Executing: " << _action->name() << endl; if(_executed) - cerr << "Already executed" << endl; + cerr << "Already executed " << _action->name() << endl; if ((_preconditions.size() != 0) || _executed) return; @@ -60,8 +57,6 @@ void Node::execute(){ int value = _action->execute(); _effects = _action->effects(value); - cerr << "Got returnvalue: " << value << ", number of effects: " << _effects.size() << endl; - } const std::vector& Node::children() const{ @@ -71,6 +66,12 @@ StartNode::StartNode(const Literals& init){ EffectsMap initial; initial[0] = init; _action = new Action("start",Preconditions(),"", initial); + _executed = true; + _effects = init; +} + +const Preconditions& Node::preconditions() const{ + return _preconditions; } EndNode::EndNode(const Literals& goal){ diff --git a/node.h b/node.h index 669657d..cbe1734 100644 --- a/node.h +++ b/node.h @@ -20,6 +20,7 @@ class Node { bool satisfyCondition(std::string effect); bool satisfyConditions(const Literals& effects); const std::vector& children() const; + const Preconditions& preconditions() const; protected: const Action* _action; diff --git a/planner.cpp b/planner.cpp index 9feeec8..02e6342 100644 --- a/planner.cpp +++ b/planner.cpp @@ -25,22 +25,19 @@ Planner::Planner(std::vector actions, Literals init, Literals goal){ _actions.push_back(act); const Literals& effects = act->effects(0); for (Literals::const_iterator effect = effects.begin(); effect != effects.end(); ++effect){ - //cerr << "Adding effect: '" << *effect << "', action: " << action->name() << endl; _actionEffects[*effect] = act; } } - //cerr << "Number of actions: " << _actions.size() << endl; makePlan(_finish); } Planner::~Planner(){ - //cerr << "Deleting " << _addedNodes.size() << " nodes" << endl; + //Iterating over the remaining nodes and deleting them for (vector::iterator node = _addedNodes.begin(); node != _addedNodes.end(); ++node){ - //cerr << "Deleting node " << (*node)->action()->name() << endl; delete *node; } + //iterating over the the remaining actions and deleting them for (vector::iterator action = _actions.begin(); action != _actions.end(); ++action){ - //cerr << "Deleting action " << (*action)->name() << endl; delete *action; } } @@ -49,35 +46,38 @@ Planner::~Planner(){ void Planner::makePlan(Node* node){ addNode(node); - //cerr << "Fetching preconditions for action: " << node->action()->name() << ".. "; const Preconditions& preconds = node->action()->preconditions(); - //cerr << "done" << endl; - if (preconds.size() == 0){ - //cerr << "Found no preconds" << endl; + //Add the node as a child to start if there are no preconditions _start->addChild(node); }else{ + //iterate over the preconditions for (Preconditions::const_iterator precond = preconds.begin(); precond != preconds.end(); ++precond){ - //cerr << "Looking for: '" << precond->first << "'" << endl; + //Check if there is a node with this precondition as an effect hash_map::iterator addedNode = _addedEffects.find(precond->first); if(addedNode != _addedEffects.end()){ - //cerr << "Using already added node" << endl; + //Use this node if there is one + //cerr << "Using already added node for effect " << precond->first << ", on node: " << node->action()->name() << endl; addedNode->second->addChild(node); }else { + //Check if there is an action which satisfies this effect hash_map::iterator action = _actionEffects.find(precond->first); if (action != _actionEffects.end()){ - //cerr << "Adding new node" << endl; + //Create a new node for the found effect and add the current + //one as a child Node* newnode = new Node(action->second); newnode->addChild(node); makePlan(newnode); }else if (precond->second){ - //cerr << "Action with effect: " << precond->first << " not found!" << endl; - //cerr << "This is a hard precondition, so this action and the children can't be executed." << endl; + //No such action found, and since it was a hard preconition + //we need to stop here. + cerr << "Could not satisfy the effect: " << precond->first << " for: " << node->action()->name() << endl; return; }else{ - //cerr << "Action with effect: " << precond->first << " not found!" << endl; - //cerr << "This is a soft precondition, so we will continue" << endl; + //No action found, but it was a soft precondition, so we can + //satisfy it and continue. + //cerr << "Could not satisfy the effect: " << precond->first << " for: " << node->action()->name() << endl; node->satisfyCondition(precond->first); _start->addChild(node); } @@ -87,104 +87,122 @@ void Planner::makePlan(Node* node){ } void Planner::addNode(Node* node){ - //cerr << "Adding node for action: " << node->action()->name() << endl; const Literals& effects = node->action()->effects(0); - //cerr << "Number of effects: " << effects.size() << endl; _addedNodes.push_back(node); + //Iterate over the effects for this node and add them to the map. for (Literals::const_iterator effect = effects.begin(); effect != effects.end(); ++effect){ - //cerr << "Adding node for effect: " << *effect << endl; _addedEffects[*effect] = node; } } void Planner::execute(){ - _executedNodes.push(_start); sem_init(&_nodes, 0, 1); sem_init(&_list, 0, 1); + //We've "executed" the start node. + _executedNodes.push(_start); int executions = 1; + + //As long as there executed nodes in the queue. while (executions > 0){ - cerr << "Waiting for a node to finish execution" << endl; - int retval; - sem_getvalue(&_nodes, &retval); - cerr << "Semaphore before wait: " << retval << endl; + //Wait for a node to be added to the queue sem_wait(&_nodes); - sem_getvalue(&_nodes, &retval); - cerr << "Semaphore after wait: " << retval << endl; --executions; - cerr << "Getting node: "; + //Pop the first node from the queue. sem_wait(&_list); - cerr << "Number of nodes in queue: " << _executedNodes.size() << endl; Node* node = _executedNodes.front(); _executedNodes.pop(); sem_post(&_list); - cerr << (int) node << endl; - cerr << node->action()->name() << ", and children.. "; - vector children = node->children(); - cerr << " done" << endl; + + //We don't need to continue if the end node was executed. if (node == _finish) return; - cerr << "Iterating over the children, number: " << children.size() << endl; + + vector children = node->children(); + + //Iterate over the children for this node for(vector::iterator child = children.begin(); child != children.end(); ++child){ + //Satisfy the preconditions the current node had as effect. if ((*child)->satisfyConditions(node->effects())){ + //If all preconditions were satisified we can create a new thread + //and execute this child. ++executions; - cerr << "Creating new thread" << endl; pthread_attr_t tattr; pthread_t tid; pthread_attr_init(&tattr); pthread_attr_setscope(&tattr, PTHREAD_SCOPE_SYSTEM); + //Create a struct object for the thread and add the + //needed members to it. ExecutionStuff* es = new ExecutionStuff; es->nodes = &_nodes; es->list = &_list; es->node = *child; es->execQueue = &_executedNodes; pthread_create(&tid, &tattr, executeNode, es); - //executeNode(es); } } } cerr << "Number of nodes: " << _addedNodes.size() << endl; + //Clearing the init vector, effects will be added below. + _goal.clear(); + Preconditions goal = _finish->preconditions(); + for (Preconditions::const_iterator precond = goal.begin(); precond != goal.end(); ++precond){ + _goal.push_back(precond->first); + } + + cout << "Unsatisfied preconditions so far: " << _goal.size() << ": "; + copy(_goal.begin(), _goal.end(), ostream_iterator(cout, " ")); + cout << endl; + + //iterator for inserting effects at the end of _init back_insert_iterator ii(_init); - _init.clear(); + + //Iterate throu the nodes, to see what's been done for (vector::iterator node = _addedNodes.begin(); node != _addedNodes.end(); ++node){ - cerr << "Deleting node " << (*node)->action()->name() << endl; if ((*node)->executed()){ + //Node was executed, adding the effects to _init. executions++; const Literals& effects = (*node)->effects(); copy(effects.begin(),effects.end(),ii); - cerr << "Finding action" << endl; + vector::iterator action = find(_actions.begin(), _actions.end(), (*node)->action()); if (action != _actions.end()){ - cerr << "Removing executed action: " << (*action)->name() << endl; + //The action can be deleted, since it's already been executed. delete *action; _actions.erase(action); } } + //The node is not needed anymore delete *node; } + //Clearing the vectors and maps, since they are obsolete now. _addedNodes.clear(); _addedEffects.clear(); _actionEffects.clear(); - copy(_init.begin(), _init.end(), ostream_iterator(cerr, " ")); - cerr << endl; - cerr << "Number of actions left: " << _actions.size() << endl; - //TODO: Fill _actionEffects with the remaining effects, create start end end nodes and create a new plan. + + cout << "Effects achieved so far: " << _init.size() << ": "; + copy(_init.begin(), _init.end(), ostream_iterator(cout, " ")); + cout << endl; + if (executions <= 1){ cerr << "Non of the remaining actions could be executed, quiting." << endl; return; } if (_actions.size() == 0){ - cerr << "No remaining actions, quiting." << endl; + //Nothing left to do, quitting. return; } + //REPLANNING + cout << "Replanning..." << endl; + + //Adding the effecs for the remaining actions. for (vector::iterator action = _actions.begin(); action != _actions.end(); ++action){ const Literals& effects = (*action)->effects(0); for (Literals::const_iterator effect = effects.begin(); effect != effects.end(); ++effect){ - cerr << "Adding effect: '" << *effect << "', action: " << (*action)->name() << endl; _actionEffects[*effect] = *action; } } @@ -200,20 +218,16 @@ void Planner::execute(){ } void* executeNode(void* arg){ - cerr << "Running new thred." << endl; ExecutionStuff* es = (ExecutionStuff*)arg; - if (es == 0) - pthread_exit((void*)1); - es->node->execute(); sem_wait(es->list); - cerr << "Adding pointer with value: " << (int)es->node << endl; + //Add this node to the queue with executed nodes es->execQueue->push(es->node); sem_post(es->list); - cerr << "Increasing semaphore" << endl; + //Signal that the node is available. sem_post(es->nodes); pthread_exit((void*)0); -- 2.39.2