From 22e92ed5583875fbc4e46fce044059f936b0be4e Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 14:40:28 +0100 Subject: [PATCH] Simplified calc_target_index function in stanley_controller.py Used: - np.hypot to replace np.sqrt(x**2 + y**2) - np.argmin to find location of a minimum in a numpy array --- PathTracking/stanley_controller/stanley_controller.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index 42a446dda3..9b808dd963 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -127,20 +127,19 @@ def calc_target_index(state, cx, cy): :param cy: [float] :return: (int, float) """ - # Calc front axle position + # Calc front axle position fx = state.x + L * np.cos(state.yaw) fy = state.y + L * np.sin(state.yaw) # Search nearest point index dx = [fx - icx for icx in cx] dy = [fy - icy for icy in cy] - d = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] - closest_error = min(d) - target_idx = d.index(closest_error) + d = np.hypot(dx, dy) + target_idx = np.argmin(d) # Project RMS error onto front axle vector front_axle_vec = [-np.cos(state.yaw + np.pi / 2), - - np.sin(state.yaw + np.pi / 2)] + -np.sin(state.yaw + np.pi / 2)] error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec) return target_idx, error_front_axle