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()