Simplified calc_target_index function in stanley_controller.py

Gjacquenot · web-flow · commit 22e92ed55838 · 2019-12-07T14:40:28.000+01:00
Used:

- np.hypot to replace np.sqrt(x**2 + y**2) 
- np.argmin to find location of a minimum in a numpy array
diff --git 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
