diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 351232d8..8090e336 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -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 [], []