Add files via upload

Author: finleyhewson

Diff for: Astar_heuristic.py

@@ -0,0 +1,298 @@
+import math
+import heapq
+import matplotlib.pyplot as plt
+import time
+
+show_animation = False
+
+
+class Node:
+
+    def __init__(self, x, y, cost, pind):
+        self.x = x
+        self.y = y
+        self.cost = cost
+        self.pind = pind
+
+    def __str__(self):
+        return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
+
+
+def calc_final_path(ngoal, closedset, reso):
+    # generate final course
+    rx, ry = [ngoal.x * reso], [ngoal.y * reso]
+    pind = ngoal.pind
+    while pind != -1:
+        n = closedset[pind]
+        rx.append(n.x * reso)
+        ry.append(n.y * reso)
+        pind = n.pind
+
+    return rx, ry
+
+
+def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
+    """
+    sx: start x position [m]
+    sy: start y position [m]
+    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]
+    """
+
+    nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
+    ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
+    ox = [iox / reso for iox in ox] #divides all of ox by the resolution
+    oy = [ioy / reso for ioy in oy] #divides all of oy by the resolution
+    
+    obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
+    #t1=time.perf_counter()
+    #defines movement in terms of relative positions and gives the cost of each movement 
+    motion = get_motion_model()
+
+    #initialising both the yet to visit and visited list
+    openset, closedset = dict(), dict()
+    openset[calc_index(ngoal, xw, minx, miny)] = ngoal
+    pq = []
+    pq.append((0, calc_index(ngoal, xw, minx, miny)))
+
+    while 1:
+        if not pq:
+            break
+        cost, c_id = heapq.heappop(pq) #c_id is the current index, heapop returns the smallest data element from the heap pq
+        #popping current node out of openset into closed set
+        if c_id in openset:
+            current = openset[c_id]
+            closedset[c_id] = current
+            openset.pop(c_id)
+        else:
+            continue
+
+        # show graph
+        if show_animation:  # pragma: no cover
+            plt.plot(current.x * reso, current.y * reso, "xc")
+            # for stopping simulation with the esc key.
+            plt.gcf().canvas.mpl_connect('key_release_event',
+                    lambda event: [exit(0) if event.key == 'escape' else None])
+            if len(closedset.keys()) % 10 == 0:
+                plt.pause(0.001)
+
+        # Remove the item from the open set
+
+        # expand search grid based on motion model, generates child nodes from adjacent squares
+        for i, _ in enumerate(motion):
+            node = Node(current.x + motion[i][0],
+                        current.y + motion[i][1],
+                        current.cost + motion[i][2], c_id)
+            n_id = calc_index(node, xw, minx, miny)
+            #making sure not to use nodes already visited
+            if n_id in closedset:
+                continue
+            #checking node is within boundary and not in obstacle
+            if not verify_node(node, obmap, minx, miny, maxx, maxy):
+                continue
+
+            if n_id not in openset:
+                openset[n_id] = node  # Discover a new node
+                heapq.heappush(
+                    pq, (node.cost, calc_index(node, xw, minx, miny)))
+            else:
+                if openset[n_id].cost >= node.cost:
+                    # This path is the best until now. record it!
+                    openset[n_id] = node
+                    heapq.heappush(
+                        pq, (node.cost, calc_index(node, xw, minx, miny)))
+
+    rx, ry = calc_final_path(closedset[calc_index(
+        nstart, xw, minx, miny)], closedset, reso)
+    #t2=time.perf_counter()
+    #print(f"Path Planner in {t2 - t1:0.4f} seconds")
+    return rx, ry, closedset
+
+
+def calc_heuristic(n1, n2):
+    w = 1.0  # weight of heuristic
+    d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
+    return d
+
+#Making sure the node is within range and not in an obstacle
+def verify_node(node, obmap, minx, miny, maxx, maxy):
+
+    if node.x < minx:
+        return False
+    elif node.y < miny:
+        return False
+    elif node.x >= maxx:
+        return False
+    elif node.y >= maxy:
+        return False
+    #Need to subtract minx and miny to scale to the obmap indicies correctly
+    if obmap[int(round(node.x-minx))][int(round(node.y-miny))]:
+        return False
+
+    return True
+
+#function determines whether position would cause a collision with an obstacle 
+def calc_obstacle_map(ox, oy, reso, vr):
+
+    minx = int(round(min(ox)))
+    miny = int(round(min(oy)))
+    maxx = int(round(max(ox))) 
+    maxy = int(round(max(oy)))
+
+    xwidth = round(maxx - minx)
+    ywidth = round(maxy - miny)
+    # obstacle map generation, determines which positions would cause a collision 
+    # with an obstacle given the device's radius
+    obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
+    #for ix in range(xwidth):
+    #    x = ix + minx #the current x position
+    #    for iy in range(ywidth):
+    #        y = iy + miny #the current y position
+    #        #  print(x, y)
+    #        for iox, ioy in zip(ox, oy):
+    #            d = math.sqrt((iox - x)**2 + (ioy - y)**2) #distance from current x and y position to obstacle
+    #            if d <= vr / reso:
+    #                obmap[ix][iy] = True
+    #                break
+    obmap_motion = obmap_motion_model()
+
+    for iox, ioy in zip(ox, oy):
+        rox=iox
+        roy=ioy
+        iox=int(round(iox))
+        ioy=int(round(ioy))
+        
+        for i, _ in enumerate(obmap_motion):
+            adjind = [iox + obmap_motion[i][0],
+                      ioy + obmap_motion[i][1]]
+            
+            if not verify_obmap(adjind, minx, miny, maxx, maxy):
+                continue
+
+            d = math.sqrt((rox - adjind[0])**2 + (roy - adjind[1])**2)
+            if d <= vr / reso:
+                ix = adjind[0] - minx
+                iy = adjind[1] - miny
+                obmap[ix][iy] = True
+
+
+    return obmap, minx, miny, maxx, maxy, xwidth, ywidth
+
+
+def calc_index(node, xwidth, xmin, ymin):
+    return (node.y - ymin) * xwidth + (node.x - xmin)
+
+#function just giving the option to move in 8 directions(up, down, diagonally, etc..) 
+#and the costs of moving in those directions
+def get_motion_model():
+    # dx, dy, cost
+    motion = [[1, 0, 1],
+              [0, 1, 1],
+              [-1, 0, 1],
+              [0, -1, 1],
+              [-1, -1, math.sqrt(2)],
+              [-1, 1, math.sqrt(2)],
+              [1, -1, math.sqrt(2)],
+              [1, 1, math.sqrt(2)]]
+
+    return motion
+
+# function needs to be changed depending on how the resolution and car radius change
+def obmap_motion_model():
+    # dx, dy
+    obmap_motion = [[0, 0],
+                    [1, 0],
+                    [2, 0],
+                    [0, 1],
+                    [0, 2],
+                    [-1, 0],
+                    [-2, 0],
+                    [0, -1],
+                    [0, -2],
+                    [-1, -1],
+                    [-1, 1],
+                    [1, -1],
+                    [1, 1],
+                    [1, 2],
+                    [2, 1],
+                    [2, 2],
+                    [-1, 2],
+                    [-2, 1],
+                    [-2, 2],
+                    [1, -2],
+                    [2, -1],
+                    [2, -2],
+                    [-1, -2],
+                    [-2, -1],
+                    [-2, -2]]
+
+    return obmap_motion
+
+
+def verify_obmap(adjind, minx, miny, maxx, maxy):
+
+    if adjind[0] < minx:
+        return False
+    elif adjind[1] < miny:
+        return False
+    elif adjind[0] >= maxx:
+        return False
+    elif adjind[1] >= maxy:
+        return False
+
+    return True
+
+
+def main():
+    print(__file__ + " start!!")
+
+    # start and goal position
+    sx = 10.0  # [m]
+    sy = 10.0  # [m]
+    gx = 50.0  # [m]
+    gy = 50.0  # [m]
+    grid_size = 2.0  # [m]
+    robot_size = 1.0  # [m]
+
+    ox, oy = [], []
+
+    for i in range(60):
+        ox.append(i)
+        oy.append(0.0)
+    for i in range(60):
+        ox.append(60.0)
+        oy.append(i)
+    for i in range(61):
+        ox.append(i)
+        oy.append(60.0)
+    for i in range(61):
+        ox.append(0.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(20.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(40.0)
+        oy.append(60.0 - i)
+
+    if show_animation:  # pragma: no cover
+        plt.plot(ox, oy, ".k")
+        plt.plot(sx, sy, "xr")
+        plt.plot(gx, gy, "xb")
+        plt.grid(True)
+        plt.axis("equal")
+
+    rx, ry, _ = dp_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
+
+    if show_animation:  # pragma: no cover
+        plt.plot(rx, ry, "-r")
+        plt.show()
+
+
+if __name__ == '__main__':
+    show_animation = True
+    main()

Diff for: Car.py

@@ -0,0 +1,94 @@
+import matplotlib.pyplot as plt
+from math import sqrt, cos, sin, tan, pi
+
+WB = 0.33  # rear to front wheel
+W = 0.26  # width of car
+LF = 0.5  # distance from rear to vehicle front end
+LB = 0.15  # distance from rear to vehicle back end
+MAX_STEER = 0.5  # [rad] maximum steering angle
+
+WBUBBLE_DIST = (LF - LB) / 2.0
+WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1)
+
+# vehicle rectangle verticles
+VRX = [LF, LF, -LB, -LB, LF]
+VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2]
+
+
+def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
+    for x, y, yaw in zip(xlist, ylist, yawlist):
+        cx = x + WBUBBLE_DIST * cos(yaw)
+        cy = y + WBUBBLE_DIST * sin(yaw)
+
+        ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R)
+
+        if not ids:
+            continue
+
+        if not rectangle_check(x, y, yaw,
+                               [ox[i] for i in ids], [oy[i] for i in ids]):
+            return False  # collision
+
+    return True  # no collision
+
+
+def rectangle_check(x, y, yaw, ox, oy):
+    # transform obstacles to base link frame
+    c, s = cos(-yaw), sin(-yaw)
+    for iox, ioy in zip(ox, oy):
+        tx = iox - x
+        ty = ioy - y
+        rx = c * tx - s * ty
+        ry = s * tx + c * ty
+
+        if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
+            return False  # no collision
+
+    return True  # collision
+
+
+def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
+    """Plot arrow."""
+    if not isinstance(x, float):
+        for (ix, iy, iyaw) in zip(x, y, yaw):
+            plot_arrow(ix, iy, iyaw)
+    else:
+        plt.arrow(x, y, length * cos(yaw), length * sin(yaw),
+                  fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)
+        # plt.plot(x, y)
+
+
+def plot_car(x, y, yaw):
+    car_color = '-k'
+    c, s = cos(yaw), sin(yaw)
+
+    car_outline_x, car_outline_y = [], []
+    for rx, ry in zip(VRX, VRY):
+        tx = c * rx - s * ry + x
+        ty = s * rx + c * ry + y
+        car_outline_x.append(tx)
+        car_outline_y.append(ty)
+
+    arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw
+    plot_arrow(arrow_x, arrow_y, arrow_yaw)
+
+    plt.plot(car_outline_x, car_outline_y, car_color)
+
+
+def pi_2_pi(angle):
+    return (angle + pi) % (2 * pi) - pi
+
+
+def move(x, y, yaw, distance, steer, L=WB):
+    x += distance * cos(yaw)
+    y += distance * sin(yaw)
+    yaw += pi_2_pi(distance * tan(steer) / L)  # distance/2
+
+    return x, y, yaw
+
+
+if __name__ == '__main__':
+    x, y, yaw = 0., 0., 1.
+    plt.axis('equal')
+    plot_car(x, y, yaw)
+    plt.show()