├── __init__.py ├── 1.jpg ├── 2.jpg ├── 3.jpg ├── 4.jpg ├── 5.jpg ├── sample1.jpg ├── output1 ├── 1.jpg ├── 2.jpg ├── 3.jpg ├── 4.jpg ├── 5.jpg ├── output11.jpg ├── output12.jpg ├── output13.jpg ├── output14.jpg └── output15.jpg ├── output2 ├── 1.jpg ├── 2.jpg ├── 3.jpg ├── 4.jpg ├── 5.jpg └── 6.jpg ├── .idea ├── encodings.xml ├── modules.xml ├── Hybrid-Artificial-Potential-Field-A-star-Planning.iml ├── misc.xml └── workspace.xml ├── README.md ├── A-star-potential-field-hybrid-type-1.py ├── A-star-potential-field-hybrid-type-1_1.py ├── A-star-potential-field-hybrid-type-1-controller.py └── A-star-potetial-field-hybrid-type-2.py /__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/1.jpg -------------------------------------------------------------------------------- /2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/2.jpg -------------------------------------------------------------------------------- /3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/3.jpg -------------------------------------------------------------------------------- /4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/4.jpg -------------------------------------------------------------------------------- /5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/5.jpg -------------------------------------------------------------------------------- /sample1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/sample1.jpg -------------------------------------------------------------------------------- /output1/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/1.jpg -------------------------------------------------------------------------------- /output1/2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/2.jpg -------------------------------------------------------------------------------- /output1/3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/3.jpg -------------------------------------------------------------------------------- /output1/4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/4.jpg -------------------------------------------------------------------------------- /output1/5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/5.jpg -------------------------------------------------------------------------------- /output2/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output2/1.jpg -------------------------------------------------------------------------------- /output2/2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output2/2.jpg -------------------------------------------------------------------------------- /output2/3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output2/3.jpg -------------------------------------------------------------------------------- /output2/4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output2/4.jpg -------------------------------------------------------------------------------- /output2/5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output2/5.jpg -------------------------------------------------------------------------------- /output2/6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output2/6.jpg -------------------------------------------------------------------------------- /output1/output11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/output11.jpg -------------------------------------------------------------------------------- /output1/output12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/output12.jpg -------------------------------------------------------------------------------- /output1/output13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/output13.jpg -------------------------------------------------------------------------------- /output1/output14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/output14.jpg -------------------------------------------------------------------------------- /output1/output15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Hybrid-Artificial-Potential-Field-A-star-Planning/HEAD/output1/output15.jpg -------------------------------------------------------------------------------- /.idea/encodings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/Hybrid-Artificial-Potential-Field-A-star-Planning.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Hybrid-Artificial-Potential-Field-A-star-Planning 2 | This method takes best of both world. On one hand,it tries to reduce the overall path cost by using A-star and on other hand it reduces the time complexity by adapting real time reactive power from Artificial-Potential method of motion Planning. 3 | Welcome to the Hybrid-Artificial-Potential-Field-A-star-Planning wiki! 4 | 5 | Here we are trying to mix two known method of motion planning , A-star and Artificial-Potential Field method. 6 | 7 | We are implementing these two methods using following two mixtures(Environment is considered to be static). 8 | 9 | We have find the planned path using A*, which will run offline ad find a path from source to destination. 10 | Now two variations of reactive method (Artificial Potential field ) is boosted by this already planned path. 11 | 12 | 1) We know that A-star is offline algorithm, which calculates path to goal directly without any feedback from next frame. Now we can divide our planned path into local goals and use Artificial-potential method to get to those. This way it will be faster and cheaper.The algorithm goes like this. 13 | 14 | ``` 15 | local_goals = divide path from A* 16 | local_source = current source 17 | local_goal = local_goals[0] 18 | path = empty_list 19 | while local_source != final_goal: 20 | path += Artificial_potential_path(local_source, local_goal) 21 | local_source = local_goal 22 | local_goal = next(local_goals) 23 | print path 24 | ``` 25 | 2)Here we use A-star and Potential field simultaneously depending on some parameters. Currently in this code we are considering that parameter as distance from nearest obstacle. So algorithm goes like this. 26 | 27 | ``` 28 | if distance from nearest obstacle > k(some parameter): 29 | next position = A-star planner 30 | else: 31 | next position = Artificial-Potential-field planner 32 | ``` 33 | 34 | --- 35 | 36 | Prerequisites 37 | - Python 38 | - OpenCV 39 | - Numpy 40 | - Matplotlib 41 | 42 | Input Images 43 | It will take all images in root folder as input images. 44 | 45 | Sample Input Images: 46 | ![Alt text](1.jpg?raw=true "Sample Image") 47 | 48 | 49 | ![Alt text](2.jpg?raw=true "Sample Image") 50 | 51 | 52 | ![Alt text](3.jpg?raw=true "Sample Image") 53 | 54 | 55 | Output Type 1: 56 | 57 | ![Alt text](output1/1.jpg?raw=true "Sample Image") 58 | 59 | 60 | ![Alt text](output1/2.jpg?raw=true "Sample Image") 61 | 62 | 63 | ![Alt text](output1/3.jpg?raw=true "Sample Image") 64 | 65 | 66 | Output Type 2: 67 | ![Alt text](output2/1.jpg?raw=true "Sample Image") 68 | 69 | 70 | 71 | ![Alt text](output2/2.jpg?raw=true "Sample Image") 72 | 73 | 74 | ![Alt text](output2/3.jpg?raw=true "Sample Image") 75 | -------------------------------------------------------------------------------- /.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 1457433692565 84 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /A-star-potential-field-hybrid-type-1.py: -------------------------------------------------------------------------------- 1 | ''' 2 | In this method , we are trying to find offline path first using A* algorithm, then we use Artificial potential method 3 | to reach local goals on A* planned path. 4 | Suppose, a1, a2, a3, ....... an be path we get from A*, then to potential field we give following as consecutive goals: 5 | a(i), a(i+4), a(i+8), a(i+12), ..... a(i+n) 6 | 7 | Hence this way we are using power of both A-star and potential field :) 8 | ''' 9 | 10 | import cv2 11 | import numpy as np 12 | from time import sleep 13 | import copy 14 | import glob 15 | import math 16 | import time 17 | import Queue as Q 18 | 19 | 20 | def printx(x): 21 | #print x 22 | pass 23 | ''' 24 | function definition from A-star 25 | start 26 | ''' 27 | class pixel1(object): 28 | def __init__(self, penalty, pointx, pointy, parent, h): # parent is that pixel from which this current pixel is generated 29 | self.penalty = penalty 30 | self.pointx = int(pointx) 31 | self.pointy = int(pointy) 32 | self.parent = parent 33 | self.h = h #heuristic 34 | 35 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 57 | return True 58 | else: 59 | return False 60 | 61 | def bfs(arr, sx, sy, dx, dy, final_contours): # sx, sy :- source coordinates dx, dy :- destination coordinates 62 | q = Q.PriorityQueue() 63 | temp1 = True 64 | temp2 = True 65 | 66 | for cnt in final_contours: 67 | if cv2.pointPolygonTest(cnt, (sx, sy), False) > -1: 68 | temp1 = False 69 | 70 | for cnt in final_contours: 71 | if cv2.pointPolygonTest(cnt, (dx, dy), False) > -1: 72 | temp2 = False 73 | 74 | if temp1 == False or temp2 == False: 75 | return [] 76 | 77 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 78 | solution = [] 79 | ex, ey, ez = arr.shape 80 | #visit = [[False for x in range(ey)] for x in range(ex)] 81 | dist = [[10000 for x in range(ey)] for x in range(ex)] 82 | distplusHeuristic = [[10000 for x in range(ey)] for x in range(ex)] 83 | 84 | q.put(pixel1(0, sx, sy, None, heuristic(sx, sy, dx, dy))) 85 | dist[sx][sy] = 0 86 | distplusHeuristic[sx][sy] = dist[sx][sy]+heuristic(sx, sy, dx, dy) 87 | s = time.clock() 88 | cnt = 0 89 | cntq = 0 90 | while not q.empty(): 91 | p = q.get() 92 | x = int(p.pointx) 93 | y = int(p.pointy) 94 | pen = p.penalty 95 | h = p.h 96 | cnt = cnt+1 97 | if dist[x][y] < pen: 98 | continue 99 | if x == dx and y == dy: 100 | while p is not None: 101 | solution.append([p.pointx, p.pointy]) 102 | p = p.parent 103 | #print 'time : ', time.clock()-s 104 | #print cnt, cntq 105 | return solution 106 | 107 | for i in range(len(actions)): 108 | nx = int(actions[i][0] + x) 109 | ny = int(actions[i][1] + y) 110 | if check_boundaries1(ex, ey, nx, ny) == True: 111 | #if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 112 | pen = dist[x][y] 113 | pen_new = cost(x, y, nx, ny, pen, arr[nx][ny][0]) 114 | h_new = heuristic(nx, ny, dx, dy) 115 | if dist[nx][ny] > pen_new : 116 | dist[nx][ny] = pen_new 117 | nx = int(nx) 118 | ny = int(ny) 119 | if distplusHeuristic[nx][ny] > dist[nx][ny]+h_new : 120 | distplusHeuristic[nx][ny] = dist[nx][ny] + h_new 121 | cntq = cntq+1 122 | q.put(pixel1(pen_new, nx, ny, p, h_new)) 123 | #print 'time : ', time.clock()-s 124 | return [] 125 | 126 | ''' 127 | function definition from Clearance-feasibility 128 | end 129 | ''' 130 | 131 | ''' 132 | function definition from Clearance-feasibility 133 | start 134 | ''' 135 | 136 | class pixel(object): 137 | def __init__(self, penalty, pointx, pointy): # parent is that pixel from which this current pixel is generated 138 | self.penalty = penalty 139 | self.pointx = int(pointx) 140 | self.pointy = int(pointy) 141 | 142 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 152 | return True 153 | else: 154 | return False 155 | 156 | def fill_clearance(arr,cmax, final_contours): # sx, sy :- source coordinates dx, dy :- destination coordinates 157 | q = Q.PriorityQueue() 158 | 159 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 160 | ex, ey, ez = arr.shape 161 | #print ex, ey, ez 162 | min_cost = [[100000 for x in range(ey)] for x in range(ex)] 163 | for cnt in final_contours: 164 | for pts in cnt: 165 | q.put(pixel(0, pts[0, 1], pts[0, 0])) 166 | cnt = 0 167 | cntq = 0 168 | while not q.empty(): 169 | p = q.get() 170 | x = int(p.pointx) 171 | y = int(p.pointy) 172 | pen = p.penalty 173 | if p.penalty > cmax: 174 | continue 175 | if min_cost[x][y] <= p.penalty: 176 | continue 177 | min_cost[x][y] = p.penalty 178 | 179 | for i in range(len(actions)): 180 | nx = int(actions[i][0] + x) 181 | ny = int(actions[i][1] + y) 182 | if check_boundaries(ex, ey, nx, ny) == True: 183 | if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 184 | if min_cost[nx][ny] > penalty(x, y, nx, ny, pen): 185 | q.put(pixel(penalty(x,y,nx,ny,pen), nx, ny)) 186 | return min_cost 187 | ''' 188 | function definition from Clearance-feasibility 189 | end 190 | ''' 191 | 192 | 193 | 194 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 195 | if arr[ansx][ansy][0] == 255: 196 | return True 197 | else: 198 | return False 199 | 200 | def feasible(arr, x, y): #function to check if a point is feasible or not 201 | ex, ey, ez = arr.shape 202 | x = int(x) 203 | y = int(y) 204 | 205 | if check_boundaries(ex, ey, x, y): 206 | return not check_obstacles(arr, x, y) 207 | else: 208 | return False 209 | 210 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 211 | ansx = sx 212 | ansy = sy 213 | flag = True 214 | count = 1 215 | while True: 216 | if count > q_star: 217 | return (-1, -1) 218 | ansx = sx + count*math.sin(theta) 219 | ansy = sy + count*math.cos(theta) 220 | 221 | if check_boundaries(x, y, ansx, ansy) == False: 222 | break 223 | else: 224 | if check_obstacles(arr, ansx, ansy) == True: 225 | break 226 | count += 1 227 | 228 | return (ansx-sx,ansy- sy) 229 | 230 | 231 | def obstacle_force(arr, sx, sy, q_star): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 232 | forcex = 0 233 | forcey = 0 234 | neta = 3000 235 | x, y , z= arr.shape 236 | for i in range(8): 237 | (ox,oy) = dist(sx, sy, x, y, i*math.pi/4, arr, q_star) 238 | theta = i*math.pi/4 239 | ox = math.fabs(ox) 240 | oy = math.fabs(oy) 241 | 242 | d = math.hypot(ox,oy) 243 | fx = 0 244 | fy = 0 245 | if ox == -1 or oy == -1: 246 | fx = 0 247 | fy = 0 248 | else: 249 | if d == 0: 250 | d = 1 251 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 252 | fx = f*math.sin(theta) 253 | fy = f*math.cos(theta) 254 | 255 | forcex += fx 256 | forcey += fy 257 | 258 | return (forcex, forcey) 259 | 260 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 261 | forcex = 0 262 | forcey = 0 263 | tau = 1 #constant 264 | #printx('10') 265 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 266 | if d > d_star: 267 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 268 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 269 | 270 | else: 271 | forcex += ((dx-sx)*tau) 272 | forcey += ((dy-sy)*tau) 273 | 274 | #printx('11') 275 | return (forcex, forcey) 276 | 277 | 278 | def path_planning(arr, sx1, sy1, dx, dy, theta): 279 | ''' 280 | 281 | :param arr: input map 282 | :param sx1: source x 283 | :param sy1: source y 284 | :param dx: destination x 285 | :param dy: destination y 286 | :return: path 287 | ''' 288 | 289 | #Parameters Declaration 290 | 291 | flx = 10000 #maximum total force in x 292 | fly = 10000 #maximum total force in y 293 | v = 4 #velocity magnitude 294 | t = 1 #time lapse 295 | #theta = 0 #initial angle 296 | x,y,z = arr.shape 297 | theta_const = math.pi*60/180 #maximum allowed turn angle 298 | q_star = 50000 299 | d_star = 20000 300 | 301 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 302 | return [] 303 | sx = sx1 304 | sy = sy1 305 | 306 | sol = [] 307 | sol.append((sx, sy)) 308 | 309 | 310 | sx += int(v*math.sin(theta)) 311 | sy += int(v*math.cos(theta)) 312 | sol.append((sx, sy)) 313 | 314 | ''' 315 | if Q and P are two vectors and @ is angle between them 316 | 317 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 318 | 319 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 320 | ''' 321 | 322 | #count = 0 323 | while True: 324 | #count += 1 325 | (fx, fy) = obstacle_force(arr, sx, sy, q_star) 326 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 327 | 328 | tx = gx+fx 329 | ty = gy+fy 330 | if(tx < 0): 331 | tx = max(tx, -flx) 332 | else: 333 | tx = min(tx, flx) 334 | if(ty < 0): 335 | ty = max(ty, -fly) 336 | else: 337 | ty = min(ty, fly) 338 | theta1 = math.atan2(tx, ty) 339 | 340 | if arr[sx][sy][0] == 255: 341 | print gx, gy, fx, fy 342 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 343 | print theta1*180/math.pi, theta*180/math.pi 344 | sleep(10) 345 | 346 | P = v 347 | angle = theta1-theta #angle between velocity and force vector 348 | 349 | Q = math.sqrt(tx*tx + ty*ty) 350 | 351 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 352 | 353 | if theta2 < 0: 354 | theta2 = max(theta2, -theta_const) 355 | else: 356 | theta2 = min(theta2, theta_const) 357 | 358 | theta += theta2 359 | 360 | theta = (theta + 2*math.pi)%(2*math.pi) 361 | 362 | sx = sx + v*math.sin(theta) 363 | sy = sy + v*math.cos(theta) 364 | sx = int(sx) 365 | sy = int(sy) 366 | 367 | if not check_boundaries(x, y, sx, sy): 368 | print 'out of boundaries' , sx, sy 369 | return sol 370 | 371 | sol.append((sx, sy)) 372 | 373 | if sx < dx+ 2 and sx > dx - 2 and sy < dy+2 and sy > dy-2: 374 | break 375 | 376 | return sol 377 | 378 | 379 | def final_path(sol, arr): 380 | l = len(sol) 381 | print l 382 | div = 250 383 | 384 | start = 0 385 | end = div 386 | solution = [] 387 | theta = 0 388 | 389 | while start < l-1: 390 | print sol[start][0], sol[start][1], sol[end][0], sol[end][1] 391 | ret = path_planning(arr, sol[start][0], sol[start][1], sol[end][0], sol[end][1], theta) 392 | for i in ret: 393 | solution.append(i) 394 | l1 = len(ret) 395 | if l1 > 2: 396 | x1 = ret[l1-1][0] 397 | x2 = ret[l1-2][0] 398 | y1 = ret[l1-1][1] 399 | y2 = ret[l1-2][1] 400 | theta = math.atan2(x1-x2, y1-y2) 401 | start = end 402 | end += div 403 | if end > l-1: 404 | end = l-1 405 | 406 | return solution 407 | 408 | 409 | def main(): 410 | counter = 1 411 | for im in images: 412 | 413 | img = cv2.imread(im) 414 | 415 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 416 | img2 = cv2.medianBlur(cimg,13) 417 | 418 | ret,thresh1 = cv2.threshold(cimg,100,120,cv2.THRESH_BINARY) 419 | t2 = copy.copy(thresh1) 420 | 421 | x, y = thresh1.shape 422 | arr = np.zeros((x, y, 3), np.uint8) 423 | arr1 = np.zeros((x, y, 3), np.uint8) 424 | final_contours= [] 425 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 426 | for i in range(len(contours)): 427 | cnt = contours[i] 428 | if cv2.contourArea(cnt) > 1000 and cv2.contourArea(cnt) < 15000 : 429 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 430 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 431 | cv2.fillConvexPoly(arr1, cnt, [255, 255, 255]) 432 | final_contours.append(cnt) 433 | cmax = 50 434 | start = time.clock() 435 | min_cost = fill_clearance(arr,cmax, final_contours) 436 | print 'time: ', time.clock()-start 437 | ''' 438 | for i in xrange(x): 439 | for j in xrange(y): 440 | if min_cost[i][j] == 100000: 441 | min_cost[i][j] = 0; 442 | ''' 443 | 444 | for i in xrange(x): 445 | for j in xrange(y): 446 | pix_val = int(5*min_cost[i][j]) 447 | if(min_cost[i][j] > 10000): 448 | pix_val = 255 449 | arr[i, j] = (pix_val, pix_val, pix_val) 450 | for cnt in final_contours: 451 | cv2.fillConvexPoly(arr, cnt, [0, 0, 0]) 452 | 453 | ''' 454 | Code from A-star.py 455 | ''' 456 | sx = 20 # raw_input("Enter source and destination Coordinates") 457 | sy = 20 # raw_input() 458 | dx = 500 # raw_input() 459 | dy = 1000 # raw_input() 460 | 461 | sol = bfs(arr, sx, sy, dx, dy, final_contours) 462 | solution = final_path(sol, arr1) 463 | 464 | if len(solution) == 0: 465 | print 'No solution from source to destination' 466 | else: 467 | for i in range(len(solution)): 468 | start = (solution[i][1], solution[i][0]) 469 | cv2.circle(arr,start, 1, [255, 255, 255]) 470 | cv2.circle(img, start, 1, [255, 255, 255]) 471 | 472 | for i in range(len(sol)): 473 | start = (sol[i][1], sol[i][0]) 474 | cv2.circle(arr,start, 1, [255, 0, 0]) 475 | cv2.circle(img, start, 1, [255, 0, 0]) 476 | 477 | cv2.circle(arr, (sy, sx), 2, [0, 255, 0]) 478 | cv2.circle(arr, (dy, dx), 2, [0, 255, 0]) 479 | cv2.circle(img, (sy, sx), 2, [0, 255, 0]) 480 | cv2.circle(img, (dy, dx), 2, [0, 255, 0]) 481 | output = "output1/"+`counter` 482 | output += ".jpg" 483 | cv2.imwrite(output, img) 484 | counter += 1 485 | cv2.imshow('image', img) 486 | cv2.imshow('arr', arr) 487 | cv2.waitKey(0) 488 | cv2.destroyAllWindows() 489 | main() -------------------------------------------------------------------------------- /A-star-potential-field-hybrid-type-1_1.py: -------------------------------------------------------------------------------- 1 | ''' 2 | In this method , we are trying to find offline path first using A* algorithm, then we use Artificial potential method 3 | to reach local goals on A* planned path. 4 | Suppose, a1, a2, a3, ....... an be path we get from A*, then to potential field we give following as consecutive goals: 5 | a(i), a(i+4), a(i+8), a(i+12), ..... a(i+n) 6 | 7 | Hence this way we are using power of both A-star and potential field :) 8 | ''' 9 | 10 | import cv2 11 | import numpy as np 12 | from time import sleep 13 | import copy 14 | import glob 15 | import math 16 | import time 17 | import Queue as Q 18 | 19 | 20 | def printx(x): 21 | #print x 22 | pass 23 | ''' 24 | function definition from A-star 25 | start 26 | ''' 27 | class pixel1(object): 28 | def __init__(self, penalty, pointx, pointy, parent, h): # parent is that pixel from which this current pixel is generated 29 | self.penalty = penalty 30 | self.pointx = int(pointx) 31 | self.pointy = int(pointy) 32 | self.parent = parent 33 | self.h = h #heuristic 34 | 35 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 57 | return True 58 | else: 59 | return False 60 | 61 | def bfs(arr, sx, sy, dx, dy, final_contours): # sx, sy :- source coordinates dx, dy :- destination coordinates 62 | q = Q.PriorityQueue() 63 | temp1 = True 64 | temp2 = True 65 | 66 | for cnt in final_contours: 67 | if cv2.pointPolygonTest(cnt, (sx, sy), False) > -1: 68 | temp1 = False 69 | 70 | for cnt in final_contours: 71 | if cv2.pointPolygonTest(cnt, (dx, dy), False) > -1: 72 | temp2 = False 73 | 74 | if temp1 == False or temp2 == False: 75 | return [] 76 | 77 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 78 | solution = [] 79 | ex, ey, ez = arr.shape 80 | #visit = [[False for x in range(ey)] for x in range(ex)] 81 | dist = [[10000 for x in range(ey)] for x in range(ex)] 82 | distplusHeuristic = [[10000 for x in range(ey)] for x in range(ex)] 83 | 84 | q.put(pixel1(0, sx, sy, None, heuristic(sx, sy, dx, dy))) 85 | dist[sx][sy] = 0 86 | distplusHeuristic[sx][sy] = dist[sx][sy]+heuristic(sx, sy, dx, dy) 87 | s = time.clock() 88 | cnt = 0 89 | cntq = 0 90 | while not q.empty(): 91 | p = q.get() 92 | x = int(p.pointx) 93 | y = int(p.pointy) 94 | pen = p.penalty 95 | h = p.h 96 | cnt = cnt+1 97 | if dist[x][y] < pen: 98 | continue 99 | if x == dx and y == dy: 100 | while p is not None: 101 | solution.append([p.pointx, p.pointy]) 102 | p = p.parent 103 | #print 'time : ', time.clock()-s 104 | #print cnt, cntq 105 | return solution 106 | 107 | for i in range(len(actions)): 108 | nx = int(actions[i][0] + x) 109 | ny = int(actions[i][1] + y) 110 | if check_boundaries1(ex, ey, nx, ny) == True: 111 | #if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 112 | pen = dist[x][y] 113 | pen_new = cost(x, y, nx, ny, pen, arr[nx][ny][0]) 114 | h_new = heuristic(nx, ny, dx, dy) 115 | if dist[nx][ny] > pen_new : 116 | dist[nx][ny] = pen_new 117 | nx = int(nx) 118 | ny = int(ny) 119 | if distplusHeuristic[nx][ny] > dist[nx][ny]+h_new : 120 | distplusHeuristic[nx][ny] = dist[nx][ny] + h_new 121 | cntq = cntq+1 122 | q.put(pixel1(pen_new, nx, ny, p, h_new)) 123 | #print 'time : ', time.clock()-s 124 | return [] 125 | 126 | ''' 127 | function definition from A-star 128 | end 129 | ''' 130 | 131 | ''' 132 | function definition from Clearance-feasibility 133 | start 134 | ''' 135 | 136 | class pixel(object): 137 | def __init__(self, penalty, pointx, pointy): # parent is that pixel from which this current pixel is generated 138 | self.penalty = penalty 139 | self.pointx = int(pointx) 140 | self.pointy = int(pointy) 141 | 142 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 152 | return True 153 | else: 154 | return False 155 | 156 | def fill_clearance(arr,cmax, final_contours): # sx, sy :- source coordinates dx, dy :- destination coordinates 157 | q = Q.PriorityQueue() 158 | 159 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 160 | ex, ey, ez = arr.shape 161 | #print ex, ey, ez 162 | min_cost = [[100000 for x in range(ey)] for x in range(ex)] 163 | for cnt in final_contours: 164 | for pts in cnt: 165 | q.put(pixel(0, pts[0, 1], pts[0, 0])) 166 | cnt = 0 167 | cntq = 0 168 | while not q.empty(): 169 | p = q.get() 170 | x = int(p.pointx) 171 | y = int(p.pointy) 172 | pen = p.penalty 173 | if p.penalty > cmax: 174 | continue 175 | if min_cost[x][y] <= p.penalty: 176 | continue 177 | min_cost[x][y] = p.penalty 178 | 179 | for i in range(len(actions)): 180 | nx = int(actions[i][0] + x) 181 | ny = int(actions[i][1] + y) 182 | if check_boundaries(ex, ey, nx, ny) == True: 183 | if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 184 | if min_cost[nx][ny] > penalty(x, y, nx, ny, pen): 185 | q.put(pixel(penalty(x,y,nx,ny,pen), nx, ny)) 186 | return min_cost 187 | ''' 188 | function definition from Clearance-feasibility 189 | end 190 | ''' 191 | 192 | ''' 193 | function definition from Artificial Potential potential starts 194 | ''' 195 | 196 | 197 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 198 | if arr[ansx][ansy][0] == 255: 199 | return True 200 | else: 201 | return False 202 | 203 | def feasible(arr, x, y): #function to check if a point is feasible or not 204 | ex, ey, ez = arr.shape 205 | x = int(x) 206 | y = int(y) 207 | 208 | if check_boundaries(ex, ey, x, y): 209 | return not check_obstacles(arr, x, y) 210 | else: 211 | return False 212 | 213 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 214 | ansx = sx 215 | ansy = sy 216 | flag = True 217 | count = 1 218 | while True: 219 | if count > q_star: 220 | return (-1, -1) 221 | ansx = sx + count*math.sin(theta) 222 | ansy = sy + count*math.cos(theta) 223 | 224 | if check_boundaries(x, y, ansx, ansy) == False: 225 | break 226 | else: 227 | if check_obstacles(arr, ansx, ansy) == True: 228 | break 229 | count += 1 230 | 231 | 232 | return (ansx-sx,ansy- sy) 233 | 234 | def obstacle_force(arr, sx, sy, q_star, theta1): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 235 | forcex = 0 236 | forcey = 0 237 | neta = 30000000000000 238 | x, y , z= arr.shape 239 | for i in range(-8, 9): 240 | (ox,oy) = dist(sx, sy, x, y, (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi), arr, q_star) 241 | theta = (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi) 242 | fx = 0 243 | fy = 0 244 | #print 'ox ', ox, 'oy ', oy 245 | if ox == -1 or oy == -1: 246 | fx = 0 247 | fy = 0 248 | else: 249 | ox = math.fabs(ox) 250 | oy = math.fabs(oy) 251 | d = math.hypot(ox, oy) 252 | if d == 0: 253 | d = 1 254 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 255 | fx = f*math.sin(theta) 256 | fy = f*math.cos(theta) 257 | 258 | forcex += fx 259 | forcey += fy 260 | thet = math.atan2(forcex, forcey) 261 | arr1 = arr 262 | cv2.line(arr1, (sy, sx), (int(sy + 10*math.cos(thet)), int(sx + math.sin(thet))), (0, 255, 255), 1) 263 | cv2.imshow('arr', arr1) 264 | k = cv2.waitKey(20) 265 | return (forcex, forcey) 266 | 267 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 268 | forcex = 0 269 | forcey = 0 270 | tau = 100000000 #constant 271 | printx('10') 272 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 273 | if d > d_star: 274 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 275 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 276 | 277 | else: 278 | forcex += ((dx-sx)*tau) 279 | forcey += ((dy-sy)*tau) 280 | 281 | printx('11') 282 | return (forcex, forcey) 283 | 284 | def path_planning(arr, sx1, sy1, dx, dy, theta): 285 | ''' 286 | 287 | :param arr: input map 288 | :param sx1: source x 289 | :param sy1: source y 290 | :param dx: destination x 291 | :param dy: destination y 292 | :return: path 293 | ''' 294 | 295 | #Parameters Declaration 296 | 297 | flx = 10000 #maximum total force in x 298 | fly = 10000 #maximum total force in y 299 | v = 1 #velocity magnitude 300 | t = 1 #time lapse 301 | #theta = 0 #initial angle 302 | x,y,z = arr.shape 303 | theta_const = math.pi*30/180 #maximum allowed turn angle 304 | q_star = 10 305 | d_star = 2 306 | 307 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 308 | return [] 309 | sx = sx1 310 | sy = sy1 311 | 312 | sol = [] 313 | sol.append((sx, sy)) 314 | 315 | 316 | sx += int(v*math.sin(theta)) 317 | sy += int(v*math.cos(theta)) 318 | sol.append((sx, sy)) 319 | 320 | ''' 321 | if Q and P are two vectors and @ is angle between them 322 | 323 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 324 | 325 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 326 | ''' 327 | 328 | #count = 0 329 | while True: 330 | #count += 1 331 | (fx, fy) = obstacle_force(arr, sx, sy, q_star, theta) 332 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 333 | 334 | tx = gx+fx 335 | ty = gy+fy 336 | if(tx < 0): 337 | tx = max(tx, -flx) 338 | else: 339 | tx = min(tx, flx) 340 | if(ty < 0): 341 | ty = max(ty, -fly) 342 | else: 343 | ty = min(ty, fly) 344 | theta1 = math.atan2(tx, ty) 345 | 346 | if arr[sx][sy][0] == 255: 347 | print gx, gy, fx, fy 348 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 349 | print theta1*180/math.pi, theta*180/math.pi 350 | sleep(10) 351 | 352 | P = v 353 | angle = theta1-theta #angle between velocity and force vector 354 | 355 | Q = math.sqrt(tx*tx + ty*ty) 356 | 357 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 358 | 359 | if theta2 < 0: 360 | theta2 = max(theta2, -theta_const) 361 | else: 362 | theta2 = min(theta2, theta_const) 363 | 364 | theta += theta2 365 | 366 | theta = (theta + 2*math.pi)%(2*math.pi) 367 | 368 | sx = sx + v*math.sin(theta) 369 | sy = sy + v*math.cos(theta) 370 | sx = int(sx) 371 | sy = int(sy) 372 | 373 | if not check_boundaries(x, y, sx, sy): 374 | print 'out of boundaries' , sx, sy 375 | return sol 376 | 377 | sol.append((sx, sy)) 378 | 379 | if sx < dx+ 2 and sx > dx - 2 and sy < dy+2 and sy > dy-2: 380 | break 381 | sol.append((theta)) 382 | return sol 383 | 384 | ''' 385 | function definition from Artificial Potential potential starts 386 | ''' 387 | 388 | def print_path_to_file(sol): 389 | with open('path.txt', 'w') as f: 390 | for i in range(len(sol)): 391 | f.write(`sol[i][0]` + ' ' + `sol[i][1]` + '\n') 392 | print sol[i][0] 393 | 394 | def read_path_from_file(): 395 | sol = [] 396 | with open('path.txt', 'r') as f: 397 | data = f.readlines() 398 | 399 | for line in data: 400 | s = [] 401 | words = line.split() 402 | for j in words: 403 | s.append(int(j)) 404 | sol.append(s) 405 | 406 | return sol 407 | 408 | def final_path(sol, arr): 409 | l = len(sol) 410 | print l 411 | div = 150 412 | arr1 = copy.copy(arr) 413 | 414 | start = 0 415 | end = div 416 | solution = [] 417 | theta = 0 418 | 419 | while start < l-1: 420 | print sol[start][0], sol[start][1], sol[end][0], sol[end][1] 421 | ret = path_planning(arr, sol[start][0], sol[start][1], sol[end][0], sol[end][1], theta) 422 | for i in range(len(ret)-1): 423 | solution.append(ret[i]) 424 | l1 = len(ret)-1 425 | theta= ret[-1] 426 | ''' 427 | if l1 > 2: 428 | x1 = ret[l1-1][0] 429 | x2 = ret[l1-2][0] 430 | y1 = ret[l1-1][1] 431 | y2 = ret[l1-2][1] 432 | theta = math.atan2(x1-x2, y1-y2) 433 | cv2.line(arr1, (int(y1), int(x1)), (int(y2), int(x2)), (255, 0, 0), 1) 434 | cv2.imshow('img', arr1) 435 | k = cv2.waitKey(0) 436 | ''' 437 | start = end 438 | end += div 439 | if end > l-1: 440 | end = l-1 441 | 442 | for i in solution: 443 | print i 444 | return solution 445 | 446 | def main(): 447 | counter = 1 448 | for im in images: 449 | 450 | img = cv2.imread(im) 451 | 452 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 453 | img2 = cv2.medianBlur(cimg,13) 454 | 455 | ret,thresh1 = cv2.threshold(cimg,100,120,cv2.THRESH_BINARY) 456 | t2 = copy.copy(thresh1) 457 | 458 | x, y = thresh1.shape 459 | arr = np.zeros((x, y, 3), np.uint8) 460 | arr1 = np.zeros((x, y, 3), np.uint8) 461 | final_contours= [] 462 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 463 | for i in range(len(contours)): 464 | cnt = contours[i] 465 | if cv2.contourArea(cnt) > 1000 and cv2.contourArea(cnt) < 15000 : 466 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 467 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 468 | cv2.fillConvexPoly(arr1, cnt, [255, 255, 255]) 469 | final_contours.append(cnt) 470 | cmax = 50 471 | start = time.clock() 472 | min_cost = fill_clearance(arr,cmax, final_contours) 473 | print 'time: ', time.clock()-start 474 | ''' 475 | for i in xrange(x): 476 | for j in xrange(y): 477 | if min_cost[i][j] == 100000: 478 | min_cost[i][j] = 0; 479 | ''' 480 | 481 | for i in xrange(x): 482 | for j in xrange(y): 483 | pix_val = int(5*min_cost[i][j]) 484 | if(min_cost[i][j] > 10000): 485 | pix_val = 255 486 | arr[i, j] = (pix_val, pix_val, pix_val) 487 | for cnt in final_contours: 488 | cv2.fillConvexPoly(arr, cnt, [0, 0, 0]) 489 | 490 | ''' 491 | Code from A-star.py 492 | ''' 493 | sx = 50 # raw_input("Enter source and destination Coordinates") 494 | sy = 50 # raw_input() 495 | dx = 450 # raw_input() 496 | dy = 900 # raw_input() 497 | 498 | sol = bfs(arr, sx, sy, dx, dy, final_contours) 499 | #print_path_to_file(sol) 500 | 501 | #sol = read_path_from_file() 502 | #print sol 503 | solution = final_path(sol, arr1) 504 | #print solution 505 | 506 | if len(solution) == 0: 507 | print 'No solution from source to destination' 508 | else: 509 | for i in range(len(solution)): 510 | start = (solution[i][1], solution[i][0]) 511 | cv2.circle(arr,start, 1, [255, 255, 255]) 512 | cv2.circle(img, start, 1, [255, 255, 255]) 513 | 514 | for i in range(len(sol)): 515 | start = (sol[i][1], sol[i][0]) 516 | cv2.circle(arr,start, 1, [255, 0, 0]) 517 | cv2.circle(img, start, 1, [255, 0, 0]) 518 | 519 | cv2.circle(arr, (sy, sx), 2, [0, 255, 0]) 520 | cv2.circle(arr, (dy, dx), 2, [0, 255, 0]) 521 | cv2.circle(img, (sy, sx), 2, [0, 255, 0]) 522 | cv2.circle(img, (dy, dx), 2, [0, 255, 0]) 523 | output = "output1/"+`counter` 524 | output += ".jpg" 525 | cv2.imwrite(output, img) 526 | counter += 1 527 | cv2.imshow('image', img) 528 | cv2.imshow('arr', arr) 529 | cv2.waitKey(0) 530 | cv2.destroyAllWindows() 531 | main() -------------------------------------------------------------------------------- /A-star-potential-field-hybrid-type-1-controller.py: -------------------------------------------------------------------------------- 1 | ''' 2 | In this method , we are trying to find offline path first using A* algorithm, then we use Artificial potential method 3 | to reach local goals on A* planned path. 4 | Suppose, a1, a2, a3, ....... an be path we get from A*, then to potential field we give following as consecutive goals: 5 | a(i), a(i+4), a(i+8), a(i+12), ..... a(i+n) 6 | 7 | Hence this way we are using power of both A-star and potential field :) 8 | ''' 9 | 10 | import cv2 11 | import numpy as np 12 | from time import sleep 13 | import copy 14 | import glob 15 | import math 16 | import time 17 | import Queue as Q 18 | 19 | 20 | def printx(x): 21 | #print x 22 | pass 23 | ''' 24 | function definition from A-star 25 | start 26 | ''' 27 | class pixel1(object): 28 | def __init__(self, penalty, pointx, pointy, parent, h): # parent is that pixel from which this current pixel is generated 29 | self.penalty = penalty 30 | self.pointx = int(pointx) 31 | self.pointy = int(pointy) 32 | self.parent = parent 33 | self.h = h #heuristic 34 | 35 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 57 | return True 58 | else: 59 | return False 60 | 61 | def bfs(arr, sx, sy, dx, dy, final_contours): # sx, sy :- source coordinates dx, dy :- destination coordinates 62 | q = Q.PriorityQueue() 63 | temp1 = True 64 | temp2 = True 65 | 66 | for cnt in final_contours: 67 | if cv2.pointPolygonTest(cnt, (sx, sy), False) > -1: 68 | temp1 = False 69 | 70 | for cnt in final_contours: 71 | if cv2.pointPolygonTest(cnt, (dx, dy), False) > -1: 72 | temp2 = False 73 | 74 | if temp1 == False or temp2 == False: 75 | return [] 76 | 77 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 78 | solution = [] 79 | ex, ey, ez = arr.shape 80 | #visit = [[False for x in range(ey)] for x in range(ex)] 81 | dist = [[10000 for x in range(ey)] for x in range(ex)] 82 | distplusHeuristic = [[10000 for x in range(ey)] for x in range(ex)] 83 | 84 | q.put(pixel1(0, sx, sy, None, heuristic(sx, sy, dx, dy))) 85 | dist[sx][sy] = 0 86 | distplusHeuristic[sx][sy] = dist[sx][sy]+heuristic(sx, sy, dx, dy) 87 | s = time.clock() 88 | cnt = 0 89 | cntq = 0 90 | while not q.empty(): 91 | p = q.get() 92 | x = int(p.pointx) 93 | y = int(p.pointy) 94 | pen = p.penalty 95 | h = p.h 96 | cnt = cnt+1 97 | if dist[x][y] < pen: 98 | continue 99 | if x == dx and y == dy: 100 | while p is not None: 101 | solution.append([p.pointx, p.pointy]) 102 | p = p.parent 103 | #print 'time : ', time.clock()-s 104 | #print cnt, cntq 105 | return solution 106 | 107 | for i in range(len(actions)): 108 | nx = int(actions[i][0] + x) 109 | ny = int(actions[i][1] + y) 110 | if check_boundaries1(ex, ey, nx, ny) == True: 111 | #if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 112 | pen = dist[x][y] 113 | pen_new = cost(x, y, nx, ny, pen, arr[nx][ny][0]) 114 | h_new = heuristic(nx, ny, dx, dy) 115 | if dist[nx][ny] > pen_new : 116 | dist[nx][ny] = pen_new 117 | nx = int(nx) 118 | ny = int(ny) 119 | if distplusHeuristic[nx][ny] > dist[nx][ny]+h_new : 120 | distplusHeuristic[nx][ny] = dist[nx][ny] + h_new 121 | cntq = cntq+1 122 | q.put(pixel1(pen_new, nx, ny, p, h_new)) 123 | #print 'time : ', time.clock()-s 124 | return [] 125 | 126 | ''' 127 | function definition from Clearance-feasibility 128 | end 129 | ''' 130 | 131 | ''' 132 | function definition from Clearance-feasibility 133 | start 134 | ''' 135 | 136 | class pixel(object): 137 | def __init__(self, penalty, pointx, pointy): # parent is that pixel from which this current pixel is generated 138 | self.penalty = penalty 139 | self.pointx = int(pointx) 140 | self.pointy = int(pointy) 141 | 142 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 152 | return True 153 | else: 154 | return False 155 | 156 | def fill_clearance(arr,cmax, final_contours): # sx, sy :- source coordinates dx, dy :- destination coordinates 157 | q = Q.PriorityQueue() 158 | 159 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 160 | ex, ey, ez = arr.shape 161 | #print ex, ey, ez 162 | min_cost = [[100000 for x in range(ey)] for x in range(ex)] 163 | for cnt in final_contours: 164 | for pts in cnt: 165 | q.put(pixel(0, pts[0, 1], pts[0, 0])) 166 | cnt = 0 167 | cntq = 0 168 | while not q.empty(): 169 | p = q.get() 170 | x = int(p.pointx) 171 | y = int(p.pointy) 172 | pen = p.penalty 173 | if p.penalty > cmax: 174 | continue 175 | if min_cost[x][y] <= p.penalty: 176 | continue 177 | min_cost[x][y] = p.penalty 178 | 179 | for i in range(len(actions)): 180 | nx = int(actions[i][0] + x) 181 | ny = int(actions[i][1] + y) 182 | if check_boundaries(ex, ey, nx, ny) == True: 183 | if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 184 | if min_cost[nx][ny] > penalty(x, y, nx, ny, pen): 185 | q.put(pixel(penalty(x,y,nx,ny,pen), nx, ny)) 186 | return min_cost 187 | ''' 188 | function definition from Clearance-feasibility 189 | end 190 | ''' 191 | 192 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 193 | if arr[ansx][ansy][0] == 255: 194 | return True 195 | else: 196 | return False 197 | 198 | def feasible(arr, x, y): #function to check if a point is feasible or not 199 | ex, ey, ez = arr.shape 200 | x = int(x) 201 | y = int(y) 202 | 203 | if check_boundaries(ex, ey, x, y): 204 | return not check_obstacles(arr, x, y) 205 | else: 206 | return False 207 | 208 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 209 | ansx = sx 210 | ansy = sy 211 | flag = True 212 | count = 1 213 | while True: 214 | if count > q_star: 215 | return (-1, -1) 216 | ansx = sx + count*math.sin(theta) 217 | ansy = sy + count*math.cos(theta) 218 | 219 | if check_boundaries(x, y, ansx, ansy) == False: 220 | break 221 | else: 222 | if check_obstacles(arr, ansx, ansy) == True: 223 | break 224 | count += 1 225 | 226 | return (ansx-sx,ansy- sy) 227 | 228 | def obstacle_force(arr, sx, sy, q_star, theta1): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 229 | forcex = 0 230 | forcey = 0 231 | neta = 3000000000 232 | x, y , z= arr.shape 233 | for i in range(-8, 9): 234 | (ox,oy) = dist(sx, sy, x, y, (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi), arr, q_star) 235 | theta = (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi) 236 | fx = 0 237 | fy = 0 238 | #print 'ox ', ox, 'oy ', oy 239 | if ox == -1 or oy == -1: 240 | fx = 0 241 | fy = 0 242 | else: 243 | ox = math.fabs(ox) 244 | oy = math.fabs(oy) 245 | d = math.hypot(ox, oy) 246 | if d == 0: 247 | d = 1 248 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 249 | fx = f*math.sin(theta) 250 | fy = f*math.cos(theta) 251 | 252 | forcex += fx 253 | forcey += fy 254 | thet = math.atan2(forcex, forcey) 255 | arr1 = arr 256 | cv2.line(arr1, (sy, sx), (int(sy + 10*math.cos(thet)), int(sx + math.sin(thet))), (0, 255, 255), 1) 257 | cv2.imshow('arr', arr1) 258 | k = cv2.waitKey(20) 259 | return (forcex, forcey) 260 | 261 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 262 | forcex = 0 263 | forcey = 0 264 | tau = 1000000 #constant 265 | printx('10') 266 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 267 | if d > d_star: 268 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 269 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 270 | 271 | else: 272 | forcex += ((dx-sx)*tau) 273 | forcey += ((dy-sy)*tau) 274 | 275 | printx('11') 276 | return (forcex, forcey) 277 | 278 | def path_planning(arr, sx1, sy1, dx, dy, theta): 279 | ''' 280 | 281 | :param arr: input map 282 | :param sx1: source x 283 | :param sy1: source y 284 | :param dx: destination x 285 | :param dy: destination y 286 | :return: path 287 | ''' 288 | 289 | #Parameters Declaration 290 | 291 | flx = 10000 #maximum total force in x 292 | fly = 10000 #maximum total force in y 293 | v = 25 #velocity magnitude 294 | t = 1 #time lapse 295 | #theta = 0 #initial angle 296 | x,y,z = arr.shape 297 | theta_const = math.pi*40/180 #maximum allowed turn angle 298 | q_star = 80 299 | d_star = 2 300 | 301 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 302 | return [] 303 | sx = sx1 304 | sy = sy1 305 | 306 | sol = [] 307 | sol.append((sx, sy)) 308 | 309 | 310 | sx += int(v*math.sin(theta)) 311 | sy += int(v*math.cos(theta)) 312 | sol.append((sx, sy)) 313 | 314 | ''' 315 | if Q and P are two vectors and @ is angle between them 316 | 317 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 318 | 319 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 320 | ''' 321 | 322 | #count = 0 323 | while True: 324 | #count += 1 325 | (fx, fy) = obstacle_force(arr, sx, sy, q_star, theta) 326 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 327 | 328 | tx = gx+fx 329 | ty = gy+fy 330 | if(tx < 0): 331 | tx = max(tx, -flx) 332 | else: 333 | tx = min(tx, flx) 334 | if(ty < 0): 335 | ty = max(ty, -fly) 336 | else: 337 | ty = min(ty, fly) 338 | theta1 = math.atan2(tx, ty) 339 | 340 | if arr[sx][sy][0] == 255: 341 | print gx, gy, fx, fy 342 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 343 | print theta1*180/math.pi, theta*180/math.pi 344 | sleep(10) 345 | 346 | P = v 347 | angle = theta1-theta #angle between velocity and force vector 348 | 349 | Q = math.sqrt(tx*tx + ty*ty) 350 | 351 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 352 | 353 | if theta2 < 0: 354 | theta2 = max(theta2, -theta_const) 355 | else: 356 | theta2 = min(theta2, theta_const) 357 | 358 | theta += theta2 359 | 360 | theta = (theta + 2*math.pi)%(2*math.pi) 361 | 362 | sx = sx + v*math.sin(theta) 363 | sy = sy + v*math.cos(theta) 364 | sx = int(sx) 365 | sy = int(sy) 366 | 367 | if not check_boundaries(x, y, sx, sy): 368 | print 'out of boundaries' , sx, sy 369 | return sol 370 | 371 | sol.append((sx, sy)) 372 | 373 | if sx < dx+ 2 and sx > dx - 2 and sy < dy+2 and sy > dy-2: 374 | break 375 | 376 | return sol 377 | 378 | def final_path(sol, arr): 379 | l = len(sol) 380 | print l 381 | div = 150 382 | 383 | start = 0 384 | end = div 385 | solution = [] 386 | theta = 0 387 | 388 | while start < l-1: 389 | print sol[start][0], sol[start][1], sol[end][0], sol[end][1] 390 | ret = path_planning(arr, sol[start][0], sol[start][1], sol[end][0], sol[end][1], theta) 391 | for i in ret: 392 | solution.append(i) 393 | l1 = len(ret) 394 | if l1 > 2: 395 | x1 = ret[l1-1][0] 396 | x2 = ret[l1-2][0] 397 | y1 = ret[l1-1][1] 398 | y2 = ret[l1-2][1] 399 | theta = math.atan2(x1-x2, y1-y2) 400 | start = end 401 | end += div 402 | if end > l-1: 403 | end = l-1 404 | return solution 405 | 406 | 407 | def show_image(im): 408 | cv2.imshow('image', im) 409 | k = cv2.waitKey(0) 410 | 411 | def find_goal(frame): 412 | # converting to HSV 413 | 414 | hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) 415 | #show_image(hsv) 416 | 417 | lower_blue = np.array([113, 40, 29]) 418 | upper_blue = np.array([123, 180, 255]) 419 | 420 | mask = cv2.inRange(hsv, lower_blue, upper_blue) 421 | #show_image(mask) 422 | result = cv2.bitwise_and(frame, frame, mask=mask) 423 | #show_image(result) 424 | blur = cv2.blur(result, (5, 5)) 425 | 426 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 427 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 428 | 429 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 430 | # th3 = cv2.adaptiveThreshold(bw2,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\ 431 | # cv2.THRESH_BINARY,11,2) 432 | edges = cv2.Canny(th3, 100, 200) 433 | th4 = copy.copy(th3) 434 | 435 | perimeter = 0 436 | j = 0 437 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 438 | # print len(contours) 439 | # if(len(contours) > 5): 440 | # continue 441 | cnt = np.array([]) 442 | for i in range(len(contours)): 443 | if (perimeter < cv2.contourArea(contours[i])): 444 | perimeter = cv2.contourArea(contours[i]) 445 | j = i; 446 | cnt = contours[j] 447 | if (len(cnt) == 0): 448 | return (-1, -1) 449 | cv2.drawContours(frame, cnt, -1, (0, 255, 0), 3) 450 | x = 0 451 | y = 0 452 | #print 'find goal' 453 | #print len(cnt), j 454 | #print cnt 455 | for i in range(len(cnt)): 456 | x = x + cnt[i][0][0] 457 | y = y + cnt[i][0][1] 458 | x = x/len(cnt) 459 | y = y/len(cnt) 460 | #print x, y 461 | x = int(x) 462 | y = int(y) 463 | cv2.circle(frame, (x, y), 5, (255, 0, 255), -1) 464 | 465 | #cv2.imshow('image', frame) 466 | #k = cv2.waitKey(0) 467 | 468 | return (int(x), int(y)) 469 | 470 | def find_robot(im): 471 | hsv = cv2.cvtColor(im, cv2.COLOR_BGR2HSV) 472 | lower = np.array([50, 28, 0]) 473 | upper = np.array([60, 168, 255]) 474 | mask = cv2.inRange(hsv, lower, upper) 475 | result = cv2.bitwise_and(im, im, mask=mask) 476 | blur = cv2.blur(result, (5, 5)) 477 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 478 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 479 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 480 | edges = cv2.Canny(th3, 100, 200) 481 | th4 = copy.copy(th3) 482 | perimeter = 0 483 | j = 0 484 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 485 | cnt = np.array([]) 486 | for i in range(len(contours)): 487 | if (perimeter < cv2.contourArea(contours[i])): 488 | perimeter = cv2.contourArea(contours[i]) 489 | j = i; 490 | cnt = contours[j] 491 | 492 | x = 0 493 | y = 0 494 | for i in range(len(cnt)): 495 | x = x + cnt[i][0][0] 496 | y = y + cnt[i][0][1] 497 | x = x / len(cnt) 498 | y = y / len(cnt) 499 | #print x, y 500 | x = int(x) 501 | y = int(y) 502 | cv2.circle(im, (x, y), 5, (255, 0, 255), 2) 503 | #show_image(im) 504 | return (int(x), int(y)) 505 | 506 | def get_direction(): 507 | direction = 0 508 | return direction 509 | 510 | def classify(img): 511 | cimg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 512 | img2 = cv2.medianBlur(cimg, 13) 513 | 514 | ret, thresh1 = cv2.threshold(cimg, 100, 120, cv2.THRESH_BINARY) 515 | t2 = copy.copy(thresh1) 516 | 517 | x, y = thresh1.shape 518 | arr = np.zeros((x, y, 3), np.uint8) 519 | final_contours = [] 520 | image, contours, hierarchy = cv2.findContours(t2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 521 | #cv2.imshow('image', image) 522 | #k = cv2.waitKey(0) 523 | for i in range(len(contours)): 524 | cnt = contours[i] 525 | if cv2.contourArea(cnt) > 3000 and cv2.contourArea(cnt) < 25000: 526 | cv2.drawContours(img, [cnt], -1, [0, 255, 255]) 527 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 528 | final_contours.append(cnt) 529 | #cv2.imshow('arr', arr) 530 | #k = cv2.waitKey(0) 531 | return final_contours,arr 532 | 533 | def negate(arr): 534 | (x, y, z) = arr.shape 535 | arr1 = np.zeros((x, y, 3), np.uint8) 536 | for i in range(x): 537 | for j in range(y): 538 | if arr[i][j][0] == 255: 539 | arr1[i][j] = [0, 0, 0] 540 | else: 541 | arr1[i][j] = [255, 255, 255] 542 | return arr1 543 | 544 | def getAngle(po, im): 545 | angle = math.atan2(float(po[1][0]-po[0][0]),float(po[1][1]-po[0][1])) 546 | #angle = (angle + 2*math.pi)% (2*math.pi) 547 | return angle 548 | 549 | 550 | def main(): 551 | counter = 1 552 | for im in images: 553 | 554 | img = cv2.imread(im) 555 | start1 = time.clock() 556 | x, y, z = img.shape 557 | final_contours,arr = classify(img) 558 | cmax = 50 559 | #cv2.imshow('arr', arr) 560 | #k = cv2.waitKey(0) 561 | arr1 = copy.copy(arr) 562 | min_cost = fill_clearance(arr,cmax, final_contours) 563 | #print 'time: ', time.clock()-start 564 | 565 | for i in xrange(x): 566 | for j in xrange(y): 567 | pix_val = int(5*min_cost[i][j]) 568 | if(min_cost[i][j] > 10000): 569 | pix_val = 255 570 | arr[i, j] = (pix_val, pix_val, pix_val) 571 | for cnt in final_contours: 572 | cv2.fillConvexPoly(arr, cnt, [0, 0, 0]) 573 | #cv2.imshow('arr', arr) 574 | #k = cv2.waitKey(0) 575 | 576 | ''' 577 | Code from A-star.py 578 | ''' 579 | sx = 20 # raw_input("Enter source and destination Coordinates") 580 | sy = 20 # raw_input() 581 | dx = 500 # raw_input() 582 | dy = 1000 # raw_input() 583 | 584 | print 'time : ', time.clock() - start1 585 | 586 | sol = bfs(arr, sx, sy, dx, dy, final_contours) 587 | for i in range(len(sol)): 588 | start = (sol[i][1], sol[i][0]) 589 | cv2.circle(arr, start, 1, [255, 0, 0]) 590 | cv2.circle(img, start, 1, [255, 0, 0]) 591 | print 'time : ', time.clock()-start1 592 | cv2.imshow('img', img) 593 | k = cv2.waitKey(0) 594 | 595 | solution = final_path(sol, arr1) 596 | 597 | if len(solution) == 0: 598 | print 'No solution from source to destination' 599 | else: 600 | for i in range(len(solution)): 601 | start = (solution[i][1], solution[i][0]) 602 | cv2.circle(arr,start, 1, [255, 255, 255]) 603 | cv2.circle(img, start, 1, [255, 255, 255]) 604 | 605 | for i in range(len(sol)): 606 | start = (sol[i][1], sol[i][0]) 607 | cv2.circle(arr,start, 1, [255, 0, 0]) 608 | cv2.circle(img, start, 1, [255, 0, 0]) 609 | 610 | cv2.circle(arr, (sy, sx), 2, [0, 255, 0]) 611 | cv2.circle(arr, (dy, dx), 2, [0, 255, 0]) 612 | cv2.circle(img, (sy, sx), 2, [0, 255, 0]) 613 | cv2.circle(img, (dy, dx), 2, [0, 255, 0]) 614 | output = "output1/"+`counter` 615 | output += ".jpg" 616 | cv2.imwrite(output, img) 617 | counter += 1 618 | cv2.imshow('image', img) 619 | cv2.imshow('arr', arr) 620 | cv2.waitKey(0) 621 | cv2.destroyAllWindows() 622 | main() -------------------------------------------------------------------------------- /A-star-potetial-field-hybrid-type-2.py: -------------------------------------------------------------------------------- 1 | ''' 2 | In this method ,we are actually trying to find out a mix solution. Potetial field works best when it comes near obstacles, 3 | while on other had A-star works best in reverse case. 4 | So here we will calculate the distance from nearest obstacle and accordingly we will change our method to A-star from Potential Field ad vice versa. 5 | Hence this way we are using power of both A-star and potential field :) 6 | 7 | // merge function need improvement, write now it will not work for large values of switch_d 8 | 9 | ''' 10 | 11 | import cv2 12 | import numpy as np 13 | from time import sleep 14 | import copy 15 | import glob 16 | import math 17 | import time 18 | import Queue as Q 19 | 20 | def printx(x): 21 | #print x 22 | pass 23 | ''' 24 | function definition from A-star 25 | start 26 | ''' 27 | class pixel1(object): 28 | def __init__(self, penalty, pointx, pointy, parent, h): # parent is that pixel from which this current pixel is generated 29 | self.penalty = penalty 30 | self.pointx = int(pointx) 31 | self.pointy = int(pointy) 32 | self.parent = parent 33 | self.h = h #heuristic 34 | 35 | def __cmp__(self, other): # comparable which will return self.penalty -1: 86 | temp1 = False 87 | 88 | for cnt in final_contours: 89 | if cv2.pointPolygonTest(cnt, (dx, dy), False) > -1: 90 | temp2 = False 91 | 92 | if temp1 == False or temp2 == False: 93 | return [] 94 | 95 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 96 | solution = [] 97 | ex, ey, ez = arr.shape 98 | #visit = [[False for x in range(ey)] for x in range(ex)] 99 | dist = [[10000 for x in range(ey)] for x in range(ex)] 100 | distplusHeuristic = [[10000 for x in range(ey)] for x in range(ex)] 101 | 102 | q.put(pixel1(0, sx, sy, None, heuristic(sx, sy, dx, dy))) 103 | dist[sx][sy] = 0 104 | distplusHeuristic[sx][sy] = dist[sx][sy]+heuristic(sx, sy, dx, dy) 105 | s = time.clock() 106 | cnt = 0 107 | cntq = 0 108 | count = 0 109 | while not q.empty(): 110 | count += 1 111 | p = q.get() 112 | x = int(p.pointx) 113 | y = int(p.pointy) 114 | pen = p.penalty 115 | h = p.h 116 | cnt = cnt+1 117 | if dist[x][y] < pen: 118 | continue 119 | if x == dx and y == dy: 120 | while p is not None: 121 | solution.append([p.pointx, p.pointy]) 122 | p = p.parent 123 | print 'time : ', time.clock()-s 124 | print cnt, cntq 125 | return solution 126 | 127 | for i in range(len(actions)): 128 | nx = int(actions[i][0] + x) 129 | ny = int(actions[i][1] + y) 130 | if check_boundaries(ex, ey, nx, ny) == True: 131 | #if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 132 | pen = dist[x][y] 133 | pen_new = cost(x, y, nx, ny, pen, 255-arr[nx][ny][0]) 134 | h_new = heuristic(nx, ny, dx, dy) 135 | if dist[nx][ny] > pen_new : 136 | dist[nx][ny] = pen_new 137 | nx = int(nx) 138 | ny = int(ny) 139 | if distplusHeuristic[nx][ny] > dist[nx][ny]+h_new : 140 | distplusHeuristic[nx][ny] = dist[nx][ny] + h_new 141 | cntq = cntq+1 142 | q.put(pixel1(pen_new, nx, ny, p, h_new)) 143 | print 'time : ', time.clock()-s 144 | return [] 145 | 146 | ''' 147 | function definition from A-star 148 | end 149 | ''' 150 | 151 | ''' 152 | function definition from Clearance-feasibility 153 | start 154 | ''' 155 | 156 | class pixel(object): 157 | def __init__(self, penalty, pointx, pointy): # parent is that pixel from which this current pixel is generated 158 | self.penalty = penalty 159 | self.pointx = int(pointx) 160 | self.pointy = int(pointy) 161 | 162 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 172 | return True 173 | else: 174 | return False 175 | 176 | def fill_clearance(arr,cmax, final_contours): # sx, sy :- source coordinates dx, dy :- destination coordinates 177 | q = Q.PriorityQueue() 178 | 179 | actions = [[0, 1], [0, -1], [1, 0], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]] 180 | ex, ey, ez = arr.shape 181 | #print ex, ey, ez 182 | min_cost = [[100000 for x in range(ey)] for x in range(ex)] 183 | for cnt in final_contours: 184 | for pts in cnt: 185 | q.put(pixel(0, pts[0, 1], pts[0, 0])) 186 | cnt = 0 187 | cntq = 0 188 | while not q.empty(): 189 | p = q.get() 190 | x = int(p.pointx) 191 | y = int(p.pointy) 192 | pen = p.penalty 193 | if p.penalty > cmax: 194 | continue 195 | if min_cost[x][y] <= p.penalty: 196 | continue 197 | min_cost[x][y] = p.penalty 198 | 199 | for i in range(len(actions)): 200 | nx = int(actions[i][0] + x) 201 | ny = int(actions[i][1] + y) 202 | if check_boundaries(ex, ey, nx, ny) == True: 203 | if arr.item(nx, ny, 0) == 0 and arr.item(nx, ny, 1) == 0 and arr.item(nx, ny, 2) == 0: 204 | if min_cost[nx][ny] > penalty(x, y, nx, ny, pen): 205 | q.put(pixel(penalty(x,y,nx,ny,pen), nx, ny)) 206 | return min_cost 207 | 208 | ''' 209 | function definition from Clearance-feasibility 210 | end 211 | ''' 212 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 213 | if arr[ansx][ansy][0] == 255: 214 | return True 215 | else: 216 | return False 217 | 218 | def feasible(arr, x, y): #function to check if a point is feasible or not 219 | ex, ey, ez = arr.shape 220 | x = int(x) 221 | y = int(y) 222 | 223 | if check_boundaries(ex, ey, x, y): 224 | return not check_obstacles(arr, x, y) 225 | else: 226 | return False 227 | 228 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 229 | ansx = sx 230 | ansy = sy 231 | flag = True 232 | count = 1 233 | while True: 234 | if count > q_star: 235 | return (-1, -1) 236 | ansx = sx + count*math.sin(theta) 237 | ansy = sy + count*math.cos(theta) 238 | 239 | if check_boundaries(x, y, ansx, ansy) == False: 240 | break 241 | else: 242 | if check_obstacles(arr, ansx, ansy) == True: 243 | break 244 | count += 1 245 | 246 | 247 | return (ansx-sx,ansy- sy) 248 | 249 | def obstacle_force(arr, sx, sy, q_star, theta1): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 250 | forcex = 0 251 | forcey = 0 252 | neta = 300000000000 253 | x, y , z= arr.shape 254 | for i in range(-8, 9): 255 | (ox,oy) = dist(sx, sy, x, y, (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi), arr, q_star) 256 | theta = (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi) 257 | fx = 0 258 | fy = 0 259 | #print 'ox ', ox, 'oy ', oy 260 | if ox == -1 or oy == -1: 261 | fx = 0 262 | fy = 0 263 | else: 264 | ox = math.fabs(ox) 265 | oy = math.fabs(oy) 266 | d = math.hypot(ox, oy) 267 | if d == 0: 268 | d = 1 269 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 270 | fx = f*math.sin(theta) 271 | fy = f*math.cos(theta) 272 | 273 | forcex += fx 274 | forcey += fy 275 | thet = math.atan2(forcex, forcey) 276 | arr1 = arr 277 | cv2.circle(arr1, (sy, sx), 1, (0, 255, 255),1) 278 | cv2.imshow('Artificial only', arr1) 279 | k = cv2.waitKey(1) 280 | #print 'yes ' 281 | return (forcex, forcey) 282 | 283 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 284 | forcex = 0 285 | forcey = 0 286 | tau = 1000000 #constant 287 | #printx('10') 288 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 289 | if d > d_star: 290 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 291 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 292 | 293 | else: 294 | forcex += ((dx-sx)*tau) 295 | forcey += ((dy-sy)*tau) 296 | 297 | #printx('11') 298 | return (forcex, forcey) 299 | 300 | def path_planning(arr, sx1, sy1, dx, dy, theta, sd): 301 | ex, ey, ez = arr.shape 302 | arr1 = np.zeros((ex, ey)) 303 | #cv2.imshow('img', arr) 304 | #k = cv2.waitKey(0) 305 | #print 'abc' 306 | ''' 307 | 308 | :param arr: input map 309 | :param sx1: source x 310 | :param sy1: source y 311 | :param dx: destination x 312 | :param dy: destination y 313 | :param theta: current angle 314 | :param d: switch distance 315 | :return: path 316 | ''' 317 | 318 | #Parameters Declaration 319 | 320 | flx = 10000 # maximum total force in x 321 | fly = 10000 # maximum total force in y 322 | v = 5 # velocity magnitude 323 | t = 1 # time lapse 324 | # theta = 0 #initial angle 325 | x, y, z = arr.shape 326 | theta_const = math.pi * 30 / 180 # maximum allowed turn angle 327 | q_star = 30 328 | d_star = 2 329 | #print 'xyz' 330 | 331 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 332 | print arr[sx1][sy1], sx1, sy1 333 | print arr[dx][dy], dx, dy 334 | return [] 335 | sx = sx1 336 | sy = sy1 337 | 338 | sol = [] 339 | sol.append((sx, sy)) 340 | 341 | #print 'def' 342 | sx += int(v*math.sin(theta)) 343 | sy += int(v*math.cos(theta)) 344 | sol.append((sx, sy)) 345 | 346 | ''' 347 | if Q and P are two vectors and @ is angle between them 348 | 349 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 350 | 351 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 352 | ''' 353 | count = 0 354 | while True: 355 | count += 1 356 | (fx, fy) = obstacle_force(arr, sx, sy, q_star, theta) 357 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 358 | 359 | tx = gx+fx 360 | ty = gy+fy 361 | if(tx < 0): 362 | tx = max(tx, -flx) 363 | else: 364 | tx = min(tx, flx) 365 | if(ty < 0): 366 | ty = max(ty, -fly) 367 | else: 368 | ty = min(ty, fly) 369 | theta1 = math.atan2(tx, ty) 370 | 371 | #print 'tx' ,tx, 'ty' ,ty 372 | #sleep(1) 373 | if arr[sx][sy][0] == 255: 374 | print gx, gy, fx, fy 375 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 376 | print theta1*180/math.pi, theta*180/math.pi 377 | 378 | 379 | P = v 380 | angle = theta1-theta #angle between velocity and force vector 381 | 382 | Q = math.sqrt(tx*tx + ty*ty) 383 | 384 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 385 | 386 | if theta2 < 0: 387 | theta2 = max(theta2, -theta_const) 388 | else: 389 | theta2 = min(theta2, theta_const) 390 | 391 | theta += theta2 392 | 393 | theta = (theta + 2*math.pi)%(2*math.pi) 394 | 395 | sx = sx + v*math.sin(theta) 396 | sy = sy + v*math.cos(theta) 397 | sx = int(sx) 398 | sy = int(sy) 399 | 400 | if not check_boundaries(x, y, sx, sy): 401 | print 'out of boundaries' , sx, sy 402 | return sol 403 | 404 | sol.append((sx, sy)) 405 | if sx < dx+ 2 and sx > dx - 2 and sy < dy+2 and sy > dy-2: 406 | print 'abc' 407 | break 408 | 409 | nd = nearestObstacle(sx, sy, arr) 410 | #print nd, sx, sy 411 | #print 'nd ',nd, sx, sy 412 | #sleep(0.5) 413 | if nd > sd: 414 | print 'nd ',nd, sx, sy 415 | break 416 | 417 | #sleep(10) 418 | return sol 419 | 420 | def print_path_to_file(sol): 421 | with open('path.txt', 'w') as f: 422 | for i in range(len(sol)): 423 | f.write(`sol[i][0]` + ' ' + `sol[i][1]` + '\n') 424 | print sol[i][0] 425 | 426 | def read_path_from_file(): 427 | sol = [] 428 | with open('path.txt', 'r') as f: 429 | data = f.readlines() 430 | 431 | for line in data: 432 | s = [] 433 | words = line.split() 434 | for j in words: 435 | s.append(int(j)) 436 | sol.append(s) 437 | 438 | return sol 439 | 440 | def check(x, y, dx, dy): 441 | if x < dx+2 and x > dx -2 and y < dy+2 and y > dy-2: 442 | return True 443 | else: 444 | return False 445 | 446 | def make_path(sx, sy, dx, dy, arr1): 447 | #print sx, sy, dx, dy 448 | sol = [] 449 | theta = math.atan2(dx-sx, dy-sy) 450 | i = 1 451 | x = sx 452 | y = sy 453 | while not (x > dx-2 and x < dx + 2 and y > dy - 2 and y < dy + 2): 454 | sol.append((x, y)) 455 | #print x, y 456 | x = sx + int(i*math.sin(theta)) 457 | y = sy + int(i*math.cos(theta)) 458 | i += 1 459 | cv2.circle(arr1, (sy, sx), 1, (0, 255, 255), 1) 460 | cv2.imshow('Artificial only', arr1) 461 | k = cv2.waitKey(1) 462 | return sol 463 | 464 | def final_path(sx, sy, dx, dy, arr, sol): 465 | print 'len ', len(sol) 466 | switch_d = 70 467 | solution = [] 468 | delta = 5 469 | theta = math.pi/8 470 | l = len(sol) 471 | dist1 = [0 for x in range(l)] 472 | dict = {} 473 | for i in range(len(sol)): 474 | dict[(sol[i][0], sol[i][1])] = i 475 | i = 0 476 | while i < l: 477 | #print 'i ', i 478 | if sx < dx + 2 and sx > dx - 2 and sy < dy + 2 and sy > dy - 2: 479 | break 480 | nd = nearestObstacle(sx, sy, arr) 481 | print "nd : ", nd 482 | if nd < switch_d: 483 | #print 'nd ', nd 484 | sol1 = path_planning(arr, sx, sy, dx, dy,theta, switch_d) 485 | print 'sol returned' 486 | for k in sol1: 487 | solution.append(k) 488 | sx1 = sol1[-1][0] 489 | sy1 = sol1[-1][1] 490 | 491 | d = 1000000000 492 | cx = -1 493 | cy = -1 494 | for j in sol: 495 | if math.sqrt((sx1-j[0])*(sx1-j[0])+ (sy1-j[1])*(sy1-j[1])) <= d: 496 | cx = j[0] 497 | cy = j[1] 498 | d = math.sqrt((sx1-j[0])*(sx1-j[0])+ (sy1-j[1])*(sy1-j[1])) 499 | sx = cx 500 | sy = cy 501 | 502 | endx = sol[-1][0] 503 | endy = sol[-1][1] 504 | cnt = 0 505 | i = dict[(sx, sy)] 506 | while sx != endx and sy != endy and cnt < 25: 507 | (sx, sy) = sol[i] 508 | i += 1 509 | cnt += 1 510 | 511 | ''' 512 | d = dict[(sx, sy)] + 4*len(sol1) 513 | d1 = d - delta 514 | d2 = d + delta 515 | x1 = 0 516 | x2 = 0 517 | y1 = 0 518 | y2 = 0 519 | 520 | d1 = max(d1, 0) 521 | if d1 > len(sol)-1 or d1 < 0: 522 | print 'd1 : ' , d1, x1, y1 523 | 524 | (x1, y1) = sol[d1] 525 | 526 | d2 = min(len(sol)-1, d2) 527 | (x2, y2) = sol[d2] 528 | 529 | d1 = dict[(x1, y1)] 530 | d2 = dict[(x2, y2)] 531 | 532 | solx = sol[-1][0] 533 | soly = sol[-1][1] 534 | cost = 100000000 535 | for j in range(d1, d2): 536 | (x, y) = sol[j] 537 | cost1 = math.sqrt((x-solx)*(x-solx) + (y-soly)*(y-soly)) 538 | if cost > cost1: 539 | cost = cost1 540 | (sx, sy) = sol[j] 541 | 542 | ''' 543 | i = dict[(sx, sy)] 544 | 545 | 546 | sol1 = make_path(sx1, sy1, sx, sy, arr) 547 | for k in sol1: 548 | solution.append(k) 549 | else: 550 | solution.append((sol[i][0], sol[i][1])) 551 | i += 1 552 | if i >= l: 553 | break 554 | sx = sol[i][0] 555 | sy = sol[i][1] 556 | theta = math.atan2(sol[i][0]-sol[i-1][0], sol[i][1]-sol[i-1][1]) 557 | return solution 558 | 559 | def main(): 560 | counter = 1 561 | for img in images: 562 | #if not im == "5.jpg": 563 | # continue 564 | img = cv2.imread('1.jpg') 565 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 566 | img2 = cv2.medianBlur(cimg,13) 567 | 568 | ret,thresh1 = cv2.threshold(cimg,100,120,cv2.THRESH_BINARY) 569 | t2 = copy.copy(thresh1) 570 | 571 | x, y = thresh1.shape 572 | arr = np.zeros((x, y, 3), np.uint8) 573 | arr1 = np.zeros((x, y, 3), np.uint8) 574 | final_contours= [] 575 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 576 | for i in range(len(contours)): 577 | cnt = contours[i] 578 | if cv2.contourArea(cnt) > 1000 and cv2.contourArea(cnt) < 15000 : 579 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 580 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 581 | cv2.fillConvexPoly(arr1, cnt, [255, 255, 255]) 582 | final_contours.append(cnt) 583 | cmax = 50 584 | start = time.clock() 585 | min_cost = fill_clearance(arr,cmax, final_contours) 586 | print 'time: ', time.clock()-start 587 | 588 | for i in xrange(x): 589 | for j in xrange(y): 590 | pix_val = int(255-5*min_cost[i][j]) 591 | if(min_cost[i][j] > 10000): 592 | pix_val = 0 593 | arr[i, j] = (pix_val, pix_val, pix_val) 594 | for cnt in final_contours: 595 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 596 | 597 | ''' 598 | Code from A-star.py 599 | ''' 600 | sx = 60 # raw_input("Enter source and destination Coordinates") 601 | sy = 60 # raw_input() 602 | dx = 480 # raw_input() 603 | dy = 900 # raw_input() 604 | 605 | sol = bfs(arr, sx, sy, dx, dy, final_contours) 606 | #sol = read_path_from_file() 607 | for i in range(len(sol)): 608 | start = (sol[i][1], sol[i][0]) 609 | cv2.circle(arr, start, 1, [0, 0, 255]) 610 | cv2.circle(img, start, 1, [0, 0, 255]) 611 | cv2.circle(arr1, start, 1, [0, 0, 255]) 612 | 613 | l = len(sol) 614 | s = [] 615 | for i in range(l): 616 | s.append((sol[l-i-1][0], sol[l-i-1][1])) 617 | solution = final_path(sx,sy, dx, dy, arr, s) 618 | #print solution 619 | 620 | 621 | if len(solution) == 0: 622 | print 'No solution from source to destination' 623 | else: 624 | for i in range(len(solution)): 625 | start = (solution[i][1], solution[i][0]) 626 | cv2.circle(arr, start, 1, [255, 0, 0]) 627 | cv2.circle(img, start, 1, [255, 0, 0]) 628 | cv2.circle(arr1, start, 1, [255, 0, 0 ]) 629 | 630 | cv2.circle(arr, (sy, sx), 2, [0, 255, 0]) 631 | cv2.circle(arr, (dy, dx), 2, [0, 255, 0]) 632 | cv2.circle(img, (sy, sx), 2, [0, 255, 0]) 633 | cv2.circle(img, (dy, dx), 2, [0, 255, 0]) 634 | 635 | output = "output2/"+`counter` 636 | output += ".jpg" 637 | cv2.imwrite(output, img) 638 | counter += 1 639 | 640 | cv2.imshow('image', img) 641 | cv2.imshow('arr', arr) 642 | cv2.imshow('arr1', arr1) 643 | cv2.waitKey(0) 644 | cv2.destroyAllWindows() 645 | 646 | main() --------------------------------------------------------------------------------