From 1ef134767d234e081aa74e12ea954c41e5be9ff1 Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 13:52:32 +0200 Subject: [PATCH] Only calculate all distances between car and path points once --- PathTracking/pure_pursuit/pure_pursuit.py | 29 +++++++++++++++++++---- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 9413c24a..4fbeadb6 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -9,6 +9,8 @@ import numpy as np import math import matplotlib.pyplot as plt +old_nearest_point_index = None + k = 0.1 # look forward gain Lfc = 1.0 # look-ahead distance Kp = 1.0 # speed proportional gain @@ -71,14 +73,31 @@ def pure_pursuit_control(state, cx, cy, pind): return delta, ind +def calc_distance(state, point_x, point_y): + dx = state.rear_x - point_x + dy = state.rear_y - point_y + return math.sqrt(distance_x ** 2 + distance_y ** 2) + def calc_target_index(state, cx, cy): - # search nearest point index - dx = [state.rear_x - icx for icx in cx] - dy = [state.rear_y - icy for icy in cy] - d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] - ind = d.index(min(d)) + if old_nearest_point_index is None: + # search nearest point index + dx = [state.rear_x - icx for icx in cx] + dy = [state.rear_y - icy for icy in cy] + d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] + ind = d.index(min(d)) + else: + index = old_nearest_point_index + distance_this_index = calc_distance(state, cx[index], cy[index]) + while True: + index = index + 1 if (index + 1) < len(cx) else index + distance_next_index = calc_distance(state, cx[index], cy[index]) + if distance_this_index < distance_next_index + break + distance_this_index = distance_next_index + old_nearest_point_index = index + L = 0.0 Lf = k * state.v + Lfc