A* and DFS path planning algorithms

2019 campus placement programming round

To clone this repository, use:

    git clone -b master https://github.com/athish-thirumal/athish_rapyuta.git

Setup and running

1. Install cmake, Visual Studio
2. Navigate to map_planner directory. Then create build files in 'build' directory

mkdir build
cd build
cmake -G "Visual Studio 15 2017" ..

3. To compile from command prompt:

cmake --build .

4. To run .exe from command prompt

cd Debug
map_planner_node.exe

Brief outline of program flow

Get start and goal coordinates in pixels from user --> Read image from /data folder and convert pixels to nodes of a graph --> Call A* planner function, print planned path to console out --> Call DFS planner function, print planned path to console out --> End

Reference material

https://www.redblobgames.com/pathfinding/a-star/introduction.html

Most of the knowledge required for programming this task was obtained from the above website. It contains interactive tools to understand how A* algorithm works. The author has also provided a basic template of code snippets to start implementing the algorithm in our own project, and the program I have written is inspired from this.

Description of some variables, classes and functions used

Frontier

In DFS, it is the stack that keeps track of the visited nodes. The stack is what gives the algorithm its 'Depth-First' logic because of its LIFO approach. On the other hand, BFS uses a queue to keep track of the visited nodes.

In A*, the frontier is a priority queue (not FIFO), where the elements are queued according to its priority value. Here we access elements from the priority queue that has the least priority. The priority value here is the sum of distance travelled so far and distance to goal.

came_from

This is used in both DFS and A* to keep track of which node was visited first before being in the current node. The reconstruct_path() function uses this to return the final path. It is updated for every node after the algorithm decides which node to visit next.

std::map<GridLocation, GridLocation> came_from

is a map/dictionary whose mapped value is the previously visited node, and the key value is the current node.

cost_so_far

Used by the A* algorithm to update the number of nodes it has visited so far. This when added with the distance to goal gives the priority used in this algorithm.

std::map<GridLocation, double> cost_so_far 

is a map whose key value is a grid location, and the mapped value is the distance in nodes from the start

GridLocation{}

Class that contains x and y coordinates of a particular pixel

heuristic()

Function to calculate distance from current location in grid to the goal, for use in A* algorithm. Here, the manhattan Distance is calculated.

reconstructpath(start, goal, came_from)

Function that recieves the start and goal locations and uses the came_from map to construct a path starting from the goal to the start. Then the vector of grid locations is reversed to get the path from start to goal. It returns std::vector.

SquareGrid{}

DIRS

Array that contains directions of traversal from a node. Here, traversal is possible in all eight directions.

Walls

A std::set that contains all the grid locations that are walls. This set is updated while reading from the image file, and cannot be modified later.

neighbours(GridLocation)

A member function that recieves a grid location as argument and returns a vector consisting of its neighboring nodes. It uses the DIRS directions defined and the walls set to find neighbours. std::vector neighbors(GridLocation){}

in_bounds(GridLocation)

Returns true if a node is within boundaries of the grid

notWall(GridLocation)

Returns true if a node is not a wall.

Overview of algorithms used

DFS

Depth First Search is a classic traversal method for graphs. It uses a stack to keep track of the visited nodes. It traverses throughout the depth of an adjacent node until a dead-end is reached, pushing each visited node into the stack. Once a dead-end is reached, it goes to the node in the stack that has unvisited neighbours.

DFS is not a commonly used algorithm for shortest path planning, as opposed to Breadth First Search, as it tends to expore the graph unevenly, and thus may take longer to reach the goal. It may even miss the goal when it's right next to the current visited node, since it only follows the stack to traverse. BFS on the other hand, searches the graph in a uniform fashion from the start node, and thus can reach the goal quicker.

A*

A* is a very commonly used for shortest path planning. Unlike BFS and DFS that searches the graph in all directions, A* tends to search in a more efficient manner. It used a priority queue to keep track of the visited nodes, where the priority is the sum of distance traversed so far and the distance to goal. This is different from Dijkstra's algorithm in the fact that Dijkstra's uses only the distance travelled so far, but A* also factors in the distance to goal. The node with the highest priority is visited first.

Future work and known issues

