def search_best_goal_node(self, scan):
dist_to_goal_list = [
math.sqrt((n.x - self.goal.x)**2 + (n.y - self.goal.y)**2)
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
]
safe_goal_inds = []
for goal_ind in goal_inds:
theta = math.atan2(self.node_list[goal_ind].y - self.goal.y,
self.node_list[goal_ind].x - self.goal.x)
t_node = Node(self.node_list[goal_ind].x + math.cos(theta),
self.node_list[goal_ind].y + math.sin(theta))
if scan_obstacle_checker(scan, (t_node.x, t_node.y)):
safe_goal_inds.append(goal_ind)
if not safe_goal_inds:
return None
min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
for i in safe_goal_inds:
if self.node_list[i].cost == min_cost:
return i
return None
def __call__(self, start_point, goal_point, scan, animation=False):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point: tuple with start point coordinates.
end_point: tuple with end point coordinates.
scan_list:LaserScan polar distances to Obstacles [r1,r2,r3...] initially assuming every scan occurs at 1 rad interval
animation: flag for showing planning visualization (default False)
Returns:
A list of points representing the path determined from
start to goal while avoiding obstacles.
An list containing just the start point means path could not be planned.
"""
#Make line obstacles and scan in x,y from scan_list
line_obstacles, pts = make_obstacles_scan(scan)
# Initialize start and goal nodes
start = Node.from_coordinates(start_point)
goal_node = Node.from_coordinates(goal_point)
# Initialize node_list with start
node_list = [start]
# Calculate distances between start and goal
del_x, del_y = start.x - goal_node.x, start.y - goal_node.y
distance_to_goal = math.sqrt(del_x**2 + del_y**2)
# Loop to keep expanding the tree towards goal if there is no direct connection
if check_intersection_scan([start_point, goal_point], line_obstacles):
while True:
# Sample random point in specified area
rnd_point = self.sampler(self.sample_area, goal_point,
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [
(node.x - rnd_point[0])**2 + (node.y - rnd_point[1])**2
for node in node_list
]
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(rnd_point[1] - nearest_node.y,
rnd_point[0] - nearest_node.x)
new_point = nearest_node.x + self.expand_dis*math.cos(theta), \
nearest_node.y + self.expand_dis*math.sin(theta)
# Check whether new point is inside an obstacles
new_point = scan_obstacle_checker(scan, new_point)
# Expand tree
if math.isnan(new_point[0]):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
del_x, del_y = new_node.x - goal_node.x, new_node.y - goal_node.y
distance_to_goal = math.sqrt(del_x**2 + del_y**2)
if distance_to_goal < self.expand_dis or not check_intersection_scan(\
[new_node.to_tuple(), goal_node.to_tuple()], line_obstacles):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start
node_list = [start, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while node_list[node_list.index(last_node)].parent is not None:
node = node_list[node_list.index(last_node)]
path.append(node.to_tuple())
last_node = node.parent
path.append(start.to_tuple())
if animation == True:
RRT.visualize_tree(node_list, obstacle_list)
return path, node_list
def __call__(self, goal_point, scan, start_point=[0, 0], animation=True):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point: tuple with start point coordinates.
end_point: tuple with end point coordinates.
scan: list of obstacles which themselves are list of points
animation: flag for showing planning visualization (default False)
Returns:
A list of points representing the path determined from
start to goal while avoiding obstacles.
An list containing just the start point means path could not be planned.
"""
search_until_max_iter = True
# Make line obstacles and scan in x,y from scan
line_obstacles, pts = make_obstacles_scan(scan)
# Setting Start and End
self.start = Node(start_point[0], start_point[1])
self.goal = Node(goal_point[0], goal_point[1])
# Initialize node with Starting Position
self.node_list = [self.start]
# Loop for maximum iterations to get the best possible path
for iter in range(self.max_iter):
########################################
# print("ITER-->")
# print(iter)
########################################
#########################################
# print("NODE_LIST-->")
# for printer_i in self.node_list:
# print(printer_i)
#########################################
# Sample a Random point in the sample area
rnd_point = sampler(self.sample_area, (self.goal.x, self.goal.y),
self.goal_sample_rate)
########################################
# print("RANDOM POINT-->")
# print(rnd_point)
########################################
# Find nearest node to the sampled point
distance_list = [
(node.x - rnd_point[0])**2 + (node.y - rnd_point[1])**2
for node in self.node_list
]
nearest_node = self.node_list[distance_list.index(
min(distance_list))]
########################################
# print("NEAREST_NODE-->")
# print(nearest_node.x , nearest_node.y , nearest_node.cost)
########################################
# Creating a new Point in the Direction of sampled point
theta = math.atan2(rnd_point[1] - nearest_node.y,
rnd_point[0] - nearest_node.x)
new_point = nearest_node.x + self.expand_dis*math.cos(theta), \
nearest_node.y + self.expand_dis*math.sin(theta)
#########################################
# print("NEW_POINT-->")
# print(new_point[0],new_point[1])
#########################################
# Check obstacle collision
new_point = scan_obstacle_checker(scan, new_point)
if math.isnan(new_point[0]):
########################################
#print("ISNAN-->"+str(iter))
# print(new_point)
########################################
continue
#If iterations is less than certain no. try exploring a bit
if iter < 20:
new_node = Node(new_point[0], new_point[1])
new_node.parent = nearest_node
new_node.cost = nearest_node.cost + math.sqrt(
(new_node.x - nearest_node.x)**2 +
(new_node.y - nearest_node.y)**2)
#Set the path for new node
present_node = new_node
px = []
py = []
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
new_node.path_x = px[:]
new_node.path_y = py[:]
self.node_list.append(new_node)
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
continue
nearest_indexes = find_near_nodes(self.node_list, new_point,
self.circle)
# Getting the parent node from nearest indices
costs = [
] # List of Total costs from the start to new_node when attached to parent node in node_list
temp_points = []
for index in nearest_indexes:
near_node = self.node_list[index]
point_list = [(near_node.x, near_node.y),
(new_point[0], new_point[1])]
if not check_intersection_scan(point_list, line_obstacles):
costs.append(near_node.cost +
math.sqrt((near_node.x - new_point[0])**2 +
(near_node.y - new_point[1])**2))
else:
costs.append(float("inf"))
min_cost = min(costs)
# Calculating the minimum cost and selecting the node for which it occurs as parent child
if min_cost == float("inf"):
continue
# Setting the new node as the one with min cost
min_ind = nearest_indexes[costs.index(min_cost)]
new_node = Node(new_point[0], new_point[1])
new_node.parent = self.node_list[min_ind]
new_node.cost = min_cost
#########################################
# print("NEW_NODE-->")
# print(new_node.x , new_node.y , new_node.cost)
#########################################
if new_node:
self.node_list.append(new_node)
for ind in nearest_indexes:
node_check = self.node_list[ind]
point_list = [(new_node.x, new_node.y),
(node_check.x, node_check.y)]
no_coll = not check_intersection_scan(
point_list, line_obstacles)
cost_improv = new_node.cost + math.sqrt(
(new_node.x - node_check.x)**2 +
(new_node.y - node_check.y)**2) < node_check.cost
if no_coll and cost_improv:
node_check.parent = new_node
present_node = new_node
px = []
py = []
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
new_node.path_x = px[:]
new_node.path_y = py[:]
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
if (not search_until_max_iter
) and new_node: # check reaching the goal
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
return None
def __call__(self, goal_point, scan, start_point=[0, 0], animation=False):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point: tuple with start point coordinates.
end_point: tuple with end point coordinates.
scan: list of obstacles which themselves are list of points
animation: flag for showing planning visualization (default False)
Returns:
A list of points representing the path determined from
start to goal while avoiding obstacles.
An list containing just the start point means path could not be planned.
"""
# Make line obstacles and scan in x,y from scan
line_obstacles, _ = make_obstacles_scan(scan)
# Setting Start and End
self.start = Node(start_point[0], start_point[1])
self.goal = Node(goal_point[0], goal_point[1])
# Initialize node with Starting Position
self.node_list = [self.start]
# Loop for maximum iterations to get the best possible path
for iter in range(self.max_iter):
# Sample a Random point in the sample area
rnd_point = sampler(self.sample_area, (self.goal.x, self.goal.y),
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [
(node.x - rnd_point[0])**2 + (node.y - rnd_point[1])**2
for node in self.node_list
]
nearest_node = self.node_list[distance_list.index(
min(distance_list))]
# Creating a new Point in the Direction of sampled point
theta = math.atan2(rnd_point[1] - nearest_node.y,
rnd_point[0] - nearest_node.x)
new_point = nearest_node.x + self.expand_dis*math.cos(theta), \
nearest_node.y + self.expand_dis*math.sin(theta)
# Check obstacle collision
new_point = scan_obstacle_checker(scan, new_point)
if math.isnan(new_point[0]):
continue
###############################################################################################################
#THIS WILL ONLY WORK FOR SOME INITIAL ITERATIONS
#If iterations is less than certain no. try exploring a bit, run similar to RRT
if iter < self.initial_explore:
new_node = Node(new_point[0], new_point[1])
new_node.parent = nearest_node
new_node.cost = nearest_node.cost + math.sqrt(
(new_node.x - nearest_node.x)**2 +
(new_node.y - nearest_node.y)**2)
#Set the path for new node
present_node = new_node
px = [] #X-coordinate path
py = [] #Y-coordinate path
#Keep on appending path until reaches start
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
#Setting Path
new_node.path_x = px[:]
new_node.path_y = py[:]
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
continue
###############################################################################################################
###############################################################################################################
#FINDING NEAREST INDICES
nnode = len(self.node_list) + 1
#The circle in which to check parent node and rewiring
r = self.circle * math.sqrt((math.log(nnode) / nnode))
dist_list = [
(node.x - new_point[0])**2 + (node.y - new_point[1])**2
for node in self.node_list
]
#Getting all the indexes within r units of new_node
nearest_indexes = [
dist_list.index(i) for i in dist_list if i <= r**2
]
###############################################################################################################
###############################################################################################################
#GETTING THE PARENT NODE FROM NEAREST INDICES FOR BEST PARENT WITH LEAST COST
costs = [
] # List of Total costs from the start to new_node when attached to parent node in node_list
for index in nearest_indexes:
near_node = self.node_list[index]
point_list = [(near_node.x, near_node.y),
(new_point[0], new_point[1])]
if not check_intersection_scan(point_list, line_obstacles):
costs.append(near_node.cost +
math.sqrt((near_node.x - new_point[0])**2 +
(near_node.y - new_point[1])**2))
else:
costs.append(float("inf"))
# If costs is empty continue
try:
min_cost = min(costs)
except:
continue
# Calculating the minimum cost and selecting the node for which it occurs as parent child
if min_cost == float("inf"):
continue
# Setting the new node as the one with min cost
min_ind = nearest_indexes[costs.index(min_cost)]
new_node = Node(new_point[0], new_point[1])
new_node.parent = self.node_list[min_ind]
new_node.cost = min_cost
###############################################################################################################
###############################################################################################################
#REWIRING
if new_node:
#First append the node to nodelist
self.node_list.append(new_node)
#Rewiring
for ind in nearest_indexes:
#Check for Every Nearest Node in node_list the possibility of rewiring to new node
node_check = self.node_list[ind]
point_list = [(new_node.x, new_node.y),
(node_check.x, node_check.y)]
#Check if the straight line from new_node to node_check is collision free, all others will automatically be collision free
no_coll = not check_intersection_scan(
point_list, line_obstacles)
#Check for Cost improvement
cost_improv = new_node.cost + math.sqrt(
(new_node.x - node_check.x)**2 +
(new_node.y - node_check.y)**2) < node_check.cost
#If both the above conditions are met, set the parent node of node check to new node
if no_coll and cost_improv:
node_check.parent = new_node
###############################################################################################################
###############################################################################################################
#SETTING PATH THE NODE
present_node = new_node
px = []
py = []
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
new_node.path_x = px[:]
new_node.path_y = py[:]
###############################################################################################################
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
###############################################################################################################
#TO PREEMPT BEFORE REACHING MAX ITERATIONS, ONCE GOAL FOUND
if (not self.search_until_max_iter
) and new_node: # check reaching the goal
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
###############################################################################################################
###############################################################################################################
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
return None