From af474687d1066e608ff95b3dd4df3c4615b37bbe Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 13:39:43 +0200 Subject: [PATCH 1/7] Use rear axle instead of center --- PathTracking/pure_pursuit/pure_pursuit.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index af953e30..9413c24a 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -26,6 +26,8 @@ class State: self.y = y self.yaw = yaw self.v = v + self.rear_x = self.x - ((L / 2) * math.cos(self.yaw)) + self.rear_y = self.y - ((L / 2) * math.sin(self.yaw)) def update(state, a, delta): @@ -34,6 +36,8 @@ def update(state, a, delta): state.y = state.y + state.v * math.sin(state.yaw) * dt state.yaw = state.yaw + state.v / L * math.tan(delta) * dt state.v = state.v + a * dt + state.rear_x = state.x - ((L / 2) * math.cos(state.yaw)) + state.rear_y = state.y - ((L / 2) * math.sin(state.yaw)) return state @@ -59,7 +63,7 @@ def pure_pursuit_control(state, cx, cy, pind): ty = cy[-1] ind = len(cx) - 1 - alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw + alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw Lf = k * state.v + Lfc @@ -71,8 +75,8 @@ def pure_pursuit_control(state, cx, cy, pind): def calc_target_index(state, cx, cy): # search nearest point index - dx = [state.x - icx for icx in cx] - dy = [state.y - icy for icy in cy] + 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)) L = 0.0 @@ -81,8 +85,8 @@ def calc_target_index(state, cx, cy): # search look ahead target point index while Lf > L and (ind + 1) < len(cx): - dx = cx[ind] - state.x - dy = cy[ind] - state.y + dx = cx[ind] - state.rear_x + dy = cy[ind] - state.rear_y L = math.sqrt(dx ** 2 + dy ** 2) ind += 1 From 1ef134767d234e081aa74e12ea954c41e5be9ff1 Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 13:52:32 +0200 Subject: [PATCH 2/7] 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 From 4706e4be0ae62ffeb770cec18b0b29bf0799be42 Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 14:12:05 +0200 Subject: [PATCH 3/7] Fixed syntax --- PathTracking/pure_pursuit/pure_pursuit.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 4fbeadb6..66483b13 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -93,7 +93,7 @@ def calc_target_index(state, cx, cy): 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 + if distance_this_index < distance_next_index: break distance_this_index = distance_next_index old_nearest_point_index = index From d5d026db0f17a7426fb317c0abb6776ca296d8d4 Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 14:22:15 +0200 Subject: [PATCH 4/7] Set old index --- PathTracking/pure_pursuit/pure_pursuit.py | 1 + 1 file changed, 1 insertion(+) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 66483b13..73c86c42 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -87,6 +87,7 @@ def calc_target_index(state, cx, cy): 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)) + old_nearest_point_index = ind else: index = old_nearest_point_index distance_this_index = calc_distance(state, cx[index], cy[index]) From 82c55d42c4331bcd3b82cd3a980b89c6236fce77 Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 14:49:56 +0200 Subject: [PATCH 5/7] Made old index global --- PathTracking/pure_pursuit/pure_pursuit.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 73c86c42..fb3a90b8 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -74,13 +74,16 @@ 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) + return math.sqrt(dx ** 2 + dy ** 2) def calc_target_index(state, cx, cy): + global old_nearest_point_index + if old_nearest_point_index is None: # search nearest point index dx = [state.rear_x - icx for icx in cx] From 54c872aa2b91e2dfa430a1dd5a628c5448e47d9b Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 15:03:42 +0200 Subject: [PATCH 6/7] Use correct name --- PathTracking/pure_pursuit/pure_pursuit.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index fb3a90b8..758bf1b9 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -92,7 +92,7 @@ def calc_target_index(state, cx, cy): ind = d.index(min(d)) old_nearest_point_index = ind else: - index = old_nearest_point_index + ind = 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 @@ -100,7 +100,7 @@ def calc_target_index(state, cx, cy): if distance_this_index < distance_next_index: break distance_this_index = distance_next_index - old_nearest_point_index = index + old_nearest_point_index = ind L = 0.0 From 58c2a80e9728375ddce2c4e9231de6804f378b2c Mon Sep 17 00:00:00 2001 From: Kitsunow Date: Thu, 9 May 2019 15:16:07 +0200 Subject: [PATCH 7/7] Set index to ind --- PathTracking/pure_pursuit/pure_pursuit.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 758bf1b9..68cd70d5 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -93,10 +93,10 @@ def calc_target_index(state, cx, cy): old_nearest_point_index = ind else: ind = old_nearest_point_index - distance_this_index = calc_distance(state, cx[index], cy[index]) + distance_this_index = calc_distance(state, cx[ind], cy[ind]) while True: - index = index + 1 if (index + 1) < len(cx) else index - distance_next_index = calc_distance(state, cx[index], cy[index]) + ind = ind + 1 if (ind + 1) < len(cx) else ind + distance_next_index = calc_distance(state, cx[ind], cy[ind]) if distance_this_index < distance_next_index: break distance_this_index = distance_next_index