1. Implement multithreading to run the two algorithms in two threads I tried this, but encountered some problem with sharing variables so the final path was not correct. So I deleted all changes I made completely. However, I missed adding the changes to git, so am not able to retrieve them for documentation
2. Use smart pointers Initial study was done, but not tried yet.
3. Grouping pixels as a node to make the algorithm faster
4. Add logic to skip through the extra characters at the end of rows in .pgm file
5. Prioritize traversal direction (might be useful in DFS)

Output of program

Wall: # Path: @ For start = (10,10) and goal = (30,20)

Path planned using A* algorithm:

(10,10) (11,11) (12,12) (13,13) (14,14) (15,15) (16,16) (16,17) (15,18) (14,19) (14,20) (14,21) (14,22) (14,23) (14,24) (14,25) (13,26) (12,27) (13,28) (14,28) (15,28) (16,27) (17,26) (18,25) (19,24) (20,23) (21,22) (22,21) (23,20) (24,20) (25,20) (26,20) (27,20) (28,20) (29,20) (30,20)
..............................................................................................................
..............................................................................................................
..............................................................................................................
..............................................................................................................
..............................................................................................................
....#................................................##################################################.......
.....##################################################################################################.......
.....##################################################################################################.......
.....##################################################################################################.......
.....###.........###.................................###............................................###.......
.....###..@......###.................................###............................................###.......
.....###...@.....###.................................###............................................###.......
.....###....@....###.................................###............................................###.......
.....###.....@...###.................................###............................................###.......
.....###......@..###.................................###............................................###.......
.....###.......@.###.................................###............................................###.......
.....###........@###.................................###............................................###.......
.....###........@###.................................###.................................##.........###.......
.....###.......@.###.................................###........#########################.........###.........
...###........@###.................................###........###########################.........###.........
...###........@###.....@@@@@@@@....................###........#############..############.........###.........
...###........@###....@............................###........###.....................###.........###.........
...###........@###...@.............................###........###.....................###.........###.........
...###........@###..@..............................###........###.....................###.........###.........
...###........@###.@...............................###........###.....................###.........###.........
...###........@###@................................###........###.....................##.........###..........
..###........@###@................................###.........##.....................##.........###...........
.###........@###@................................###.........##.....................##.........###............
.........###.@@@.............................###.........##.....................##.........###............###.
........###.................................###.........##.....................##.........###............###..
.......##########################..........###.........##......................##.........###............###..
.......##########################..........###.........##......................##.........###............###..
.......##########################..........###.........##.....................###.........###............###..
...........................................###.........##.....................###.........###............###..

(showing only 33 rows)

Path planned using DFS algorithm:

