├── .gitattributes ├── Visualization_Output.png ├── LICENSE ├── Visualization_Script.m ├── A_Star_Algorithm_Nodes.csv ├── README.md └── A_Star_Algorithm.py /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Visualization_Output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arunumd/A_Star_Algorithm_Path_Planning/HEAD/Visualization_Output.png -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Arun Kumar D 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Visualization_Script.m: -------------------------------------------------------------------------------- 1 | xL = 250; 2 | yL = 150; 3 | line([0 0], yL); %y-axis 4 | line(xL, [0 0]); %x-axis 5 | 6 | r = rectangle('Position',[55 78.5 50 45]); 7 | %axis([0 250 0 150]) 8 | 9 | r.FaceColor = [0 .5 .5]; 10 | r.EdgeColor = 'b'; 11 | r.LineWidth = 3; 12 | 13 | x = [145 168 188 165 158 120]; 14 | y = [14 14 51 89 51 55]; 15 | p = patch(x,y,'blue'); 16 | 17 | p.FaceColor = [0 .5 .5]; 18 | p.EdgeColor = 'b'; 19 | p.LineWidth = 3; 20 | 21 | pos = [165 105 15 15]; 22 | c = rectangle('Position',pos,'Curvature',[1 1]); 23 | axis equal 24 | 25 | c.FaceColor = [0 .5 .5]; 26 | c.EdgeColor = 'b'; 27 | c.LineWidth = 3; 28 | 29 | hold on 30 | 31 | pointsx = [185,184,183,182,181,180,179,178,177,176,175,174,173,172,171,170,169,168,167,166,165,164,163,162,161,160,159,158,157,156,156,156,156,156,156,156,156,156,156,156,156,156,156,155,154,153,152,151,150,149,148,147,146,145,144,143,142,141,140,139,138,137,136,135,134,133,132,131,130,129,128,127,126,125,124,123,122,121,120]; 32 | pointsy = [82,82,83,84,85,86,87,88,89,90,90,90,90,90,90,90,90,90,90,90,90,89,88,87,86,85,84,83,82,81,80,79,78,77,76,75,74,73,72,71,70,69,68,67,66,65,64,63,62,61,60,59,58,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57]; 33 | plot(pointsx,pointsy); 34 | axis([0 280 0 180]) 35 | %out = zeros(10); 36 | %out(sub2ind(size(out),x,y)) = z; 37 | 38 | -------------------------------------------------------------------------------- /A_Star_Algorithm_Nodes.csv: -------------------------------------------------------------------------------- 1 | 1,"(185, 82)" 2 | 2,"(184, 82)" 3 | 3,"(183, 83)" 4 | 4,"(182, 84)" 5 | 5,"(181, 85)" 6 | 6,"(180, 86)" 7 | 7,"(179, 87)" 8 | 8,"(178, 88)" 9 | 9,"(177, 89)" 10 | 10,"(176, 90)" 11 | 11,"(175, 90)" 12 | 12,"(174, 90)" 13 | 13,"(173, 90)" 14 | 14,"(172, 90)" 15 | 15,"(171, 90)" 16 | 16,"(170, 90)" 17 | 17,"(169, 90)" 18 | 18,"(168, 90)" 19 | 19,"(167, 90)" 20 | 20,"(166, 90)" 21 | 21,"(165, 90)" 22 | 22,"(164, 89)" 23 | 23,"(163, 88)" 24 | 24,"(162, 87)" 25 | 25,"(161, 86)" 26 | 26,"(160, 85)" 27 | 27,"(159, 84)" 28 | 28,"(158, 83)" 29 | 29,"(157, 82)" 30 | 30,"(156, 81)" 31 | 31,"(156, 80)" 32 | 32,"(156, 79)" 33 | 33,"(156, 78)" 34 | 34,"(156, 77)" 35 | 35,"(156, 76)" 36 | 36,"(156, 75)" 37 | 37,"(156, 74)" 38 | 38,"(156, 73)" 39 | 39,"(156, 72)" 40 | 40,"(156, 71)" 41 | 41,"(156, 70)" 42 | 42,"(156, 69)" 43 | 43,"(156, 68)" 44 | 44,"(155, 67)" 45 | 45,"(154, 66)" 46 | 46,"(153, 65)" 47 | 47,"(152, 64)" 48 | 48,"(151, 63)" 49 | 49,"(150, 62)" 50 | 50,"(149, 61)" 51 | 51,"(148, 60)" 52 | 52,"(147, 59)" 53 | 53,"(146, 58)" 54 | 54,"(145, 57)" 55 | 55,"(144, 57)" 56 | 56,"(143, 57)" 57 | 57,"(142, 57)" 58 | 58,"(141, 57)" 59 | 59,"(140, 57)" 60 | 60,"(139, 57)" 61 | 61,"(138, 57)" 62 | 62,"(137, 57)" 63 | 63,"(136, 57)" 64 | 64,"(135, 57)" 65 | 65,"(134, 57)" 66 | 66,"(133, 57)" 67 | 67,"(132, 57)" 68 | 68,"(131, 57)" 69 | 69,"(130, 57)" 70 | 70,"(129, 57)" 71 | 71,"(128, 57)" 72 | 72,"(127, 57)" 73 | 73,"(126, 57)" 74 | 74,"(125, 57)" 75 | 75,"(124, 57)" 76 | 76,"(123, 57)" 77 | 77,"(122, 57)" 78 | 78,"(121, 57)" 79 | 79,"(120, 57)" -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A Star Algorithm - Path Planning 2 | 3 | This repository contains a Python implementation of an A Star algorithm for shortest path finding in an environment with static obstacles. The obstacles are hardcoded as a set of polygons, triangles and circles inside the algorithm inside the `class Obstacles`. You can easily create your own obstacles by modiying this class. The obstacle bounds checking is done by using half planes, slopes and intercepts concepts. The A Star algorithm involves calculation of two costs for every new node (cost to go and cost to come). Using these two costs, the total cost for reaching a node from any given node is calculated. The nodes are grown in eight directions from any given node and the total costs for all the newly generated nodes are calculated. Finally, when we arrive at our destination node, the algorithm terminates. We maintain a dictionary of all the generated nodes and then use the principle of backtracking from tail to head to get the shortest path. The visualization is done separately by using the provided MATLAB script. 4 | 5 | # Dependencies for running the code 6 | - MATLAB 7 | - Python 2.7 Interpreter 8 | - Any CSV file viewer like Microsoft Excel, Google Sheets, Libre Office, etc. 9 | 10 | # How to run the algorithm ? 11 | 12 | - First run the file `A_Star_Algorithm.py` 13 | - When the algorithm finishes perfectly, you will see the nodes being generated in `A_Star_Algorithm_Nodes.csv` 14 | - Finally, run the MATLAB script `Visualization_Script.m` 15 | 16 | # Visualization of the Shortest Path Generated from the Algorithm 17 | 18 | ![Shortest_Path_Output](Visualization_Output.png) 19 | -------------------------------------------------------------------------------- /A_Star_Algorithm.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | from itertools import groupby 3 | from operator import itemgetter 4 | import math 5 | import timeit 6 | import csv 7 | import random 8 | 9 | class Move: 10 | 11 | """Eight directional movement""" 12 | def __init__(self,vertexStart,vertexFinish): 13 | self.costgLinear = 10 14 | self.costgDiagonal = 14 15 | self.vertexStart = vertexStart 16 | self.vertexFinish = vertexFinish 17 | 18 | def setstartpoint(self,vertexStart): 19 | self.vertexStart = vertexStart 20 | 21 | def distance(self): 22 | return ((self.vertexFinish[0] - self.vertexStart[0]) + (self.vertexFinish[1] - self.vertexStart[1])) 23 | #return math.sqrt(math.pow(((float(self.vertexStart[0]) - float(self.vertexFinish[0]))*10),2) + math.pow(((float(self.vertexStart[1]) - float(self.vertexFinish[1]))*10),2)) 24 | 25 | def East(self): 26 | """Moves one step in East""" 27 | self.output = [(self.vertexStart[0] + 1), self.vertexStart[1], self.costgLinear, self.distance()] 28 | return self.output 29 | 30 | def West(self): 31 | """Moves one step West""" 32 | self.output = [(self.vertexStart[0] - 1), self.vertexStart[1], self.costgLinear, self.distance()] 33 | return self.output 34 | 35 | def North(self): 36 | """Moves one step North""" 37 | self.output = [self.vertexStart[0], (self.vertexStart[1] + 1), self.costgLinear, self.distance()] 38 | return self.output 39 | 40 | def South(self): 41 | """Moves one step South""" 42 | self.output = [self.vertexStart[0], (self.vertexStart[1] - 1), self.costgLinear, self.distance()] 43 | return self.output 44 | 45 | def Northeast(self): 46 | """Moves one step Northeast""" 47 | self.output = [(self.vertexStart[0] + 1), (self.vertexStart[1] + 1), self.costgDiagonal, self.distance()] 48 | return self.output 49 | 50 | def Southeast(self): 51 | """Moves one step Southeast""" 52 | self.output = [(self.vertexStart[0] + 1), (self.vertexStart[1] - 1), self.costgDiagonal, self.distance()] 53 | return self.output 54 | 55 | def Northwest(self): 56 | """Moves one step Northwest""" 57 | self.output = [(self.vertexStart[0] - 1), (self.vertexStart[1] + 1), self.costgDiagonal, self.distance()] 58 | return self.output 59 | 60 | def Southwest(self): 61 | """Moves one step Southwest""" 62 | self.output = [(self.vertexStart[0] - 1), (self.vertexStart[1] - 1), self.costgDiagonal, self.distance()] 63 | return self.output 64 | 65 | 66 | class Obstacles: 67 | 68 | def __init__(self,testpoint): 69 | self.testpoint = testpoint 70 | 71 | def circle(self): 72 | self.cright = math.pow(15, 2) 73 | self.cleft = math.pow((self.testpoint[0] - 180), 2) + math.pow((self.testpoint[1] - 120), 2) 74 | if self.cleft <= self.cright: 75 | return True 76 | else: 77 | return False 78 | 79 | def square(self): 80 | if self.testpoint[0] >= 55 and self.testpoint[0] <= 155 and self.testpoint[1] >= 67.5 and self.testpoint[1] <= 112.5: 81 | return True 82 | else: 83 | return False 84 | 85 | def polygon(self): 86 | self.lines = [[[188.0,51.0],[168.0,14.0]],[[165.0,89.0],[188.0,51.0]],[[158.0,51.0],[165.0,89.0]],[[120.0,55.0],[158.0,51.0]],[[145.0,14.0],[120.0,55.0]],[[168,14],[158,51]]] 87 | slopes = [] 88 | y_intercepts = [] 89 | m_and_b = [] 90 | for each in self.lines: 91 | numerator = each[1][1] - each[0][1] 92 | denominator = each[1][0] - each[0][0] 93 | slope = numerator / denominator 94 | slopes.append(slope) 95 | y_intercept = (each[0][1] - (slope * each[0][0])) 96 | y_intercepts.append(y_intercept) 97 | m_and_b.append([slope, y_intercept]) 98 | RHS_Set = [] 99 | for slope_intercept in m_and_b: 100 | RHS = ((float(slope_intercept[0]) * float(self.testpoint[0])) + float(slope_intercept[1])) 101 | RHS_Set.append(RHS) 102 | 103 | if self.testpoint[1] >= RHS_Set[0] and self.testpoint[1] <= RHS_Set[1] and self.testpoint[1] <= RHS_Set[2] and self.testpoint[1] >= RHS_Set[5]: 104 | return True 105 | elif self.testpoint[1] <= RHS_Set[3] and self.testpoint[1] >= RHS_Set[4] and self.testpoint[1] <= RHS_Set[5] and self.testpoint[1] >= 14.0: 106 | return True 107 | else: 108 | return False 109 | 110 | def scope(self): 111 | if self.testpoint[0] >= 0 and self.testpoint[0] <= 250 and self.testpoint[1] >= 0 and self.testpoint[1] <= 150: 112 | return True 113 | else: 114 | return False 115 | 116 | def clearance(self): 117 | if self.scope() == True and self.polygon() == False and self.square() == False and self.circle() == False: 118 | return True 119 | else: 120 | return False 121 | 122 | def user_input(): 123 | cond = False 124 | while cond is False: 125 | x = input("Please enter the x-coordinate : ") 126 | y = input("Please enter the y-coordinate : ") 127 | points = [x,y] 128 | if type(x) != int: 129 | print("The x co-ordinate you have entered is not an integer. Please try again !") 130 | cond = False 131 | elif type(y) != int: 132 | print("The y co-ordinate you have entered is not an integer. Please try again !") 133 | cond = False 134 | else: 135 | cond = True 136 | return points 137 | 138 | 139 | 140 | #----------------------------------------------------------------- 141 | 142 | #location_info = iterpoint; passed_list = openlist; passed_dict = storage; lastpoint = end; nodeNr = ?; 143 | 144 | def core(location_info, passed_list, passed_dict, lastpoint, nodeNr, epctr): 145 | """Then we extract our interested node for exploration""" 146 | #print("the new dict is : ",passed_dict) 147 | grabbeditem = location_info 148 | print(location_info) 149 | #print("The grabbed node is : ",grabbeditem, "of type ",type(grabbeditem)) 150 | iternode = [grabbeditem[0],grabbeditem[1]] 151 | #print("the iternode is : ",iternode, " and iternode[0] is :",iternode[0], " is of type ", type(iternode[0])) 152 | """We first copy our current node to closed dictionary""" 153 | dictleft = tuple(iternode) 154 | dictright = list(grabbeditem[2:7]) 155 | if dictleft not in passed_dict.keys(): 156 | passed_dict.update({dictleft: dictright}) 157 | #print(passed_dict) 158 | """We also grab the node number before killing it from our open list""" 159 | parent_node_no = int(grabbeditem[2]) 160 | #print("parent no is : ", parent_node_no) 161 | """We grab the Cost G which is already present in current node and use this for further increments""" 162 | Gcost = int(grabbeditem[4]) 163 | #print("Gcost is : ", Gcost) 164 | """After copying our current node, we delete the current node from openlist""" 165 | #print("passed list is : ", passed_list) 166 | passed_list.remove(grabbeditem) 167 | #print (passed_list) 168 | """We grab our current node for creating daughters""" 169 | currentnode = list(iternode) 170 | #print(currentnode) 171 | """Creating an empty swap list""" 172 | swaplist = [] 173 | 174 | mover = Move(currentnode,lastpoint) 175 | 176 | Nop = mover.North() 177 | if Obstacles([Nop[0],Nop[1]]).clearance(): 178 | swaplist.append(Nop) 179 | else: 180 | pass 181 | 182 | mover.setstartpoint(currentnode) 183 | Sop = mover.South() 184 | if Obstacles([Sop[0], Sop[1]]).clearance(): 185 | swaplist.append(Sop) 186 | else: 187 | pass 188 | 189 | mover.setstartpoint(currentnode) 190 | Eop = mover.East() 191 | if Obstacles([Eop[0], Sop[1]]).clearance(): 192 | swaplist.append(Eop) 193 | else: 194 | pass 195 | 196 | mover.setstartpoint(currentnode) 197 | Wop = mover.West() 198 | if Obstacles([Wop[0], Wop[1]]).clearance(): 199 | swaplist.append(Wop) 200 | else: 201 | pass 202 | 203 | mover.setstartpoint(currentnode) 204 | NEop = mover.Northeast() 205 | if Obstacles([NEop[0], NEop[1]]).clearance(): 206 | swaplist.append(NEop) 207 | else: 208 | pass 209 | 210 | mover.setstartpoint(currentnode) 211 | NWop = mover.Northwest() 212 | if Obstacles([NWop[0], NWop[1]]).clearance(): 213 | swaplist.append(NWop) 214 | else: 215 | pass 216 | 217 | mover.setstartpoint(currentnode) 218 | SEop = mover.Southeast() 219 | if Obstacles([SEop[0], SEop[1]]).clearance(): 220 | swaplist.append(SEop) 221 | else: 222 | pass 223 | 224 | mover.setstartpoint(currentnode) 225 | SWop = mover.Southwest() 226 | if Obstacles([SWop[0], SWop[1]]).clearance(): 227 | swaplist.append(SWop) 228 | else: 229 | pass 230 | 231 | 232 | #print("the swaplist after actions is : ",swaplist) 233 | counter = 0 234 | 235 | for daughters in swaplist: 236 | totalcostG = int(daughters[2] + Gcost) 237 | totalcostF = float(totalcostG + daughters[3]) 238 | common = [item for item in passed_list if (item[0] == daughters[0] and item[1] == daughters[1])] #Filter the common elements in openlist 239 | if len(common) >=1: 240 | for each in common: 241 | if float(each[6]) < totalcostF: 242 | counter+=1 243 | if counter >= 1: 244 | break 245 | else: 246 | nodeNr+=1 247 | appendmember = [daughters[0],daughters[1],nodeNr, parent_node_no, totalcostG, daughters[3],totalcostF] 248 | passed_list.append(appendmember) 249 | else: 250 | pass 251 | 252 | elif len(common) == 0: 253 | nodeNr += 1 254 | appendmember = [daughters[0], daughters[1], nodeNr, parent_node_no, totalcostG, daughters[3], totalcostF] 255 | passed_list.append(appendmember) 256 | 257 | else: 258 | pass 259 | 260 | for each in swaplist: 261 | if (each[0],each[1]) == tuple(lastpoint): 262 | epctr+=1 263 | left = (each[0],each[1]) 264 | nodeNr +=1 265 | totalcostG = int(each[2]+Gcost) 266 | costH = float(each[3]) 267 | totalcostF = float(totalcostG + each[3]) 268 | right = [nodeNr, parent_node_no, totalcostG, costH, totalcostF] 269 | passed_dict.update({left:right}) 270 | totalcostG = int(each[2] + Gcost) 271 | checker = (each[0],each[1]) 272 | if checker in passed_dict.keys(): 273 | value = list(passed_dict.get(checker)) 274 | if totalcostG < value[2]: 275 | value[1] = int(parent_node_no) 276 | value = tuple(value) 277 | passed_dict.update({checker:value}) 278 | else: 279 | pass 280 | else: 281 | pass 282 | 283 | 284 | return nodeNr, epctr, passed_dict, passed_list 285 | 286 | def main(): 287 | start = timeit.default_timer() 288 | print("Please enter the start points") 289 | start = user_input() 290 | startObst = Obstacles(start) 291 | while startObst.clearance() == False: 292 | start = user_input() 293 | startObst = Obstacles(start) 294 | print("Please enter the end points") 295 | end = user_input() 296 | endObst = Obstacles(end) 297 | while endObst.clearance() == False: 298 | end = user_input() 299 | endObst = Obstacles(end) 300 | movement = Move(start,end) 301 | max_costh = movement.distance() 302 | first_key = (start[0],start[1]) 303 | first_value = [1,0,0,max_costh,max_costh] 304 | storage = {first_key:first_value} 305 | openlist = [[start[0],start[1],1,0,0,max_costh,max_costh]] 306 | counter = 2 307 | node_no = 1 308 | end_counter = 0 309 | while end_counter <= 2: 310 | 311 | if end_counter == 2: 312 | #print(storage) 313 | publish = {} 314 | point_counter = 1 315 | consideration = tuple(end) 316 | publish.update({consideration:point_counter}) 317 | terminator = 0 318 | parentno = 0 319 | while terminator < 1: 320 | parentno = int(storage.get(consideration)[1]) 321 | for key,value in storage.items(): 322 | if value[0] == parentno: 323 | consideration = key 324 | point_counter +=1 325 | publish.update({consideration:point_counter}) 326 | if consideration == tuple(start): 327 | # Reference: https://stackoverflow.com/questions/8023306/get-key-by-value-in-dictionary?utm_medium=organic&utm_source=google_rich_qa&utm_campaign=google_rich_qa 328 | terminator = 2 329 | print("**** Pulling out the embedded viper from Tail to Head now ****") 330 | for key, value in sorted(publish.iteritems(), key=lambda (k, v): (v, k)): 331 | print("%s: %s" % (value, key)) 332 | 333 | 334 | 335 | print("the publish is", publish) 336 | w = csv.writer(open("A_Star_Algorithm_Nodes.csv", "w")) 337 | for key, val in publish.iteritems(): 338 | w.writerow([val, key]) 339 | 340 | break 341 | 342 | """Select the minimum of cost f column(index 6)""" 343 | templist1 = [min(openlist, key=itemgetter(6))] 344 | #print(templist1) 345 | # print("the open list inside filter 1 is ",openlist) 346 | 347 | if len(templist1) > 1: 348 | templist2 = [min(templist1, key=itemgetter(5))] 349 | iterpoint = templist2[0] 350 | #print("the open list in filter 2 is : ",openlist," the iterpoint is :",iterpoint) 351 | # location_info = iterpoint; passed_list = openlist; passed_dict = storage; lastpoint = end; nodeNr = ?; 352 | node_no, end_counter, storage, openlist = core(iterpoint, openlist, storage, end, node_no, end_counter) 353 | 354 | 355 | elif len(templist1) == 1: 356 | templist2 = templist1[0] 357 | #print("the transfer node is :",transfernode) 358 | #print("the lowest h cost is : ",transfernode[5]) 359 | node_no, end_counter, storage, openlist = core(templist2, openlist, storage, end, node_no, end_counter) 360 | 361 | 362 | else: 363 | pass 364 | 365 | 366 | main() 367 | --------------------------------------------------------------------------------