From 3cef87ea3fa28df6b1bab1bca9915aa427858453 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 2 Oct 2019 20:19:33 +0900 Subject: [PATCH 1/3] fixed rrt.py --- PathPlanning/RRT/rrt.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 5f7bab54..3bcc6221 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -33,7 +33,7 @@ class RRT: self.parent = None def __init__(self, start, goal, obstacle_list, rand_area, - expand_dis=3.0, path_resolution=1.0, goal_sample_rate=5, max_iter=500): + expand_dis=3.0, path_resolution=0.5, goal_sample_rate=5, max_iter=500): """ Setting Parameter @@ -76,8 +76,9 @@ class RRT: self.draw_graph(rnd_node) if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis: - print("Goal!!") - return self.generate_final_course(len(self.node_list) - 1) + final_node = self.steer(self.node_list[-1], self.end, self.expand_dis) + if self.check_collision(final_node, self.obstacle_list): + return self.generate_final_course(len(self.node_list) - 1) if animation and i % 5: self.draw_graph(rnd_node) @@ -141,7 +142,7 @@ class RRT: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: if node.parent: - plt.plot(node.path_x, node.path_y, "-xg") + plt.plot(node.path_x, node.path_y, "-g") for (ox, oy, size) in self.obstacle_list: self.plot_circle(ox, oy, size) @@ -200,7 +201,8 @@ def main(gx=6.0, gy=10.0): (3, 8, 2), (3, 10, 2), (7, 5, 2), - (9, 5, 2) + (9, 5, 2), + (8, 10, 1) ] # [x, y, radius] # Set Initial parameters rrt = RRT(start=[0, 0], From a5c7096c7befd4c1af423b529d62dd440f105243 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 2 Oct 2019 20:28:59 +0900 Subject: [PATCH 2/3] fixed rrt_with_pathsmoothing.py --- PathPlanning/RRT/rrt_with_pathsmoothing.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index 24b6bfeb..9826dba0 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -76,8 +76,6 @@ def line_collision_check(first, second, obstacleList): if d <= size: return False - # print("OK") - return True # OK @@ -127,7 +125,7 @@ def main(): (7, 5, 2), (9, 5, 2) ] # [x,y,size] - rrt = RRT(start=[0, 0], goal=[5, 10], + rrt = RRT(start=[0, 0], goal=[6, 10], rand_area=[-2, 15], obstacle_list=obstacleList) path = rrt.planning(animation=show_animation) @@ -141,7 +139,7 @@ def main(): plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.plot([x for (x, y) in smoothedPath], [ - y for (x, y) in smoothedPath], '-b') + y for (x, y) in smoothedPath], '-c') plt.grid(True) plt.pause(0.01) # Need for Mac From 295645f4169c4b7031f8cf0cadf3a643d242c50a Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 2 Oct 2019 20:51:22 +0900 Subject: [PATCH 3/3] fixed rrt star.py --- PathPlanning/RRTStar/rrt_star.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py index eb37964d..1c6d6828 100644 --- a/PathPlanning/RRTStar/rrt_star.py +++ b/PathPlanning/RRTStar/rrt_star.py @@ -52,6 +52,7 @@ class RRTStar(RRT): """ self.connect_circle_dist = connect_circle_dist + self.goal_node = self.Node(goal[0], goal[1]) def planning(self, animation=True, search_until_max_iter=True): """ @@ -121,7 +122,14 @@ class RRTStar(RRT): dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list] goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_dis] - if not goal_inds: + # safe_goal_inds = goal_inds + safe_goal_inds = [] + for goal_ind in goal_inds: + t_node = self.steer(self.node_list[goal_ind], self.goal_node) + if self.check_collision(t_node, self.obstacle_list): + safe_goal_inds.append(goal_ind) + + if not safe_goal_inds: return None min_cost = min([self.node_list[i].cost for i in goal_inds]) @@ -177,12 +185,13 @@ def main(): (3, 8, 2), (3, 10, 2), (7, 5, 2), - (9, 5, 2) + (9, 5, 2), + (8, 10, 1), ] # [x,y,size(radius)] # Set Initial parameters rrt_star = RRTStar(start=[0, 0], - goal=[10, 10], + goal=[6, 10], rand_area=[-2, 15], obstacle_list=obstacle_list) path = rrt_star.planning(animation=show_animation)