1 Introduction
The task of this experiment: write the two path planning algorithms learned earlier.
First write the Breadth-first search algorithm in C++. The algorithm is divided into different coding quizzes, and finally generates the shortest path for the robot to move from the starting point to the goal.
Then, you will proceed to make the necessary changes to write the A* algorithm. After coding the BFS and A* algorithms, the resulting expanded lists are compared visually. After careful inspection, decide which algorithm is more efficient.
In the later part of this lab, the A* algorithm will be applied to real-world problems. The actual problem is just a map generated using an occupancy grid mapping algorithm.
Experiment: Path Planning
The detailed steps of the experiment are listed as follows:
2. Modeling problem
The purpose of this experiment is to use different path planning algorithms to find the shortest path for the robot to move from the starting position to the goal position in a 5x6 map. The robot can only move in four directions: up, left, down, and right. We will first model this problem using classes in C++ and then solve it with BFS and A* algorithms.
Given
Grid(5x6):
0 1 0 0 0 0
0 1 0 0 0 0
0 1 0 0 0 0
0 1 0 0 0 0
0 0 0 1 1 0
Where 1 represents obstacles and 0 represents free space.
Robot start position: 0,0
Robot target position: 4,5
Moving direction: up(-1,0)-left(0,-1)-down(1,0)-right(0,1)
The movement direction vector is a collection of four different 2D vectors, each allowing movement between grid cells in the map.
Move arrows: Up(^)-Left(<)-Down(v)-Right(>)
The move arrow vector stores the robot's motion, this vector will be used later in this lab to visualize the direction of each grid cell the robot is on the shortest path.
Move Cost: 1
The Move Cost value represents the cost of moving from one cell to another. Here, the cost is equal for all possible moves.
Test
In this test, there are three main tasks to be done in order to model the problem:
NOTE
Throughout the experiments, 1D and 2D vectors in C++ will be used. vector allows easy management and manipulation of data using pre-built functions. For example: The pop_back function can be used to remove the last element in a vector.
For vectors, see the following two resources:
- 2D Vectors : Learn how to define and use 2D vectors in C++.
- Documentation : Learn about vector iterators and modifiers.
The reference code is as follows:
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
/* TODO: Define a Map class
Inside the map class, define the mapWidth, mapHeight and grid as a 2D vector
*/
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
/* TODO: Define a Planner class
Inside the Planner class, define the start, goal, cost, movements, and movements_arrows
Note: The goal should be defined it terms of the mapWidth and mapHeight
*/
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
/* TODO: Define a print2DVector function which will print 2D vectors of any data type
Example
Input:
vector<vector<int> > a{
{ 1, 0 },{ 0, 1 }};
print2DVector(a);
vector<vector<string> > b{
{ "a", "b" },{ "c", "d" }};
print2DVector(b);
Output:
1 0
0 1
a b
c d
Hint: You need to use templates
*/
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
/*############ Don't modify the main function############*/
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Print classes variables
cout << "Map:" << endl;
print2DVector(map.grid);
cout << "Start: " << planner.start[0] << " , " << planner.start[1] << endl;
cout << "Goal: " << planner.goal[0] << " , " << planner.goal[1] << endl;
cout << "Cost: " << planner.cost << endl;
cout << "Robot Movements: " << planner.movements_arrows[0] << " , " << planner.movements_arrows[1] << " , " << planner.movements_arrows[2] << " , " << planner.movements_arrows[3] << endl;
cout << "Delta:" << endl;
print2DVector(planner.movements);
return 0;
}
3. BFS: Extended List
Now use the Map and Planner classes in C++ to model the problem, and then start with the first part of the BFS algorithm. In this test, write a search function that expands cells with minimal effort until the goal is reached.
To do this, each cell needs to be represented by the triplet value [g, x, y] , where g is the total cost of extending to that cell, x is the row value, and y is the column value.
Once the expansion reaches the target, print the target's final triplet value.
When writing search functions, keep the following in mind
- When expanding to a new cell, check to see if the target has been reached; once reached, print its triplet value.
- Proactively checks for obstacles. If an obstacle is encountered, stop the extension and print a message indicating that the goal was not reached.
- Expand the cell with the lowest g value and store the expansion in an open vector. If the g values of two cells are equal, one of the cells can be selected for further expansion.
Hint
Here's how to expand the unit using the BFS algorithm until the target is reached:
Expansion #: 0
Open List: [0 0 0 ]
Cell Picked: [0 0 0]
Expansion #: 1
Open List: [1 1 0 ]
Cell Picked: [1 1 0]
Expansion #: 2
Open List: [2 2 0 ]
Cell Picked: [2 2 0]
Expansion #: 3
Open List: [3 3 0 ]
Cell Picked: [3 3 0]
Expansion #: 4
Open List: [4 4 0 ]
Cell Picked: [4 4 0]
Expansion #: 5
Open List: [5 4 1 ]
Cell Picked: [5 4 1]
Expansion #: 6
Open List: [6 4 2 ]
Cell Picked: [6 4 2]
Expansion #: 7
Open List: [7 3 2 ]
Cell Picked: [7 3 2]
Expansion #: 8
Open List: [8 3 3 ], [8 2 2 ]
Cell Picked: [8 2 2]
Expansion #: 9
Open List: [9 2 3 ], [9 1 2 ], [8 3 3 ]
Cell Picked: [8 3 3]
Expansion #: 10
Open List: [9 3 4 ], [9 2 3 ], [9 1 2 ]
Cell Picked: [9 1 2]
Expansion #: 11
Open List: [10 1 3 ], [10 0 2 ], [9 3 4 ], [9 2 3 ]
Cell Picked: [9 2 3]
Expansion #: 12
Open List: [10 2 4 ], [10 1 3 ], [10 0 2 ], [9 3 4 ]
Cell Picked: [9 3 4]
Expansion #: 13
Open List: [10 3 5 ], [10 2 4 ], [10 1 3 ], [10 0 2 ]
Cell Picked: [10 0 2]
Expansion #: 14
Open List: [11 0 3 ], [10 3 5 ], [10 2 4 ], [10 1 3 ]
Cell Picked: [10 1 3]
Expansion #: 15
Open List: [11 1 4 ], [11 0 3 ], [10 3 5 ], [10 2 4 ]
Cell Picked: [10 2 4]
Expansion #: 16
Open List: [11 2 5 ], [11 1 4 ], [11 0 3 ], [10 3 5 ]
Cell Picked: [10 3 5]
Expansion #: 17
Open List: [11 4 5 ], [11 2 5 ], [11 1 4 ], [11 0 3 ]
Cell Picked: [11 0 3]
Expansion #: 18
Open List: [12 0 4 ], [11 4 5 ], [11 2 5 ], [11 1 4 ]
Cell Picked: [11 1 4]
Expansion #: 19
Open List: [12 1 5 ], [12 0 4 ], [11 4 5 ], [11 2 5 ]
Cell Picked: [11 2 5]
Expansion #: 20
Open List: [12 1 5 ], [12 0 4 ], [11 4 5 ]
Cell Picked: [11 4 5]
The reference code is as follows:
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
/*#### TODO: Code the search function which will generate the expansion list ####*/
// You are only required to print the final triplet values
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Defined the triplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
// Store the expansions
vector<vector<int> > open;
open.push_back({ g, x, y });
// Flags
bool found = false;
bool resign = false;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove triplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[1];
y = next[2];
g = next[0];
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
open.push_back({ g2, x2, y2 });
closed[x2][y2] = 1;
}
}
}
}
}
}
}
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Search for the expansions
search(map, planner);
return 0;
}
4. BFS: Expansion Vector
Now that the cells have been expanded until the goal is reached, the system asks to print the order in which each cell is expanded. To do this, the search function needs to be modified and a 2D unwrapped vector of the same size as the map is created. Each cell in the expansion vector will store the order in which it was expanded. Some cells are never expanded and should show a value of -1.
Tip
Take a look at the list of extensions generated after running the code:
As you can see, we start with the first cell and end at the expanded target cell after 20 iterations. All barriers and some cells are never expanded, so the displayed value is -1.
Now, go ahead and modify the search function to generate and print the unwrapped 2D vector.
The reference code is as follows:
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
// Search function will generate the expansions
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the triplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
// Store the expansions
vector<vector<int> > open;
open.push_back({ g, x, y });
// Flags and counters
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove triplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[1];
y = next[2];
g = next[0];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
open.push_back({ g2, x2, y2 });
closed[x2][y2] = 1;
}
}
}
}
}
}
// Print the expansion List
print2DVector(expand);
}
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Search for the expansions
search(map, planner);
return 0;
}
5. BFS: shortest path
The final step is to print the shortest path the robot takes to get from the starting point to the goal. Every action the robot should take (eg: turn left< ) needs to be recorded and all actions stored in a policy 2D vector.
hint
Below is the output policy vector generated after running the code:
It can be seen that the different actions (v - > - < - ^) are the actions the robot has to take in order to reach the goal marked with *. Some of these cells will never be visited by robots and are marked with a "-". Now, go ahead and modify the search function to generate the Policy 2D Vector.
The reference code is as follows:
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
// Search function will generate the expansions
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the triplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
// Store the expansions
vector<vector<int> > open;
open.push_back({ g, x, y });
// Flags and counters
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove triplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[1];
y = next[2];
g = next[0];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
open.push_back({ g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
//print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the path with arrows
print2DVector(policy);
}
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Search for the expansions
search(map, planner);
return 0;
}
6.A*: shortest path
Now we will implement the A* algorithm and find the shortest path by modifying the previous code. As you know, A* is based on heuristic functions. Therefore, we will implement a Manhattan-based heuristic vector and calculate the Manhattan distance of each cell relative to the target position, where:
expand
Now instead of expanding the cell with the minimum path cost g , the cell is expanded with the minimum f value f is the sum of the path cost g and the heuristic value h of the cell.
f=g+h
Each cell is now represented by the 4-tuple value [f,g,x,y] instead of the 3-tuple value [g,x,y] .
Note: Follow the steps below and make the necessary changes to find the shortest path using the A* algorithm:
hint
Here's how to expand the cells using the A* algorithm until the goal is reached:
| Map | 0 |
1 |
2 |
3 |
4 |
5 |
|---|---|---|---|---|---|---|
0 |
0 | 1 | 0 | 0 | 0 | 0 |
1 |
0 | 1 | 0 | 0 | 0 | 0 |
2 |
0 | 1 | 0 | 0 | 0 | 0 |
3 |
0 | 1 | 0 | 0 | 0 | 0 |
4 |
0 | 0 | 0 | 1 | 1 | 0 |
Expansion #: 0
Open List: [9 0 0 0 ]
Cell Picked: [9 0 0 0]
Expansion #: 1
Open List: [9 1 1 0 ]
Cell Picked: [9 1 1 0]
Expansion #: 2
Open List: [9 2 2 0 ]
Cell Picked: [9 2 2 0]
Expansion #: 3
Open List: [9 3 3 0 ]
Cell Picked: [9 3 3 0]
Expansion #: 4
Open List: [9 4 4 0 ]
Cell Picked: [9 4 4 0]
Expansion #: 5
Open List: [9 5 4 1 ]
Cell Picked: [9 5 4 1]
Expansion #: 6
Open List: [9 6 4 2 ]
Cell Picked: [9 6 4 2]
Expansion #: 7
Open List: [11 7 3 2 ]
Cell Picked: [11 7 3 2]
Expansion #: 8
Open List: [13 8 2 2 ], [11 8 3 3 ]
Cell Picked: [11 8 3 3]
Expansion #: 9
Open List: [13 9 2 3 ], [13 8 2 2 ], [11 9 3 4 ]
Cell Picked: [11 9 3 4]
Expansion #: 10
Open List: [13 10 2 4 ], [13 9 2 3 ], [13 8 2 2 ], [11 10 3 5 ]
Cell Picked: [11 10 3 5]
Expansion #: 11
Open List: [13 11 2 5 ], [13 10 2 4 ], [13 9 2 3 ], [13 8 2 2 ], [11 11 4 5 ]
Cell Picked: [11 11 4 5]
The reference code is as follows:
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
vector<vector<int> > heuristic = {
{ 9, 8, 7, 6, 5, 4 },
{ 8, 7, 6, 5, 4, 3 },
{ 7, 6, 5, 4, 3, 2 },
{ 6, 5, 4, 3, 2, 1 },
{ 5, 4, 3, 2, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
// Search function will generate the expansions
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the quadruplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
int f = g + map.heuristic[x][y];
// Store the expansions
vector<vector<int> > open;
open.push_back({ f, g, x, y });
// Flags and Counts
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove quadruplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[2];
y = next[3];
g = next[1];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
f = g2 + map.heuristic[x2][y2];
open.push_back({ f, g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the robot path
cout << endl;
print2DVector(policy);
}
int main()
{
// Instantiate a planner and map objects
Map map;
Planner planner;
search(map, planner);
return 0;
}
7. Compare
Now that the BFS and A* algorithms have been written, let's go through their extended lists and compare them.
in conclusion
It can be clearly seen that A* is more efficient because it does not expand in free space like BFS. In A*, only 11 expansions need to be done, while in BFS 20 expansions are done.
8.A*: Real-World Map (Real-World Map )
The following map was generated using an occupancy grid mapping algorithm using sonar and odometry data. Our task now is to apply the A* algorithm to find the shortest path for the robot from the starting point o to the goal* position.
given conditions
Map Map(300x150): Map data stored in the Map.txt file in the form of logs , how should these numbers be understood:
-
A cell is considered unknown if its log odds value is equal to 0 .
-
A cell is considered occupied if its log odds value is greater than 0 .
-
A cell is considered vacant if its log odds value is less than 0 .
Grid( 300x150) : Convert log values to 0 and 1, where 0 means free space and 1 means occupied or unknown space.
Robot Start position: 230,145
Target position Robot Goal Position: 60,50
Moving direction: up(-1,0)-left(0,-1)-down(1,0)-right(0,1)
Movement Arrows: Up(^)-Left(<)-Down(v)-Right(>)
Move Cost: 1
Heuristic Vectors: Manhattan
Add three new functions to the Map class: GetMap function to read map.txt log (log odds) values and assign them to map variables; MapToGrid function to convert log (log odds) values to 0 and 1 . These 0 and 1 values will be assigned to grid variables; the GeneratedHeuristic function, in order to generate a Manhattan-based heuristic vector, by computing the Manhattan distance of each cell relative to the target location. The Manhattan distance of each cell can be calculated as follows:
Note: Follow the instructions below to generate the shortest path using the A* algorithm:
The reference code is as follows:
#include <iostream>
#include <math.h>
#include <vector>
#include <algorithm>
#include <fstream>
using namespace std;
// Map class
class Map {
public:
const static int mapHeight = 300;
const static int mapWidth = 150;
vector<vector<double> > map = GetMap();
vector<vector<int> > grid = MaptoGrid();
vector<vector<int> > heuristic = GenerateHeuristic();
private:
// Read the file and get the map
vector<vector<double> > GetMap()
{
vector<vector<double> > mymap(mapHeight, vector<double>(mapWidth));
ifstream myReadFile;
myReadFile.open("map.txt");
while (!myReadFile.eof()) {
for (int i = 0; i < mapHeight; i++) {
for (int j = 0; j < mapWidth; j++) {
myReadFile >> mymap[i][j];
}
}
}
return mymap;
}
//Convert the map to 1's and 0's
vector<vector<int> > MaptoGrid()
{
vector<vector<int> > grid(mapHeight, vector<int>(mapWidth));
for (int x = 0; x < mapHeight; x++) {
for (int y = 0; y < mapWidth; y++) {
if (map[x][y] == 0) //unkown state
grid[x][y] = 1;
else if (map[x][y] > 0) //Occupied state
grid[x][y] = 1;
else //Free state
grid[x][y] = 0;
}
}
return grid;
}
// Generate a Manhattan Heuristic Vector
vector<vector<int> > GenerateHeuristic()
{
vector<vector<int> > heuristic(mapHeight, vector<int>(mapWidth));
int goal[2] = { 60, 50 };
for (int i = 0; i < heuristic.size(); i++) {
for (int j = 0; j < heuristic[0].size(); j++) {
int xd = goal[0] - i;
int yd = goal[1] - j;
// Manhattan Distance
int d = abs(xd) + abs(yd);
// Euclidian Distance
// double d = sqrt(xd * xd + yd * yd);
// Chebyshev distance
// int d = max(abs(xd), abs(yd));
heuristic[i][j] = d;
}
}
return heuristic;
}
};
// Planner class
class Planner : Map {
public:
int start[2] = { 230, 145 };
int goal[2] = { 60, 50 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
vector<vector<int> > path;
};
// Printing vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
Planner search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the quadruplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
int f = g + map.heuristic[x][y];
// Store the expansions
vector<vector<int> > open;
open.push_back({ f, g, x, y });
// Flags and Counts
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove quadruplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[2];
y = next[3];
g = next[1];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
f = g2 + map.heuristic[x2][y2];
open.push_back({ f, g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
//print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
// Store the Path in a vector
planner.path.push_back({ x2, y2 });
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the robot path
//cout << endl;
print2DVector(policy);
return planner;
}
int main()
{
// Instantiate a planner and map objects
Map map;
Planner planner;
// Generate the shortest Path using the Astar algorithm
planner = search(map, planner);
return 0;
}
Attached map.txt : See Robot Learning - Path Planning Experiment (2)
9. A*: Visualization
route plan
So far the shortest path has been generated using the A* algorithm, but it's hard to see it. Now, write the visualization function to plot the shortest path.
Clone the experiment from GitHub
$ cd /home/workspace/
$ git clone https://github.com/udacity/RoboND-A-VisualizationNext, edit main.cpp
Use the matplotlib python library to modify visualization functions and plot start locations, destination locations, and paths. Note the use of the letter "o" (instead of the numeric character "0") and the asterisk "*" to mark the start and end states in the visualization!
void visualization(Map map, Planner planner)
{
//Graph Format
plt::title("Path");
plt::xlim(0, map.mapHeight);
plt::ylim(0, map.mapWidth);
// Draw every grid of the map:
for (double x = 0; x < map.mapHeight; x++) {
cout << "Remaining Rows= " << map.mapHeight - x << endl;
for (double y = 0; y < map.mapWidth; y++) {
if (map.map[x][y] == 0) { //Green unkown state
plt::plot({ x }, { y }, "g.");
}
else if (map.map[x][y] > 0) { //Black occupied state
plt::plot({ x }, { y }, "k.");
}
else { //Red free state
plt::plot({ x }, { y }, "r.");
}
}
}
// TODO: Plot start and end states in blue colors using o and * respectively
// TODO: Plot the robot path in blue color using a .
//Save the image and close the plot
plt::save("./Images/Path.png");
plt::clf();
}Here are some useful commands to generate graphs using the matplotlib library:
-
Set the title: plt::Title("your title");
-
Set limits: plt::xlim(x-axis lower limit, x-axis upper limit);
-
Plot data: plt::Plot({x-value}, {y-value}, "color and shape");
-
Save the Plot: plt::Save("file name and directory");
-
Close the Close Plot: plt::clf();
For more information about the matplotlib C++ library, check out this link ( GitHub - lava/matplotlib-cpp: Extremely simple yet powerful header-only C++ plotting library built on the popular matplotlib ) . See the LineSpec and LineColor sections of the MATLAB ( 2-D line plot - MATLAB plot ) documentation for information on plot colors and shapes.
Then, compile the program
$ cd RoboND-A-Visualization/
$ rm -rf Images/* #Delete the folder content and not the folder itself!
$ g++ main.cpp -o app -std=c++11 -I/usr/include/python2.7 -lpython2.7Finally, run the program
$ ./appIf you get a warning about the matplotlib library, ignore it. Now, wait while the program generates paths and stores them in the /home/workspace/RoboND-A-Visualization/Images directory!
build path
map legend
-
Green: unknown/undiscovered areas
-
Red: vacant area
-
Black: occupied area
-
blue: shortest path
The reference code is as follows:
#include <iostream>
#include <math.h>
#include <vector>
#include <iterator>
#include <fstream>
#include "src/matplotlibcpp.h" //Graph Library
using namespace std;
namespace plt = matplotlibcpp;
// Map class
class Map {
public:
const static int mapHeight = 300;
const static int mapWidth = 150;
vector<vector<double> > map = GetMap();
vector<vector<int> > grid = MaptoGrid();
vector<vector<int> > heuristic = GenerateHeuristic();
private:
// Read the file and get the map
vector<vector<double> > GetMap()
{
vector<vector<double> > mymap(mapHeight, vector<double>(mapWidth));
ifstream myReadFile;
myReadFile.open("map.txt");
while (!myReadFile.eof()) {
for (int i = 0; i < mapHeight; i++) {
for (int j = 0; j < mapWidth; j++) {
myReadFile >> mymap[i][j];
}
}
}
return mymap;
}
//Convert the map to 1's and 0's
vector<vector<int> > MaptoGrid()
{
vector<vector<int> > grid(mapHeight, vector<int>(mapWidth));
for (int x = 0; x < mapHeight; x++) {
for (int y = 0; y < mapWidth; y++) {
if (map[x][y] == 0) //unkown state
grid[x][y] = 1;
else if (map[x][y] > 0) //Occupied state
grid[x][y] = 1;
else //Free state
grid[x][y] = 0;
}
}
return grid;
}
// Generate a Manhattan Heuristic Vector
vector<vector<int> > GenerateHeuristic()
{
vector<vector<int> > heuristic(mapHeight, vector<int>(mapWidth));
int goal[2] = { 60, 50 };
for (int i = 0; i < heuristic.size(); i++) {
for (int j = 0; j < heuristic[0].size(); j++) {
int xd = goal[0] - i;
int yd = goal[1] - j;
// Manhattan Distance
int d = abs(xd) + abs(yd);
// Euclidian Distance
// double d = sqrt(xd * xd + yd * yd);
// Chebyshev distance
// int d = max(abs(xd), abs(yd));
heuristic[i][j] = d;
}
}
return heuristic;
}
};
// Planner class
class Planner : Map {
public:
int start[2] = { 230, 145 };
int goal[2] = { 60, 50 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
vector<vector<int> > path;
};
// Printing vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
Planner search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the quadruplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
int f = g + map.heuristic[x][y];
// Store the expansions
vector<vector<int> > open;
open.push_back({ f, g, x, y });
// Flags and Counts
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove quadruplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[2];
y = next[3];
g = next[1];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
f = g2 + map.heuristic[x2][y2];
open.push_back({ f, g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
// Store the Path in a vector
planner.path.push_back({ x2, y2 });
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the robot path
cout << endl;
print2DVector(policy);
return planner;
}
void visualization(Map map, Planner planner)
{
//Graph Format
plt::title("Path");
plt::xlim(0, map.mapHeight);
plt::ylim(0, map.mapWidth);
// Draw every grid of the map:
for (double x = 0; x < map.mapHeight; x++) {
cout << "Remaining Rows= " << map.mapHeight - x << endl;
for (double y = 0; y < map.mapWidth; y++) {
if (map.map[x][y] == 0) { //Green unkown state
plt::plot({ x }, { y }, "g.");
}
else if (map.map[x][y] > 0) { //Black occupied state
plt::plot({ x }, { y }, "k.");
}
else { //Red free state
plt::plot({ x }, { y }, "r.");
}
}
}
// Plot start and end states
plt::plot({ (double)planner.start[0] }, { (double)planner.start[1] }, "bo");
plt::plot({ (double)planner.goal[0] }, { (double)planner.goal[1] }, "b*");
// Plot the robot path
for (int i = 0; i < planner.path.size(); i++) {
plt::plot({ (double)planner.path[i][0] }, { (double)planner.path[i][1] }, "b.");
}
//Save the image and close the plot
plt::save("./Images/Path.png");
plt::clf();
}
int main()
{
// Instantiate a planner and map objects
Map map;
Planner planner;
// Generate the shortest Path using the Astar algorithm
planner = search(map, planner);
// Plot the Map and the path generated
visualization(map, planner);
return 0;
}