gx: goal x position [m] gx: goal x position [m] ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] reso: grid resolution [m] rr: robot radius[m]
(sx, sy, gx, gy, ox, oy, reso, rr)
| 41 | |
| 42 | |
| 43 | def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): |
| 44 | """ |
| 45 | gx: goal x position [m] |
| 46 | gx: goal x position [m] |
| 47 | ox: x position list of Obstacles [m] |
| 48 | oy: y position list of Obstacles [m] |
| 49 | reso: grid resolution [m] |
| 50 | rr: robot radius[m] |
| 51 | """ |
| 52 | |
| 53 | nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1) |
| 54 | ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1) |
| 55 | ox = [iox / reso for iox in ox] |
| 56 | oy = [ioy / reso for ioy in oy] |
| 57 | |
| 58 | obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr) |
| 59 | |
| 60 | motion = get_motion_model() |
| 61 | |
| 62 | openset, closedset = dict(), dict() |
| 63 | openset[calc_index(ngoal, xw, minx, miny)] = ngoal |
| 64 | pq = [] |
| 65 | pq.append((0, calc_index(ngoal, xw, minx, miny))) |
| 66 | |
| 67 | while 1: |
| 68 | if not pq: |
| 69 | break |
| 70 | cost, c_id = heapq.heappop(pq) |
| 71 | if c_id in openset: |
| 72 | current = openset[c_id] |
| 73 | closedset[c_id] = current |
| 74 | openset.pop(c_id) |
| 75 | else: |
| 76 | continue |
| 77 | |
| 78 | # show graph |
| 79 | if show_animation: # pragma: no cover |
| 80 | plt.plot(current.x * reso, current.y * reso, "xc") |
| 81 | if len(closedset.keys()) % 10 == 0: |
| 82 | plt.pause(0.001) |
| 83 | |
| 84 | # Remove the item from the open set |
| 85 | |
| 86 | # expand search grid based on motion model |
| 87 | for i, _ in enumerate(motion): |
| 88 | node = Node(current.x + motion[i][0], |
| 89 | current.y + motion[i][1], |
| 90 | current.cost + motion[i][2], c_id) |
| 91 | n_id = calc_index(node, xw, minx, miny) |
| 92 | |
| 93 | if n_id in closedset: |
| 94 | continue |
| 95 | |
| 96 | if not verify_node(node, obmap, minx, miny, maxx, maxy): |
| 97 | continue |
| 98 | |
| 99 | if n_id not in openset: |
| 100 | openset[n_id] = node # Discover a new node |
no test coverage detected