void Machinery::setup(const kerberos::StringMap & settings)
{
// -----------------------------------------------------------
// Creates condition, algorithms, expositors, heuristics and io handlers.
LINFO << "Starting conditions: " + settings.at("condition");
std::vector<Condition *> conditions = Factory<Condition>::getInstance()->createMultiple(settings.at("condition"));
for(int i = 0; i < conditions.size(); i++)
{
conditions[i]->setup(settings);
}
setCondition(conditions);
LINFO << "Starting algorithm: " + settings.at("algorithm");
Algorithm * algorithm = Factory<Algorithm>::getInstance()->create(settings.at("algorithm"));
algorithm->setup(settings);
setAlgorithm(algorithm);
LINFO << "Starting expositor: " + settings.at("expositor");
Expositor * expositor = Factory<Expositor>::getInstance()->create(settings.at("expositor"));
expositor->setup(settings);
setExpositor(expositor);
LINFO << "Starting heuristic: " + settings.at("heuristic");
Heuristic * heuristic = Factory<Heuristic>::getInstance()->create(settings.at("heuristic"));
heuristic->setup(settings);
setHeuristic(heuristic);
LINFO << "Starting io devices: " + settings.at("io");
std::vector<Io *> ios = Factory<Io>::getInstance()->createMultiple(settings.at("io"));
for(int i = 0; i < ios.size(); i++)
{
ios[i]->setup(settings);
}
setIo(ios);
}
void AStar::plan(const Point2D &start, const Point2D &goal, const std::vector<Point2D> &obstacles) {
// set the goal, and clear the previous plan
this->goal = goal;
clear();
goalNode.reset();
assert(start != goal);
boost::shared_ptr<Node> newNode;
Point2D pos;
//std::set<boost::shared_ptr<Node> >::iterator it;
boost::unordered_set<boost::shared_ptr<Node> >::iterator it;
// start the open nodes
boost::shared_ptr<Node> node(new Node(0,0,start,boost::shared_ptr<Node>()));
setHeuristic(node);
openHeap.push_back(node);
openNodes.insert(node);
// set obstacles as closed nodes
for (unsigned int i = 0; i < obstacles.size(); i++) {
if (obstacles[i] != goal) // ignore any obstacles at the goal
closedNodes.insert(boost::shared_ptr<Node>(new Node(0,0,obstacles[i],boost::shared_ptr<Node>())));
}
// while there are open nodes
while (openNodes.size() > 0) {
node = openHeap.front(); // get the lowest cost node
if (node->pos == goal) {
// we're done
goalNode = node;
break;
}
// move the node from open to closed
std::pop_heap(openHeap.begin(),openHeap.end(),cmp); // resort the heap
openHeap.pop_back(); // remove the node from the heap
openNodes.erase(node); // remove the node from the open set
closedNodes.insert(node); // add the node to the closed set
// search its neighbors
for (unsigned int i = 0; i < Action::NUM_NEIGHBORS; i++) {
pos = movePosition(dims,node->pos,(Action::Type)i);
newNode = boost::shared_ptr<Node>(new Node(node->gcost + 1,0,pos,node));
if (closedNodes.count(newNode) > 0) {
continue;
}
it = openNodes.find(newNode);
if (it != openNodes.end()) {
// already open
if ((*it)->gcost > newNode->gcost) {
(*it)->gcost = newNode->gcost;
(*it)->parent = node;
// have to resort because this item might be out of place now
std::make_heap(openHeap.begin(),openHeap.end(),cmp);
}
} else {
// new open node
setHeuristic(newNode);
openNodes.insert(newNode);
openHeap.push_back(newNode);
std::push_heap(openHeap.begin(),openHeap.end(),cmp);
}
}
}
}
