_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<Node*>::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<Action*>::iterator action = _actions.begin(); action != _actions.end(); ++action){
- //cerr << "Deleting action " << (*action)->name() << endl;
delete *action;
}
}
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<string,Node*>::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<string, Action*>::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);
}
}
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<Node*> 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<Node*> children = node->children();
+
+ //Iterate over the children for this node
for(vector<Node*>::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<string>(cout, " "));
+ cout << endl;
+
+ //iterator for inserting effects at the end of _init
back_insert_iterator<Literals> ii(_init);
- _init.clear();
+
+ //Iterate throu the nodes, to see what's been done
for (vector<Node*>::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<Action*>::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<string>(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<string>(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<Action*>::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;
}
}
}
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);
projects / popboot.git / commitdiff