#include "planner.h"
#include "node.h"
#include <iostream>
-#include <vector>
using namespace std;
using namespace __gnu_cxx;
+extern "C" void* executeNode(void* arg);
+
+struct ExecutionStuff {
+ sem_t* nodes;
+ sem_t* list;
+ Node* node;
+ queue<Node*>* execQueue;
+};
+
Planner::Planner(std::vector<Action> actions, Literals init, Literals goal){
_init = init;
_goal = 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;
+ //cerr << "Adding effect: '" << *effect << "', action: " << action->name() << endl;
_actionEffects[*effect] = act;
}
}
- cerr << "Number of actions: " << _actions.size() << endl;
+ //cerr << "Number of actions: " << _actions.size() << endl;
makePlan(_finish);
}
Planner::~Planner(){
- cerr << "Deleting " << _addedNodes.size() << " nodes" << endl;
+ //cerr << "Deleting " << _addedNodes.size() << " nodes" << endl;
for (vector<Node*>::iterator node = _addedNodes.begin(); node != _addedNodes.end(); ++node){
- cerr << "Deleting node " << (*node)->action()->name() << endl;
+ //cerr << "Deleting node " << (*node)->action()->name() << endl;
delete *node;
}
for (vector<Action*>::iterator action = _actions.begin(); action != _actions.end(); ++action){
- cerr << "Deleting action " << (*action)->name() << endl;
+ //cerr << "Deleting action " << (*action)->name() << endl;
delete *action;
}
}
void Planner::makePlan(Node* node){
addNode(node);
- cerr << "Fetching preconditions for action: " << node->action()->name() << ".. ";
+ //cerr << "Fetching preconditions for action: " << node->action()->name() << ".. ";
const Preconditions& preconds = node->action()->preconditions();
- cerr << "done" << endl;
+ //cerr << "done" << endl;
if (preconds.size() == 0){
- cerr << "Found no preconds" << endl;
+ //cerr << "Found no preconds" << endl;
_start->addChild(node);
}else{
for (Preconditions::const_iterator precond = preconds.begin(); precond != preconds.end(); ++precond){
- cerr << "Looking for: '" << precond->first << "'" << endl;
+ //cerr << "Looking for: '" << precond->first << "'" << endl;
hash_map<string,Node*>::iterator addedNode = _addedEffects.find(precond->first);
if(addedNode != _addedEffects.end()){
- cerr << "Using already added node" << endl;
+ //cerr << "Using already added node" << endl;
addedNode->second->addChild(node);
}else {
hash_map<string, Action*>::iterator action = _actionEffects.find(precond->first);
if (action != _actionEffects.end()){
- cerr << "Adding new node" << endl;
+ //cerr << "Adding new node" << endl;
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;
+ //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;
return;
}else{
- cerr << "Action with effect: " << precond->first << " not found!" << endl;
- cerr << "This is a soft precondition, so we will continue" << endl;
+ //cerr << "Action with effect: " << precond->first << " not found!" << endl;
+ //cerr << "This is a soft precondition, so we will continue" << endl;
node->satisfyCondition(precond->first);
_start->addChild(node);
}
}
void Planner::addNode(Node* node){
- cerr << "Adding node for action: " << node->action()->name() << endl;
+ //cerr << "Adding node for action: " << node->action()->name() << endl;
const Literals& effects = node->action()->effects(0);
- cerr << "Number of effects: " << effects.size() << endl;
+ //cerr << "Number of effects: " << effects.size() << endl;
_addedNodes.push_back(node);
for (Literals::const_iterator effect = effects.begin(); effect != effects.end(); ++effect){
- cerr << "Adding node for effect: " << *effect << endl;
+ //cerr << "Adding node for effect: " << *effect << endl;
_addedEffects[*effect] = node;
}
}
void Planner::execute(){
- _start->execute(Literals());
+ _executedNodes.push(_start);
+ sem_init(&_nodes, 0, 1);
+ sem_init(&_list, 0, 1);
+
+ int executions = 1;
+ while (executions > 0){
+ cerr << "Waiting for a node to finish execution" << endl;
+ int retval;
+ sem_getvalue(&_nodes, &retval);
+ cerr << "Semaphore before wait: " << retval << endl;
+ sem_wait(&_nodes);
+ sem_getvalue(&_nodes, &retval);
+ cerr << "Semaphore after wait: " << retval << endl;
+ --executions;
+
+ cerr << "Getting node: ";
+ 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;
+ if (node == _finish)
+ return;
+ cerr << "Iterating over the children, number: " << children.size() << endl;
+ for(vector<Node*>::iterator child = children.begin(); child != children.end(); ++child){
+ if ((*child)->satisfyConditions(node->effects())){
+ ++executions;
+ cerr << "Creating new thread" << endl;
+ pthread_attr_t tattr;
+ pthread_t tid;
+ pthread_attr_init(&tattr);
+ pthread_attr_setscope(&tattr, PTHREAD_SCOPE_SYSTEM);
+ 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;
back_insert_iterator<Literals> ii(_init);
_init.clear();
- int executions = 0;
for (vector<Node*>::iterator node = _addedNodes.begin(); node != _addedNodes.end(); ++node){
cerr << "Deleting node " << (*node)->action()->name() << endl;
if ((*node)->executed()){
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;
+ es->execQueue->push(es->node);
+ sem_post(es->list);
+
+ cerr << "Increasing semaphore" << endl;
+ sem_post(es->nodes);
+
+ pthread_exit((void*)0);
+}
projects / popboot.git / blobdiff