planner_xy.cpp

#include <iostream>
#include "ros/ros.h"
#include "planner/planner_xy.h"
#include "planner/node.h"
#include <math.h>
#include <vector>
#include <functional>
#include <queue>

using namespace std;

PlannerXY::
PlannerXY() {}

PlannerXY::
~PlannerXY() {}

vector< Node* > openList;
vector< Node* > closedList;
vector< Node* > path;


bool sortByCost (Node* lhs, Node* rhs) { return (lhs->f > rhs->f); }


std::vector< Node* >
PlannerXY::
findPath( void ) {
    cout << "Found path:\n";

    Node* current = closedList.at( closedList.size()-1 );

    int c = 0;
    while (current->id != "start") { // While current node has a 'last' node that isn't the start node
        path.push_back(current); // add current to path
        cout << current->id << endl;

        current = current->last;
        c++;
        if (c > 5000) { break; cout << "FAIL\n"; }
    }
    path.push_back(current); // add start to path
    cout << current->toString() << endl;

    return path;

}

bool
PlannerXY::
search( const geometry_msgs::Pose& start, const geometry_msgs::Pose& goal, const double& w ) {
    cout << "Weight " << w << endl;
    cout << "\nStart: (" << start.position.x << ", " << start.position.y << ")";
    cout << "\nGoal: (" << goal.position.x << ", " << goal.position.y << ")" << endl;

    string startID = "start";
    string goalID = "goal";

    //      Node(int ID, double posX, double posY, double G, double H, double F, double weight, Node Goal, Node Last) {
    Node* startNode = new Node(startID, start.position.x, start.position.y, 0, 0, 0, w, NULL, NULL);
    Node* goalNode = new Node(goalID, goal.position.x, goal.position.y, 0, 0, 0, w, NULL, NULL);

    startNode->goal = goalNode;
    startNode->last = startNode;
    startNode->recalculate(); // establishes g = 0, h = distToGoal, f = g+hw


    double distToGoal = startNode->h;
    cout << "Distance from goal: " << distToGoal << endl;

    openList.push_back(startNode);

    int count = 0;
    // A* time!
    while (!openList.empty()) { // while open list is not empty
        sort  (openList.begin(), openList.end(), sortByCost); // sort the open list
        Node* q = openList.back();  // get cheapest node
        openList.pop_back();    // remove cheapest node
        closedList.push_back(q); // add to closed list

        if (q->sameAs(q->goal)) {
            break;
        } // Reached the goal!


        // get all neighboring nodes, loop through
        for (Node* n: q->expand(w)) {
            //if (n.sameAs(q.last)) { continue; } // skip if same as prev node
            if (n->sameAs(n->goal)) {
                //cout << "Found goal!\n"; //Almost Done!
            }

            bool skip = false;
            for (Node* p: openList) {
                if (n->sameAs(p) && p->cost() < n->cost()) { skip = true; } // if n already in open list and not cheaper, skip
            }
            for (Node* c: closedList) {
                if (n->sameAs(c) && c->cost() < n->cost()) { skip = true; } // if n already in closed list and not cheaper, skip
            }
            if (!skip) { openList.push_back(n); } // otherwise, add to open list

            //cout << n->toString() << endl;

        }

        count ++;

        if (count > 100000) {
            cout << "Fail!!!" << endl;
            break;
        }
        //exit(0);
    }

    cout << "finished" << endl;
    return true;

}