From 39a918a2020ea6deb91ed5fc859387955b4f242f Mon Sep 17 00:00:00 2001 From: luym11 Date: Tue, 18 Dec 2018 14:48:18 +0100 Subject: [PATCH] fixed steer() in RRTstar --- PathPlanning/RRTstar/rrt_star.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/PathPlanning/RRTstar/rrt_star.py b/PathPlanning/RRTstar/rrt_star.py index 995992d7..1a5978e6 100644 --- a/PathPlanning/RRTstar/rrt_star.py +++ b/PathPlanning/RRTstar/rrt_star.py @@ -102,12 +102,16 @@ class RRT(): # expand tree nearestNode = self.nodeList[nind] theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) - newNode = copy.deepcopy(nearestNode) - newNode.x += self.expandDis * math.cos(theta) - newNode.y += self.expandDis * math.sin(theta) - - newNode.cost += self.expandDis - newNode.parent = nind + newNode = Node(rnd[0], rnd[1]) + currentDistance = math.sqrt( (rnd[1] - nearestNode.y) ** 2 + (rnd[0] - nearestNode.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 def get_random_point(self):