def set_parent(self, new_node, near_nodes): # connects new_node along a minimum cost path if not near_nodes: near_nodes.append(self.nearest_node(new_node)) cost_list = [] for near_node in near_nodes: # CALL TO LOCAL PATH PLANNER p1 = time.time() px, py, pangle, mode, plength, u = plan.dubins_path_planning(near_node.pose[0], near_node.pose[1], near_node.pose[2], new_node.pose[0], new_node.pose[1], new_node.pose[2], self.max_curvature) p2 = time.time() self.local_planner_time += (p2 - p1) cost = self.cost(px, py, pangle, plength) if self.check_collision_path(px, py) and self.max_dist >= plength + near_node.dist: cost_list.append(near_node.cost + cost) else: cost_list.append(float("inf")) mincost = min(cost_list) min_node = near_nodes[cost_list.index(mincost)] if mincost == float("inf"): # self.draw_near(near_nodes, new_node, new_node) return new_node.cost = mincost new_node.parent = min_node # CALL TO LOCAL PATH PLANNER p1 = time.time() px, py, pangle, mode, plength, u = plan.dubins_path_planning(min_node.pose[0], min_node.pose[1], min_node.pose[2], new_node.pose[0], new_node.pose[1], new_node.pose[2], self.max_curvature) p2 = time.time() self.local_planner_time += (p2 - p1) new_node.path_x = px new_node.path_y = py new_node.path_angle = pangle new_node.u = u new_node.dist = new_node.parent.dist + plength
def set_parent(self, new_node, near_nodes): # connects new_node along a minimum cost path if not near_nodes: near_nodes.append(self.nearest_node(new_node)) cost_list = [] for near_node in near_nodes: # CALL TO LOCAL PATH PLANNER px, py, pangle, mode, plength, u = plan.dubins_path_planning(near_node.pose[0], near_node.pose[1], near_node.pose[2], new_node.pose[0], new_node.pose[1], new_node.pose[2], self.max_curvature) path_var = self.path_var(px, py, pangle) if self.check_collision_path(px, py) and self.max_dist >= plength + near_node.dist: cost_list.append((near_node.total_var + path_var) / (near_node.dist + plength)) else: cost_list.append(float("inf")) mincost = min(cost_list) min_node = near_nodes[cost_list.index(mincost)] if mincost == float("inf"): # No parent could be found return new_node.parent = min_node # CALL TO LOCAL PATH PLANNER px, py, pangle, mode, plength, u = plan.dubins_path_planning(min_node.pose[0], min_node.pose[1], min_node.pose[2], new_node.pose[0], new_node.pose[1], new_node.pose[2], self.max_curvature) new_node.path_x = px new_node.path_y = py new_node.path_angle = pangle new_node.total_var = new_node.parent.total_var + self.path_var(px, py, pangle) new_node.u = u new_node.dist = new_node.parent.dist + plength
def set_parent(self, new_node, nearest_node): # connects new_node to nearest node and tracks cost px, py, pangle, mode, plength, u = plan.dubins_path_planning(nearest_node.pose[0], nearest_node.pose[1], nearest_node.pose[2], new_node.pose[0], new_node.pose[1], new_node.pose[2], self.max_curvature) new_node.parent = nearest_node new_node.path_x = px new_node.path_y = py new_node.path_angle = pangle new_node.total_var = new_node.parent.total_var + self.path_var(px, py, pangle) new_node.u = u new_node.dist = new_node.parent.dist + plength
def local_path(self, source_node, destination_node): # take source_node and find path to destination_node px, py, pyaw, mode, clen, u = plan.dubins_path_planning(source_node.pose[0], source_node.pose[1], source_node.pose[2], destination_node.pose[0], destination_node.pose[1], destination_node.pose[2], self.max_curvature) new_node = copy.deepcopy(source_node) new_node.pose = destination_node.pose new_node.path_x = px new_node.path_y = py new_node.path_yaw = pyaw new_node.u = u new_node.cost = np.sum(u) new_node.dist += clen new_node.parent = source_node return new_node
def rewire(self, new_node, near_nodes): for near_node in near_nodes: p1 = time.time() px, py, pangle, mode, plength, u = plan.dubins_path_planning(new_node.pose[0], new_node.pose[1], new_node.pose[2], near_node.pose[0], near_node.pose[1], near_node.pose[2], self.max_curvature) p2 = time.time() self.local_planner_time += (p2 - p1) avg_var_per_length = (new_node.total_var + self.path_var(px, py, pangle)) / (new_node.dist + plength) if near_node.dist != 0: if near_node.total_var / near_node.dist > avg_var_per_length and self.max_dist >= new_node.dist + plength \ and self.check_loop(near_node, new_node): if self.check_collision_path(px, py): near_node.parent = new_node near_node.path_x = px near_node.path_y = py near_node.path_angle = pangle near_node.u = u near_node.total_var = near_node.parent.total_var + self.path_var(px, py, pangle) near_node.dist = near_node.parent.dist + plength self.propagate_update_to_children(near_node)
def local_path(self, source_node, destination_node): # take source_node and find path to destination_node time1 = time.time() px, py, pangle, mode, plength, u = plan.dubins_path_planning(source_node.pose[0], source_node.pose[1], source_node.pose[2], destination_node.pose[0], destination_node.pose[1], destination_node.pose[2], self.max_curvature) self.local_planner_time += time.time() - time1 new_node = copy.deepcopy(source_node) new_node.pose = destination_node.pose new_node.path_x = px new_node.path_y = py new_node.path_angle = pangle new_node.u = u new_node.path_dist = plength new_node.dist += plength new_node.path_var = self.path_var(px, py, pangle) new_node.total_var += new_node.path_var new_node.cost = new_node.total_var / new_node.dist new_node.parent = source_node return new_node
def rewire(self, new_node, near_nodes): for near_node in near_nodes: p1 = time.time() px, py, pangle, mode, plength, u = plan.dubins_path_planning(new_node.pose[0], new_node.pose[1], new_node.pose[2], near_node.pose[0], near_node.pose[1], near_node.pose[2], self.max_curvature) p2 = time.time() self.local_planner_time += (p2 - p1) temp_cost = new_node.cost + self.cost(px, py, pangle, plength) if near_node.cost > temp_cost and self.max_dist >= new_node.dist + plength \ and self.check_loop(near_node, new_node): if self.check_collision_path(px, py): #self.draw_near(near_nodes, new_node, near_node) near_node.parent = new_node near_node.cost = temp_cost near_node.path_x = px near_node.path_y = py near_node.path_angle = pangle near_node.u = u near_node.dist = near_node.parent.dist + plength self.propagate_update_to_children(near_node)
import matplotlib.pyplot as plt import numpy as np from control_algorithms.base import dubins_path_planner as plan max_curvature = 1.0 pose1 = np.array([0, 0, 0]) pose2 = np.array([5, 5, 3]) pose3 = np.array([11, 3, 2]) px1, py1, pangle1, mode1, plength1, u1 = plan.dubins_path_planning( pose1[0], pose1[1], pose1[2], pose2[0], pose2[1], pose2[2], max_curvature) px2, py2, pangle2, mode2, plength2, u2 = plan.dubins_path_planning( pose1[0], pose1[1], pose1[2], pose3[0], pose3[1], pose3[2], max_curvature) print(len(px1)) print(len(px2)) plt.plot(px1, py1, color='green') plt.plot(px2, py2, color='green') plt.show()