github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 21:27:56 -05:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Guillaume Jacquenot
2019-12-07 22:53:08 +01:00
parent 077120201b
commit d20f1e336e
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -97,7 +97,7 @@ def is_collision(sx, sy, gx, gy, rr, okdtree):
dx = gx - sx
dy = gy - sy
yaw = math.atan2(gy - sy, gx - sx)
d = math.sqrt(dx**2 + dy**2)
d = math.hypot(dx, dy)
if d >= MAX_EDGE_LEN:
return True
@@ -171,7 +171,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
road_map: ??? [m]
sample_x: ??? [m]
sample_y: ??? [m]
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
"""
@@ -182,7 +182,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[len(road_map) - 2] = nstart
path_found = True
while True:
if not openset:
print("Cannot find path")
@@ -213,7 +213,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
n_id = road_map[c_id][i]
dx = sample_x[n_id] - current.x
dy = sample_y[n_id] - current.y
d = math.sqrt(dx**2 + dy**2)
d = math.hypot(dx, dy)
node = Node(sample_x[n_id], sample_y[n_id],
current.cost + d, c_id)
@@ -226,7 +226,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[n_id].pind = c_id
else:
openset[n_id] = node
if path_found is False:
return [], []