From 1ee4ceae0a7745c6614ed4ffe3ffca7647796d58 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 19 Jun 2019 22:25:54 +0900 Subject: [PATCH] add control flag for search until maxiter --- PathPlanning/RRTstar/rrt_star.py | 111 ++++++++++++++++--------------- 1 file changed, 58 insertions(+), 53 deletions(-) diff --git a/PathPlanning/RRTstar/rrt_star.py b/PathPlanning/RRTstar/rrt_star.py index a2064a21..169ead82 100644 --- a/PathPlanning/RRTstar/rrt_star.py +++ b/PathPlanning/RRTstar/rrt_star.py @@ -1,3 +1,6 @@ + + + """ Path Planning Sample Code with RRT* @@ -14,6 +17,15 @@ import matplotlib.pyplot as plt show_animation = True +class Node(): + + def __init__(self, x, y): + self.x = x + self.y = y + self.cost = 0.0 + self.parent = None + + class RRT(): """ Class for RRT Planning @@ -39,11 +51,12 @@ class RRT(): self.maxIter = maxIter self.obstacleList = obstacleList - def Planning(self, animation=True): + def Planning(self, animation=True, search_until_maxiter=True): """ - Pathplanning + rrt path planning animation: flag for animation on or off + search_until_maxiter: search until max iteration for path improving or not """ self.nodeList = [self.start] @@ -51,35 +64,39 @@ class RRT(): rnd = self.get_random_point() nind = self.GetNearestListIndex(self.nodeList, rnd) - newNode = self.steer(rnd, nind) - # print(newNode.cost) + new_node = self.steer(rnd, nind) - if self.__CollisionCheck(newNode, self.obstacleList): - nearinds = self.find_near_nodes(newNode) - newNode = self.choose_parent(newNode, nearinds) - self.nodeList.append(newNode) - self.rewire(newNode, nearinds) + if self.__CollisionCheck(new_node, self.obstacleList): + nearinds = self.find_near_nodes(new_node) + new_node = self.choose_parent(new_node, nearinds) + self.nodeList.append(new_node) + self.rewire(new_node, nearinds) if animation and i % 5 == 0: self.DrawGraph(rnd) - # generate coruse - lastIndex = self.get_best_last_index() - if lastIndex is None: - continue - path = self.gen_final_course(lastIndex) - return path + # generate course + if not search_until_maxiter: + lastIndex = self.get_best_last_index() + if lastIndex: + return self.gen_final_course(lastIndex) + + print("reached max iteration") + + lastIndex = self.get_best_last_index() + if lastIndex: + return self.gen_final_course(lastIndex) return None - def choose_parent(self, newNode, nearinds): + def choose_parent(self, new_node, nearinds): if not nearinds: - return newNode + return new_node dlist = [] for i in nearinds: - dx = newNode.x - self.nodeList[i].x - dy = newNode.y - self.nodeList[i].y + dx = new_node.x - self.nodeList[i].x + dy = new_node.y - self.nodeList[i].y d = math.sqrt(dx ** 2 + dy ** 2) theta = math.atan2(dy, dx) if self.check_collision_extend(self.nodeList[i], theta, d): @@ -92,30 +109,30 @@ class RRT(): if mincost == float("inf"): print("mincost is inf") - return newNode + return new_node - newNode.cost = mincost - newNode.parent = minind + new_node.cost = mincost + new_node.parent = minind - return newNode + return new_node def steer(self, rnd, nind): # expand tree - nearestNode = self.nodeList[nind] - theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) - newNode = Node(rnd[0], rnd[1]) + nearest_node = self.nodeList[nind] + theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x) + new_node = Node(rnd[0], rnd[1]) currentDistance = math.sqrt( - (rnd[1] - nearestNode.y) ** 2 + (rnd[0] - nearestNode.x) ** 2) + (rnd[1] - nearest_node.y) ** 2 + (rnd[0] - nearest_node.x) ** 2) # Find a point within expandDis of nind, and closest to rnd if currentDistance <= self.expandDis: pass else: - newNode.x = nearestNode.x + self.expandDis * math.cos(theta) - newNode.y = nearestNode.y + self.expandDis * math.sin(theta) - newNode.cost = float("inf") - newNode.parent = None - return newNode + new_node.x = nearest_node.x + self.expandDis * math.cos(theta) + new_node.y = nearest_node.y + self.expandDis * math.sin(theta) + new_node.cost = float("inf") + new_node.parent = None + return new_node def get_random_point(self): @@ -155,25 +172,24 @@ class RRT(): def calc_dist_to_goal(self, x, y): return np.linalg.norm([x - self.end.x, y - self.end.y]) - def find_near_nodes(self, newNode): + def find_near_nodes(self, new_node): nnode = len(self.nodeList) r = 50.0 * math.sqrt((math.log(nnode) / nnode)) - # r = self.expandDis * 5.0 - dlist = [(node.x - newNode.x) ** 2 + - (node.y - newNode.y) ** 2 for node in self.nodeList] + dlist = [(node.x - new_node.x) ** 2 + + (node.y - new_node.y) ** 2 for node in self.nodeList] nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] return nearinds - def rewire(self, newNode, nearinds): + def rewire(self, new_node, nearinds): nnode = len(self.nodeList) for i in nearinds: nearNode = self.nodeList[i] - dx = newNode.x - nearNode.x - dy = newNode.y - nearNode.y + dx = new_node.x - nearNode.x + dy = new_node.y - nearNode.y d = math.sqrt(dx ** 2 + dy ** 2) - scost = newNode.cost + d + scost = new_node.cost + d if nearNode.cost > scost: theta = math.atan2(dy, dx) @@ -232,18 +248,6 @@ class RRT(): return True # safe -class Node(): - """ - RRT Node - """ - - def __init__(self, x, y): - self.x = x - self.y = y - self.cost = 0.0 - self.parent = None - - def main(): print("Start " + __file__) @@ -260,7 +264,7 @@ def main(): # Set Initial parameters rrt = RRT(start=[0, 0], goal=[10, 10], randArea=[-2, 15], obstacleList=obstacleList) - path = rrt.Planning(animation=show_animation) + path = rrt.Planning(animation=show_animation, search_until_maxiter=False) if path is None: print("Cannot find path") @@ -278,3 +282,4 @@ def main(): if __name__ == '__main__': main() +