(10,10) (11,10) (12,9) (13,10) (14,9) (15,10) (16,11) (15,12) (14,13) (13,12) (12,13) (11,12) (10,13) (9,12) (8,13) (8,14) (8,15) (9,16) (10,15) (11,16) (12,15) (13,16) (14,15) (15,16) (16,17) (15,18) (14,19) (13,18) (12,19) (11,18) (10,19) (9,18) (8,19) (7,20) (6,21) (7,22) (6,23) (7,24) (6,25) (5,26) (4,27) (3,28) (2,29) (1,28) (0,27) (1,26) (2,25) (1,24) (2,23) (1,22) (2,21) (1,20) (2,19) (3,18) (4,17) (3,16) (4,15) (3,14) (4,13) (3,12) (4,11) (3,10) (4,9) (3,8) (4,7) (3,6) (2,5) (3,4) (4,3) (5,2) (6,1) (7,0) (8,1) (9,0) (10,1) (11,0) (12,1) (13,0) (14,1) (15,0) (16,1) (17,0) (18,1) (19,0) (20,1) (21,0) (22,1) (23,0) (24,1) (25,0) (26,1) (27,0) (28,1) (29,0) (30,1) (31,0) (32,1) (33,0) (34,1) (35,0) (36,1) (37,0) (38,1) (39,0) (40,1) (41,0) (42,1) (43,0) (44,1) (45,0) (46,1) (47,0) (48,1) (49,0) (50,1) (51,0) (52,1) (53,0) (54,1) (55,0) (56,1) (57,0) (58,1) (59,0) (60,1) (61,0) (62,1) (63,0) (64,1) (65,0) (66,1) (67,0) (68,1) (69,0) (70,1) (71,0) (72,1) (73,0) (74,1) (75,0) (76,1) (77,0) (78,1) (79,0) (80,1) (81,0) (82,1) (83,0) (84,1) (85,0) (86,1) (87,0) (88,1) (89,0) (90,1) (91,0) (92,1) (93,0) (94,1) (95,0) (96,1) (97,0) (98,1) (99,0) (100,1) (101,0) (102,1) (103,0) (104,1) (105,0) (106,1) (107,0) (108,1) (109,2) (108,3) (107,4) (106,3) (105,4) (104,3) (103,4) (103,5) (103,6) (104,7) (105,6) (106,7) (107,6) (108,7) (109,8) (108,9) (107,10) (106,9) (105,10) (104,9) (103,10) (103,11) (103,12) (104,13) (105,12) (106,13) (107,12) (108,13) (109,14) (108,15) (107,16) (106,15) (105,16) (104,15) (103,16) (103,17) (103,18) (102,19) (101,20) (102,21) (101,22) (102,23) (101,24) (100,25) (99,26) (98,27) (97,28) (96,29) (95,28) (94,27) (95,26) (96,25) (97,24) (96,23) (97,22) (96,21) (97,20) (96,19) (97,18) (98,17) (99,16) (98,15) (99,14) (98,13) (99,12) (98,11) (99,10) (98,9) (97,9) (96,9) (95,10) (94,9) (93,10) (92,9) (91,10) (90,9) (89,10) (88,9) (87,10) (86,9) (85,10) (84,9) (83,10) (82,9) (81,10) (80,9) (79,10) (78,9) (77,10) (76,9) (75,10) (74,9) (73,10) (72,9) (71,10) (70,9) (69,10) (68,9) (67,10) (66,9) (65,10) (64,9) (63,10) (62,9) (61,10) (60,9) (59,10) (58,9) (57,10) (56,11) (57,12) (56,13) (57,14) (56,15) (57,16) (56,17) (57,18) (56,19) (55,20) (54,21) (55,22) (54,23) (55,24) (54,25) (53,26) (52,27) (51,28) (50,29) (49,28) (48,27) (49,26) (50,25) (49,24) (50,23) (49,22) (50,21) (49,20) (50,19) (51,18) (52,17) (51,16) (52,15) (51,14) (52,13) (51,12) (52,11) (51,10) (50,9) (49,10) (48,9) (47,10) (46,9) (45,10) (44,9) (43,10) (42,9) (41,10) (40,9) (39,10) (38,9) (37,10) (36,9) (35,10) (34,9) (33,10) (32,9) (31,10) (30,9) (29,10) (28,9) (27,10) (26,9) (25,10) (24,9) (23,10) (22,9) (21,10) (20,11) (21,12) (20,13) (21,14) (20,15) (21,16) (20,17) (21,18) (20,19) (19,20) (18,21) (19,22) (18,23) (19,24) (18,25) (17,26) (16,27) (15,28) (14,29) (13,28) (12,27) (13,26) (14,25) (13,24) (14,23) (13,22) (12,21) (11,22) (10,21) (9,22) (9,23) (9,24) (10,25) (9,26) (8,27) (7,28) (6,29) (5,30) (4,31) (3,32) (2,31) (1,32) (0,33) (1,34) (0,35) (1,36) (0,37) (1,38) (0,39) (1,40) (0,41) (1,42) (0,43) (1,44) (0,45) (1,46) (0,47) (1,48) (2,48) (3,47) (3,46) (3,45) (3,44) (3,43) (3,42) (4,41) (5,40) (6,39) (7,38) (8,37) (9,36) (10,35) (11,34) (12,33) (13,34) (14,33) (15,34) (16,33) (17,34) (18,33) (19,34) (20,33) (21,34) (22,33) (23,34) (24,33) (25,34) (26,33) (27,34) (28,33) (29,34) (30,33) (31,34) (32,33) (33,32) (34,31) (35,30) (36,29) (37,28) (38,27) (39,26) (40,25) (41,24) (42,23) (43,22) (44,21) (45,20) (46,19) (47,18) (48,17) (49,16) (48,15) (49,14) (48,13) (47,12) (46,13) (45,12) (44,13) (43,12) (42,13) (41,12) (40,13) (39,12) (38,13) (37,12) (36,13) (35,12) (34,13) (33,12) (32,13) (31,12) (30,13) (29,12) (28,13) (27,12) (26,13) (25,12) (24,13) (23,14) (24,15) (23,16) (24,17) (23,18) (24,19) (23,20) (22,21) (21,22) (22,23) (21,24) (22,25) (21,26) (20,27) (19,28) (20,29) (21,29) (22,29) (23,28) (24,27) (25,26) (26,25) (27,24) (28,23) (29,22) (30,21) (30,20)
.......@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@..
......@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.
.....@.......................................................................................................@
....@...................................................................................................@.@.@.
...@...................................................................................................@.@.@..
..@.#................................................##################################################@......
...@.##################################################################################################@.@.@..
....@##################################################################################################.@.@.@.
...@.##################################################################################################......@
....@###....@.@..###..@.@.@.@.@.@.@.@.@.@.@.@.@.@.@..###..@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@@@.###.@.@.@.
...@.###..@@.@.@.###.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.###.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@.@...@###@.@.@..
....@###........@###@...............................@###@.........................................@.###@......
...@.###.@.@.@.@.###.@...@.@.@.@.@.@.@.@.@.@.@.@...@.###.@.........................................@###@.@.@..
....@###@.@.@.@..###@...@.@.@.@.@.@.@.@.@.@.@.@.@...@###@.........................................@.###.@.@.@.
...@.###@........###.@.@.........................@.@.###.@.........................................@###......@
....@###@.@.@.@..###@...@.......................@...@###@.........................................@.###.@.@.@.
...@.###.@.@.@.@.###.@.@.........................@.@.###.@.........................................@###@.@.@..
....@###........@###@...@.......................@...@###@................................##.......@.###@......
...@.###.@.@.@.@.###.@.@.......................@...@.###.@......#########################........@###..@......
..@###..@.@.@.@###..@...@.....................@...@###..@.....###########################.......@.###.@.......
.@.###.@.......###.@...@......@..............@...@.###.@......#############..############........@###@........
..@###@...@.@..###@...@.......@.............@.....@###@.......###.....................###.......@.###.@.......
.@.###.@.@.@.@.###.@.@.......@.............@.....@.###.@......###.....................###........@###@........
..@###@..@....@###@...@.....@.............@.......@###@.......###.....................###.......@.###.@.......
.@.###.@.@...@.###.@.@.....@.............@.......@.###.@......###.....................###........@###@........
..@###@...@...@###@...@...@.............@.........@###@.......###.....................##........@###@.........
.@###@...@...@###@...@...@.............@.........@###@........##.....................##........@###@..........
@###@...@...@###@...@...@.............@.........@###@........##.....................##........@###@...........
.@.@...@.###.@.@...@...@.............@.......###.@.@.....##.....................##.........###.@.@........###.
..@...@.###...@.....@@@.............@.......###...@.....##.....................##.........###...@........###..
.....@.##########################..@.......###.........##......................##.........###............###..
..@.@..##########################.@........###.........##......................##.........###............###..
.@.@...##########################@.........###.........##.....................###.........###............###..
@...........@.@.@.@.@.@.@.@.@.@.@..........###.........##.....................###.........###............###..
.@.........@.@.@.@.@.@.@.@.@.@.@...........###.........##.....................###.........###............###..
@.........@................................###.........##.....................###.........###............###..
.@.......@.................................###.........#.....................###.........###............###...
@.......@.................................###.........##.....................###.........###............###...
.@.....@..................................###.........##.....................###.........###............###...
@.....@...................................###.........##.....................###.........###............###...
.@...@....................................###.........##.....................###.........###............###...
@...@..##################.................###.........##.....................###.........###............###...
.@.@..#####################################.........##......................##.........###............###.....
@..@#######################################.................................##.........###............###.....
.@.@#######################################................................###.........###............###.....
@..@###.................................###................................###.........###............###.....
.@.@###.................................###................................###.........###............###.....
@..@###.................................###................................##.........###............###......
.@@###.................................###................................##.........###............###.......
..###.................................###................................###.........###............###.......
..###.................................###................................###.........###............###.......

(showing only 50 rows)