From fa3692da3e000c0d7792fc4b20257848e524036e Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 7 Feb 2018 23:42:30 -0800 Subject: [PATCH] test fixed --- .../closed_loop_rrt_star_car.py | 23 ++++- .../reeds_shepp_path_planning.py | 92 ------------------- .../RRTStarReedsShepp/rrt_star_reeds_shepp.py | 22 ++++- PathPlanning/RRTstar/rrt_star.py | 2 + .../reeds_shepp_path_planning.py | 5 +- tests/test_reeds_shepp_path_planning.py | 1 - tests/test_rrt_star_reeds_shepp.py | 1 + 7 files changed, 46 insertions(+), 100 deletions(-) delete mode 100644 PathPlanning/RRTStarReedsShepp/reeds_shepp_path_planning.py diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index ad699b61..751577cb 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -21,7 +21,7 @@ show_animation = True target_speed = 10.0 / 3.6 -STEP_SIZE = 0.5 +STEP_SIZE = 0.1 class RRT(): @@ -52,6 +52,8 @@ class RRT(): goal = Node(self.end.x, self.end.y, self.end.yaw) newNode = self.steer(goal, len(self.nodeList) - 1) + if newNode is None: + return if self.CollisionCheck(newNode, self.obstacleList): # print("goal path is OK") @@ -74,10 +76,14 @@ class RRT(): newNode = self.steer(rnd, nind) # print(newNode.cost) + if newNode is None: + continue if self.CollisionCheck(newNode, self.obstacleList): nearinds = self.find_near_nodes(newNode) newNode = self.choose_parent(newNode, nearinds) + if newNode is None: + continue self.nodeList.append(newNode) self.rewire(newNode, nearinds) @@ -97,6 +103,8 @@ class RRT(): def search_best_feasible_path(self, path_indexs): + print("Start search feasible path") + best_time = float("inf") # pure pursuit tracking @@ -189,6 +197,9 @@ class RRT(): dlist = [] for i in nearinds: tNode = self.steer(newNode, i) + if tNode is None: + continue + if self.CollisionCheck(tNode, self.obstacleList): dlist.append(tNode.cost) else: @@ -202,6 +213,8 @@ class RRT(): return newNode newNode = self.steer(newNode, minind) + if newNode is None: + return None return newNode @@ -223,6 +236,9 @@ class RRT(): nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE) + if px is None: + return None + newNode = copy.deepcopy(nearestNode) newNode.x = px[-1] newNode.y = py[-1] @@ -231,7 +247,7 @@ class RRT(): newNode.path_x = px newNode.path_y = py newNode.path_yaw = pyaw - newNode.cost += sum(clen) + newNode.cost += sum([abs(c) for c in clen]) newNode.parent = nind return newNode @@ -308,6 +324,9 @@ class RRT(): nearNode = self.nodeList[i] tNode = self.steer(nearNode, nnode - 1) + if tNode is None: + continue + obstacleOK = self.CollisionCheck(tNode, self.obstacleList) imporveCost = nearNode.cost > tNode.cost diff --git a/PathPlanning/RRTStarReedsShepp/reeds_shepp_path_planning.py b/PathPlanning/RRTStarReedsShepp/reeds_shepp_path_planning.py deleted file mode 100644 index 3757aca7..00000000 --- a/PathPlanning/RRTStarReedsShepp/reeds_shepp_path_planning.py +++ /dev/null @@ -1,92 +0,0 @@ -""" - -Reeds Shepp path planner sample code - -author Atsushi Sakai(@Atsushi_twi) - -""" -import reeds_shepp -import math -import matplotlib.pyplot as plt - -show_animation = True - - -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): - """ - Plot arrow - """ - - if not isinstance(x, float): - for (ix, iy, iyaw) in zip(x, y, yaw): - plot_arrow(ix, iy, iyaw) - else: - plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), - fc=fc, ec=ec, head_width=width, head_length=width) - plt.plot(x, y) - - -def reeds_shepp_path_planning(start_x, start_y, start_yaw, - end_x, end_y, end_yaw, curvature): - step_size = 0.1 - q0 = [start_x, start_y, start_yaw] - q1 = [end_x, end_y, end_yaw] - qs = reeds_shepp.path_sample(q0, q1, 1.0 / curvature, step_size) - xs = [q[0] for q in qs] - ys = [q[1] for q in qs] - yaw = [q[2] for q in qs] - - xs.append(end_x) - ys.append(end_y) - yaw.append(end_yaw) - - clen = reeds_shepp.path_length(q0, q1, 1.0 / curvature) - pathtypeTuple = reeds_shepp.path_type(q0, q1, 1.0 / curvature) - - ptype = "" - for t in pathtypeTuple: - if t == 1: - ptype += "L" - elif t == 2: - ptype += "S" - elif t == 3: - ptype += "R" - - return xs, ys, yaw, ptype, clen - - -def main(): - print("Reeds Shepp path planner sample start!!") - - start_x = 1.0 # [m] - start_y = 1.0 # [m] - start_yaw = math.radians(0.0) # [rad] - - end_x = -0.0 # [m] - end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] - - curvature = 1.0 - - px, py, pyaw, mode, clen = reeds_shepp_path_planning( - start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) - - if show_animation: - plt.plot(px, py, label="final course " + str(mode)) - - # plotting - plot_arrow(start_x, start_y, start_yaw) - plot_arrow(end_x, end_y, end_yaw) - - for (ix, iy, iyaw) in zip(px, py, pyaw): - plot_arrow(ix, iy, iyaw, fc="b") - # print(clen) - - plt.legend() - plt.grid(True) - plt.axis("equal") - plt.show() - - -if __name__ == '__main__': - main() diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index e4b01b48..e4cab301 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -6,6 +6,9 @@ author: AtsushiSakai(@Atsushi_twi) """ +import sys +sys.path.append("../ReedsSheppPath/") + import random import math import copy @@ -14,6 +17,8 @@ import reeds_shepp_path_planning import matplotlib.pyplot as plt show_animation = True +STEP_SIZE = 0.1 +curvature = 1.0 class RRT(): @@ -53,10 +58,14 @@ class RRT(): nind = self.GetNearestListIndex(self.nodeList, rnd) newNode = self.steer(rnd, nind) + if newNode is None: + continue if self.CollisionCheck(newNode, self.obstacleList): nearinds = self.find_near_nodes(newNode) newNode = self.choose_parent(newNode, nearinds) + if newNode is None: + continue self.nodeList.append(newNode) self.rewire(newNode, nearinds) @@ -77,6 +86,9 @@ class RRT(): dlist = [] for i in nearinds: tNode = self.steer(newNode, i) + if tNode is None: + continue + if self.CollisionCheck(tNode, self.obstacleList): dlist.append(tNode.cost) else: @@ -103,12 +115,14 @@ class RRT(): return angle def steer(self, rnd, nind): - curvature = 1.0 nearestNode = self.nodeList[nind] px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning( - nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature) + nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature, STEP_SIZE) + + if px is None: + return None newNode = copy.deepcopy(nearestNode) newNode.x = px[-1] @@ -118,7 +132,7 @@ class RRT(): newNode.path_x = px newNode.path_y = py newNode.path_yaw = pyaw - newNode.cost += clen + newNode.cost += sum([abs(c) for c in clen]) newNode.parent = nind return newNode @@ -200,6 +214,8 @@ class RRT(): for i in nearinds: nearNode = self.nodeList[i] tNode = self.steer(nearNode, nnode - 1) + if tNode is None: + continue obstacleOK = self.CollisionCheck(tNode, self.obstacleList) imporveCost = nearNode.cost > tNode.cost diff --git a/PathPlanning/RRTstar/rrt_star.py b/PathPlanning/RRTstar/rrt_star.py index 54660dcd..995992d7 100644 --- a/PathPlanning/RRTstar/rrt_star.py +++ b/PathPlanning/RRTstar/rrt_star.py @@ -65,6 +65,8 @@ class RRT(): # generate coruse lastIndex = self.get_best_last_index() + if lastIndex is None: + return None path = self.gen_final_course(lastIndex) return path diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 26a10a81..4aef9b7f 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -369,7 +369,8 @@ def reeds_shepp_path_planning(sx, sy, syaw, paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size) if len(paths) == 0: - print("No path") + # print("No path") + # print(sx, sy, syaw, gx, gy, gyaw) return None, None, None, None, None minL = float("Inf") @@ -443,7 +444,7 @@ def main(): start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size) if show_animation: - plt.plot(px, py, label="final course " + str(mode)) + # plt.plot(px, py, label="final course " + str(mode)) # plotting plot_arrow(start_x, start_y, start_yaw) diff --git a/tests/test_reeds_shepp_path_planning.py b/tests/test_reeds_shepp_path_planning.py index 86f3adf8..bc4a038c 100644 --- a/tests/test_reeds_shepp_path_planning.py +++ b/tests/test_reeds_shepp_path_planning.py @@ -7,4 +7,3 @@ class Test(TestCase): def test1(self): m.show_animation = False m.main() - m.test() diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index 611568bf..2f88338d 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/tests/test_rrt_star_reeds_shepp.py @@ -2,6 +2,7 @@ from unittest import TestCase import sys sys.path.append("./PathPlanning/RRTStarReedsShepp/") +sys.path.append("./PathPlanning/ReedsSheppPath/") from PathPlanning.RRTStarReedsShepp import rrt_star_reeds_shepp as m