github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 05:57:59 -05:00
Code Issues Packages Projects Releases Wiki Activity

Replaced sqrt(x**2+y**2) with hypot in PathPlanning/ClosedLoopRRTStar/pure_pursuit.py

Browse Source
This commit is contained in:
Guillaume Jacquenot
2019-12-07 22:24:27 +01:00
parent 1eb3ebbf26
commit 00f8fd37d3
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -68,17 +68,17 @@ def calc_target_index(state, cx, cy):
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
d = np.hypot(dx, dy)
mindis = min(d)
ind = d.index(mindis)
ind = np.argmin(d)
L = 0.0
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cy[ind + 1] - cy[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
L += math.hypot(dx, dy)
ind += 1
# print(mindis)
@@ -121,7 +121,7 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis:
if math.hypot(dx, dy) <= goal_dis:
find_goal = True
break