AustinDeric · July 11, 2018 20:54 · AustinDeric · Jul 11, 2018 · AustinDeric · Jul 11, 2018
diff --git a/fun_path_planner.cpp b/fun_path_planner.cpp
 // Compiled with: g++ -Wall -std=c++14 -pthread
 /*
 Shortest path on a grid
 1. In robotics, a robot often to find a plan (a path or a set of actions) to reach its goal. Write code to determine the shortest path on a grid. Where the grid is square with the length of side N = 10.  In one step the robot is only allowed to move to a cell adjacent to its current cell, i.e., robot can move up, down, left, right (not diagonally).  The robot can start at a point (0,0) and its goal location (7,9) assuming the row and column values are indexed starting at 0. Your output is to print the sequence of cells that the robot will visit. You can print the row, col value for each cell that the robot visits on its way to the goal.
 Bonus Question: Now lets say someone placed an obstacle at (6,7) and (2,2). An obstacle means that the robot can no longer go through that cell. How will your solution change? Implement your solution and print the path.
 Objective: Write a function "findShortestPath(const int N, const int startx, const int starty, const int goalx, const int goaly)" that prints out the shortest path on the grid given the start, goal (and obstacle locations).
 **Well documented code gets bonus points**
 */

 #include <iostream>
 #include <vector>

 using namespace std;

 // Complete this function
 /**
 I recently did a problem like this for parking in a parking lot optimization using autonomous cars.  We used a gradient minimization technique with a cost map.  I am going to give that a try.  I believe it was similar to this https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf.  This code will use the wavefront method for simplicity.
 The code will be documented using doxygen. since this IDE doesn't have a doxygen plugin i will do it manually.
 */

 /*
 Utility function to print map
 */
 void printMap(vector <vector<int>> map)
 {
    for (unsigned i = 0; i < map.size(); i++)
    {
        for (unsigned j = 0; j < map[i].size(); j++)
        {
            cout << map[i][j] << " ";
        }
        cout << endl;
    }
 }

 void printMap(vector <vector<string>> map)
 {
    for (unsigned i = 0; i < map.size(); i++)
    {
        for (unsigned j = 0; j < map[i].size(); j++)
        {
            cout << map[i][j] << " ";
        }
        cout << endl;
    }
 }

 void findShortestPath(const int N /*grid size*/, const int startx, const int starty, const int goalx, const int goaly)
 {
    //create the map
    vector <vector<int>> map(N, vector<int>(N));

    //set start point (uneeded code becuase map is all zeros but keeping to show method)
    map[startx][starty] = 0;

    //set end point
    int end_weight = 2;
    map[goalx][goaly] = end_weight;

    // implement the wavefront function using 4-point connectivity (no diagonals)
    vector <vector<int>> wavefront(N, vector<int>(N));  //datastructure to track wavefront completeness.
    wavefront[goalx][goaly] = 1;
    unsigned iteration = 0;
    bool wavefront_complete = false;

    //index of wavefront - starting location
    unsigned wavefront_x = goalx-1;
    unsigned wavefront_y = goaly-1;

    cout << "starting map:" <<endl;
    printMap(map);
    cout << "starting wavefront:" << endl;
    printMap(wavefront);
    cout << "-----------" << endl;

    //propogate wavefront
    bool mapComplete = false;
    while(!mapComplete){

        ++iteration;
        cout << "iteration: " << iteration << endl;
        cout << "wavefront_x: " << wavefront_x << endl;
        cout << "wavefront_y: " << wavefront_y << endl;

        cout << "i: " << goalx-wavefront_x << endl;
        for (int i = goalx-wavefront_x; i > 0; --i){//top row
            wavefront[wavefront_x][wavefront_y+i] = 1;
            map[wavefront_x][wavefront_y+i] = iteration+end_weight;
        }

        cout << "j " << goaly-wavefront_y << endl;
        for (int j = goaly-wavefront_y; j > 0; --j){//left column
            wavefront[wavefront_x+j][wavefront_y] = 1;
            map[wavefront_x+j][wavefront_y] = iteration+end_weight;
        }

        wavefront[wavefront_x][wavefront_y] = 1;
        map[wavefront_x][wavefront_y] = iteration+end_weight;

        if(wavefront_y==0 && wavefront_x==0)
            mapComplete = true;
        if(wavefront_x>0)
            --wavefront_x;
        if(wavefront_y>0)
            --wavefront_y;

        //set obstacles!
        map[6][7] = 0;
        map[2][2] = 0;

        //debugging information
        cout << "map:" << endl;
        printMap(map);
        //print wavefront for debugging purposes
        cout << "wavefront:" << endl;
        printMap(wavefront);
        cout << "-----------" << endl;
    }

    //now that the maps are created, lets navigate by going to the cell with the smaller non-zero value
    bool arrived = false;
    vector< pair<int, int> > path;
    //start
    path.push_back(std::make_pair(startx,starty));
    int steps = 0;
    while(!arrived){

        //look down and right
        int down_cell = map[path.back().first+1][path.back().second];
        int right_cell = map[path.back().first][path.back().second+1];

        cout << "down cell: " << down_cell << endl;
        cout << "right cell: " << right_cell << endl;

        //go down
        if(down_cell < right_cell && down_cell!=0) { //go down
            path.push_back(std::make_pair(path.back().first+1, path.back().second));
        }
        else if(down_cell > right_cell && right_cell!=0) { //go right
            path.push_back(std::make_pair(path.back().first, path.back().second+1));
        }
        else { //down and right are the same so go right
            path.push_back(std::make_pair(path.back().first, path.back().second+1));
        }

        steps++;

        cout << "current path:" << endl;
        vector <vector<string>> path_map(N, vector<string>(N, "o"));
        for (auto current_cell : path)
        {
            cout << current_cell.first << ", " << current_cell.second << endl;
            path_map[startx][starty] = "S"; //start at S
            path_map[current_cell.first][current_cell.second] = "-"; //path is a -
            path_map[6][7] = "X"; //obstacles are X
            path_map[2][2] = "X"; //obstacles are X
            if(goalx == current_cell.first && goaly == current_cell.second){
                cout << "path map: " << endl;
                path_map[goalx][goaly] = "G"; // end at G
                printMap(path_map);
                arrived = true;
            }
        }
    }

 }

 int main()
 {

    // grid size
    int N  = 10;

    // start
    int startx = 0;
    int starty = 0;

    // goal (zero-indexed)
    int goalx = 7;
    int goaly = 9;

    findShortestPath(N, startx, starty, goalx, goaly);

    return 0;
 }
	// Compiled with: g++ -Wall -std=c++14 -pthread
	/*
	Shortest path on a grid
	1. In robotics, a robot often to find a plan (a path or a set of actions) to reach its goal. Write code to determine the shortest path on a grid. Where the grid is square with the length of side N = 10. In one step the robot is only allowed to move to a cell adjacent to its current cell, i.e., robot can move up, down, left, right (not diagonally). The robot can start at a point (0,0) and its goal location (7,9) assuming the row and column values are indexed starting at 0. Your output is to print the sequence of cells that the robot will visit. You can print the row, col value for each cell that the robot visits on its way to the goal.
	Bonus Question: Now lets say someone placed an obstacle at (6,7) and (2,2). An obstacle means that the robot can no longer go through that cell. How will your solution change? Implement your solution and print the path.
	Objective: Write a function "findShortestPath(const int N, const int startx, const int starty, const int goalx, const int goaly)" that prints out the shortest path on the grid given the start, goal (and obstacle locations).
	Well documented code gets bonus points
	*/

	#include <iostream>
	#include <vector>

	using namespace std;

	// Complete this function
	/**
	I recently did a problem like this for parking in a parking lot optimization using autonomous cars. We used a gradient minimization technique with a cost map. I am going to give that a try. I believe it was similar to this https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf. This code will use the wavefront method for simplicity.
	The code will be documented using doxygen. since this IDE doesn't have a doxygen plugin i will do it manually.
	*/

	/*
	Utility function to print map
	*/
	void printMap(vector <vector<int>> map)
	{
	for (unsigned i = 0; i < map.size(); i++)
	{
	for (unsigned j = 0; j < map[i].size(); j++)
	{
	cout << map[i][j] << " ";
	}
	cout << endl;
	}
	}

	void printMap(vector <vector<string>> map)
	{
	for (unsigned i = 0; i < map.size(); i++)
	{
	for (unsigned j = 0; j < map[i].size(); j++)
	{
	cout << map[i][j] << " ";
	}
	cout << endl;
	}
	}

	void findShortestPath(const int N /grid size/, const int startx, const int starty, const int goalx, const int goaly)
	{
	//create the map
	vector <vector<int>> map(N, vector<int>(N));

	//set start point (uneeded code becuase map is all zeros but keeping to show method)
	map[startx][starty] = 0;

	//set end point
	int end_weight = 2;
	map[goalx][goaly] = end_weight;

	// implement the wavefront function using 4-point connectivity (no diagonals)
	vector <vector<int>> wavefront(N, vector<int>(N)); //datastructure to track wavefront completeness.
	wavefront[goalx][goaly] = 1;
	unsigned iteration = 0;
	bool wavefront_complete = false;

	//index of wavefront - starting location
	unsigned wavefront_x = goalx-1;
	unsigned wavefront_y = goaly-1;

	cout << "starting map:" <<endl;
	printMap(map);
	cout << "starting wavefront:" << endl;
	printMap(wavefront);
	cout << "-----------" << endl;

	//propogate wavefront
	bool mapComplete = false;
	while(!mapComplete){

	++iteration;
	cout << "iteration: " << iteration << endl;
	cout << "wavefront_x: " << wavefront_x << endl;
	cout << "wavefront_y: " << wavefront_y << endl;

	cout << "i: " << goalx-wavefront_x << endl;
	for (int i = goalx-wavefront_x; i > 0; --i){//top row
	wavefront[wavefront_x][wavefront_y+i] = 1;
	map[wavefront_x][wavefront_y+i] = iteration+end_weight;
	}

	cout << "j " << goaly-wavefront_y << endl;
	for (int j = goaly-wavefront_y; j > 0; --j){//left column
	wavefront[wavefront_x+j][wavefront_y] = 1;
	map[wavefront_x+j][wavefront_y] = iteration+end_weight;
	}

	wavefront[wavefront_x][wavefront_y] = 1;
	map[wavefront_x][wavefront_y] = iteration+end_weight;

	if(wavefront_y==0 && wavefront_x==0)
	mapComplete = true;
	if(wavefront_x>0)
	--wavefront_x;
	if(wavefront_y>0)
	--wavefront_y;

	//set obstacles!
	map[6][7] = 0;
	map[2][2] = 0;

	//debugging information
	cout << "map:" << endl;
	printMap(map);
	//print wavefront for debugging purposes
	cout << "wavefront:" << endl;
	printMap(wavefront);
	cout << "-----------" << endl;
	}

	//now that the maps are created, lets navigate by going to the cell with the smaller non-zero value
	bool arrived = false;
	vector< pair<int, int> > path;
	//start
	path.push_back(std::make_pair(startx,starty));
	int steps = 0;
	while(!arrived){

	//look down and right
	int down_cell = map[path.back().first+1][path.back().second];
	int right_cell = map[path.back().first][path.back().second+1];

	cout << "down cell: " << down_cell << endl;
	cout << "right cell: " << right_cell << endl;

	//go down
	if(down_cell < right_cell && down_cell!=0) { //go down
	path.push_back(std::make_pair(path.back().first+1, path.back().second));
	}
	else if(down_cell > right_cell && right_cell!=0) { //go right
	path.push_back(std::make_pair(path.back().first, path.back().second+1));
	}
	else { //down and right are the same so go right
	path.push_back(std::make_pair(path.back().first, path.back().second+1));
	}

	steps++;

	cout << "current path:" << endl;
	vector <vector<string>> path_map(N, vector<string>(N, "o"));
	for (auto current_cell : path)
	{
	cout << current_cell.first << ", " << current_cell.second << endl;
	path_map[startx][starty] = "S"; //start at S
	path_map[current_cell.first][current_cell.second] = "-"; //path is a -
	path_map[6][7] = "X"; //obstacles are X
	path_map[2][2] = "X"; //obstacles are X
	if(goalx == current_cell.first && goaly == current_cell.second){
	cout << "path map: " << endl;
	path_map[goalx][goaly] = "G"; // end at G
	printMap(path_map);
	arrived = true;
	}
	}
	}

	}

	int main()
	{

	// grid size
	int N = 10;

	// start
	int startx = 0;
	int starty = 0;

	// goal (zero-indexed)
	int goalx = 7;
	int goaly = 9;

	findShortestPath(N, startx, starty, goalx, goaly);

	return 0;
	}