├── README.md └── main.py /README.md: -------------------------------------------------------------------------------- 1 | # pure-pursuit-python 2 | Implementation of the pure pursuit algorithm with matplotlib visualization 3 | ![screenshot](https://i.imgur.com/C5Lc5pe.png) 4 | 5 | ## Installation 6 | In order to run the code you need to have the following dependencies matplotlib, numpy 7 | Once you have those dependencies, you can run the code with `python3 main.py` 8 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import random 4 | from matplotlib.path import Path 5 | import matplotlib.patches as patches 6 | from matplotlib.widgets import * 7 | import json 8 | import math 9 | from time import sleep 10 | 11 | 12 | class PathFollower(object): 13 | 14 | def __init__(self, x, y, speed): 15 | self.position = (x, y) 16 | self.theta = np.pi 17 | self.wheelbase = 1 18 | self.history = [self.position] 19 | self.is_dead = False 20 | 21 | def steering_angle(self, goal): 22 | v = [math.cos(self.theta), math.sin(self.theta), 0] 23 | rg = [goal[0]-self.position[0], goal[1]-self.position[1], 0] 24 | prodNorm = np.linalg.norm(v)*np.linalg.norm(rg) 25 | cosPhi = np.dot(v, rg)/prodNorm 26 | sinPhi = np.cross(v, rg)[2]/prodNorm 27 | phi = math.atan2(sinPhi, cosPhi) 28 | 29 | return min(max(phi, -math.pi/4.1), math.pi/4.1) 30 | 31 | def move_torwards(self, x, y, lookahead, speed): 32 | offset = (x - self.position[0], y - self.position[1]) 33 | dist = (offset[0] ** 2 + offset[1] ** 2) ** 0.5 34 | 35 | steering_angle = self.steering_angle((x, y)) 36 | u = speed * 0.1 * np.cos(self.theta), speed * 0.1 * np.sin(self.theta) 37 | self.position = self.position[0] + u[0], self.position[1] + u[1] 38 | self.theta += speed * 0.1 * np.tan(steering_angle) / self.wheelbase 39 | # self.position = (self.position[0] + offset[0] / dist * self.speed, 40 | # self.position[1] + offset[1] / dist * self.speed) 41 | self.history.append(self.position) 42 | 43 | def get_pos(self): 44 | return self.position 45 | 46 | 47 | class PurePursuit(object): 48 | 49 | def __init__(self, path, lookaheadDistance=2, lookaheadDistanceDelta=2.5, followerSpeed=1, followerStopDistance=1, ax=None): 50 | self.path = path 51 | self.lookaheadDistance = lookaheadDistance 52 | self.lookaheadDistanceDelta = lookaheadDistanceDelta 53 | self.followerSpeed = followerSpeed 54 | self.followerStopDistance = followerStopDistance 55 | self.ax = ax 56 | self.followers = [] 57 | 58 | def draw(self): 59 | if self.ax != None: 60 | self.ax.clear() 61 | xs, ys = zip(*self.path) 62 | self.ax.plot(xs, ys) 63 | for f in self.followers: 64 | if f.is_dead: 65 | continue 66 | pos = f.get_pos() 67 | lookahead = self.get_lookahead_pt( 68 | pos[0], pos[1], self.lookaheadDistance) 69 | self.ax.scatter(pos[0], pos[1], c='b') 70 | xs, ys = zip(*f.history) 71 | self.ax.plot(xs, ys, linestyle=':') 72 | if lookahead != None: 73 | self.ax.scatter(lookahead[0], lookahead[1], c='r') 74 | delta = lookahead[0] - pos[0], lookahead[1] - pos[1] 75 | dist = 2 * (delta[0] ** 2 + delta[1] ** 2) ** 0.5 76 | self.ax.add_artist(plt.Circle(pos, dist/2, fill=False)) 77 | self.ax.plot((pos[0], lookahead[0]), (pos[1], lookahead[1]), linestyle='--') 78 | 79 | if dist < self.followerStopDistance: 80 | f.is_dead = True 81 | else: 82 | f.move_torwards(lookahead[0], lookahead[1], dist, self.followerSpeed) 83 | 84 | def add_follower(self, x, y): 85 | self.followers.append(PathFollower( 86 | x, y, self.followerSpeed)) 87 | 88 | def sign(self, n): 89 | if n == 0: 90 | return 1 91 | else: 92 | return n/abs(n) 93 | 94 | def get_lookahead_pt(self, x, y, radius): 95 | look_pt = None 96 | # http://mathworld.wolfram.com/Circle-LineIntersection.html 97 | for i in range(len(path) - 1): 98 | seg_start = path[i] 99 | seg_end = path[i + 1] 100 | p1 = seg_start[0] - x, seg_start[1] - y 101 | p2 = seg_end[0] - x, seg_end[1] - y 102 | 103 | d = ((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2) ** 0.5 104 | D = p1[0] * p2[1] - p2[0] * p1[1] 105 | 106 | discriminant = radius ** 2 * d * d - D * D 107 | 108 | if discriminant < 0 or p1 == p2: 109 | continue 110 | 111 | x1 = (D * (p2[1] - p1[1]) + self.sign(p2[1] - p1[1]) 112 | * (p2[0] - p1[0]) * discriminant**0.5) / (d * d) 113 | y1 = (-D * (p2[0] - p1[0]) + abs(p2[1] - p1[1]) 114 | * discriminant ** 0.5) / (d * d) 115 | 116 | x2 = (D * (p2[1] - p1[1]) - self.sign(p2[1] - p1[1]) 117 | * (p2[0] - p1[0]) * discriminant ** 0.5) / (d * d) 118 | y2 = (-D * (p2[0] - p1[0]) - abs(p2[1] - p1[1]) 119 | * discriminant ** 0.5) / (d * d) 120 | 121 | intersection1 = min(p1[0], p2[0]) < x1 and x1 < max( 122 | p1[0], p2[0]) or min(p1[1], p2[1]) < y1 and y1 < max(p1[1], p2[1]) 123 | intersection2 = min(p1[0], p2[0]) < x2 and x2 < max( 124 | p1[0], p2[0]) or min(p1[1], p2[1]) < y2 and y2 < max(p1[1], p2[1]) 125 | 126 | if intersection1 or intersection2: 127 | look_pt = None 128 | 129 | if intersection1: 130 | look_pt = x1 + x, y1 + y 131 | 132 | if intersection2 and (look_pt == None or abs(x1 - p2[0]) > abs(x2 - p2[0]) or abs(y1 - p2[1]) > abs(y2 - p2[1])): 133 | look_pt = x2 + x, y2 + y 134 | 135 | if len(self.path) > 0: 136 | if ((self.path[-1][0] - x)**2 + (self.path[-1][1] - y)**2)**0.5 <= radius: 137 | return self.path[-1][0], self.path[-1][1] 138 | return look_pt 139 | 140 | 141 | with open('path.json', 'r') as f: 142 | path = json.load(f) 143 | 144 | 145 | fig, ax = plt.subplots() 146 | ax.set_aspect(1) 147 | 148 | plt.ion() 149 | plt.show() 150 | 151 | pp = [] 152 | for i in range(len(path)): 153 | if i % 100 == 0: 154 | pp.append(path[i]) 155 | pure_pursuit = PurePursuit(pp, ax=ax, followerSpeed=2, lookaheadDistance=5) 156 | 157 | 158 | def add_car(x): 159 | pure_pursuit.add_follower(random.uniform(0, 3), random.uniform(2, 20)) 160 | 161 | def set_lookahead(x): 162 | pure_pursuit.lookaheadDistance = x 163 | 164 | def set_speed(x): 165 | pure_pursuit.followerSpeed = x 166 | 167 | 168 | add_btn = Button(plt.axes([0.75, 0.9, 0.15, 0.075]), 'Add car') 169 | add_btn.on_clicked(add_car) 170 | lookahead_distance = Slider(plt.axes( 171 | [0.8, 0.7, 0.15, 0.05]), 'Lookahead', 1, 30, valinit=2.5, valstep=0.1) 172 | lookahead_distance.on_changed(set_lookahead) 173 | speed = Slider(plt.axes( 174 | [0.8, 0.5, 0.15, 0.05]), 'Speed', 0.1, 30, valinit=1, valstep=0.1) 175 | speed.on_changed(set_speed) 176 | 177 | for i in range(1000): 178 | pure_pursuit.draw() 179 | plt.pause(0.1) 180 | if not plt.fignum_exists(1): 181 | break 182 | --------------------------------------------------------------------------------