diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index dc7485ad..706b5a49 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -6,8 +6,9 @@ author Atsushi Sakai(@Atsushi_twi) """ import math -import numpy as np + import matplotlib.pyplot as plt +import numpy as np show_animation = True @@ -136,8 +137,8 @@ def LRL(alpha, beta, d): return t, p, q, mode -def dubins_path_planning_from_origin(ex, ey, eyaw, c): - # nomalize +def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE): + # normalize dx = ex dy = ey D = math.sqrt(dx ** 2.0 + dy ** 2.0) @@ -165,12 +166,12 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c): bcost = cost # print(bmode) - px, py, pyaw = generate_course([bt, bp, bq], bmode, c) + px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE) return px, py, pyaw, bmode, bcost -def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): +def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)): """ Dubins path plannner @@ -199,7 +200,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): leyaw = eyaw - syaw lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( - lex, ley, leyaw, c) + lex, ley, leyaw, c, D_ANGLE) px = [math.cos(-syaw) * x + math.sin(-syaw) * y + sx for x, y in zip(lpx, lpy)] @@ -210,7 +211,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): return px, py, pyaw, mode, clen -def generate_course(length, mode, c): +def generate_course(length, mode, c, D_ANGLE): px = [0.0] py = [0.0] @@ -221,7 +222,7 @@ def generate_course(length, mode, c): if m == "S": d = 1.0 * c else: # turning couse - d = np.deg2rad(3.0) + d = D_ANGLE while pd < abs(l - d): # print(pd, l) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 225bfa5d..768fe41d 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -19,7 +19,7 @@ class RRT: Class for RRT planning """ - class Node(): + class Node: """ RRT Node """ @@ -104,6 +104,13 @@ class RRT: new_node.path_x.append(new_node.x) new_node.path_y.append(new_node.y) + d, _ = self.calc_distance_and_angle(new_node, to_node) + if d <= self.path_resolution: + new_node.x = to_node.x + new_node.y = to_node.y + new_node.path_x[-1] = to_node.x + new_node.path_y[-1] = to_node.y + new_node.parent = from_node return new_node @@ -137,9 +144,7 @@ class RRT: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: if node.parent: - plt.plot([node.x, node.parent.x], - [node.y, node.parent.y], - "-g") + plt.plot(node.path_x, node.path_y, "-g") for (ox, oy, size) in self.obstacle_list: plt.plot(ox, oy, "ok", ms=30 * size) diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py index 446033d7..19e68126 100644 --- a/PathPlanning/RRTStar/rrt_star.py +++ b/PathPlanning/RRTStar/rrt_star.py @@ -53,7 +53,7 @@ class RRTStar(RRT): """ self.connect_circle_dist = connect_circle_dist - def planning(self, animation=True, search_until_maxiter=True): + def planning(self, animation=True, search_until_max_iter=True): """ rrt star path planning @@ -63,7 +63,7 @@ class RRTStar(RRT): self.node_list = [self.start] for i in range(self.max_iter): - print(i, len(self.node_list)) + print("Iter:", i, ", number of nodes:", len(self.node_list)) rnd = self.get_random_node() nearest_ind = self.get_nearest_list_index(self.node_list, rnd) new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis) @@ -78,10 +78,10 @@ class RRTStar(RRT): if animation and i % 5 == 0: self.draw_graph(rnd) - if (not search_until_maxiter) and new_node: # check reaching the goal - d, _ = self.calc_distance_and_angle(new_node, self.end) - if d <= self.expand_dis: - return self.generate_final_course(len(self.node_list) - 1) + if (not search_until_max_iter) and new_node: # check reaching the goal + last_index = self.search_best_goal_node() + if last_index: + return self.generate_final_course(last_index) print("reached max iteration") @@ -91,25 +91,6 @@ class RRTStar(RRT): return None - def connect_nodes(self, from_node, to_node): - new_node = self.Node(from_node.x, from_node.y) - d, theta = self.calc_distance_and_angle(new_node, to_node) - - new_node.path_x = [new_node.x] - new_node.path_y = [new_node.y] - - n_expand = math.floor(d / self.path_resolution) - - for _ in range(n_expand): - new_node.x += self.path_resolution * math.cos(theta) - new_node.y += self.path_resolution * math.sin(theta) - new_node.path_x.append(new_node.x) - new_node.path_y.append(new_node.y) - - new_node.parent = from_node - - return new_node - def choose_parent(self, new_node, near_inds): if not near_inds: return None @@ -130,8 +111,8 @@ class RRTStar(RRT): return None min_ind = near_inds[costs.index(min_cost)] + new_node = self.steer(self.node_list[min_ind], new_node) new_node.parent = self.node_list[min_ind] - new_node = self.steer(new_node.parent, new_node) new_node.cost = min_cost return new_node @@ -162,14 +143,14 @@ class RRTStar(RRT): for i in near_inds: near_node = self.node_list[i] edge_node = self.steer(new_node, near_node) - edge_node.cost = self.calc_new_cost(near_node, new_node) + edge_node.cost = self.calc_new_cost(new_node, near_node) no_collision = self.check_collision(edge_node, self.obstacle_list) improved_cost = near_node.cost > edge_node.cost if no_collision and improved_cost: + near_node = edge_node near_node.parent = new_node - near_node.cost = edge_node.cost self.propagate_cost_to_leaves(new_node) def calc_new_cost(self, from_node, to_node): @@ -177,6 +158,7 @@ class RRTStar(RRT): return from_node.cost + d def propagate_cost_to_leaves(self, parent_node): + for node in self.node_list: if node.parent == parent_node: node.cost = self.calc_new_cost(parent_node, node)