def astar(self, habitat_list, obs_lst, boundary_list, start, pathLenLimit, weights):
"""
Find the optimal path from start to goal avoiding given obstacles
Parameter:
obs_lst - a list of motion_plan_state objects that represent obstacles
start - a tuple of two elements: x and y coordinates
goal - a tuple of two elements: x and y coordinates
"""
w1 = weights[0]
w2 = weights[1]
w3 = weights[2]
habitats_time_spent = 0
cal_cost = Cost()
start_node = Node(None, start)
start_node.g = start_node.h = start_node.f = 0
cost = []
open_list = [] # hold neighbors of the expanded nodes
closed_list = [] # hold all the exapnded nodes
# habitats = habitat_list[:]
open_list.append(start_node)
while len(open_list) > 0:
current_node = open_list[0] # initialize the current node
current_index = 0
for index, item in enumerate(open_list): # find the current node with the smallest f
if item.f < current_node.f:
current_node = item
current_index = index
open_list.pop(current_index)
closed_list.append(current_node)
if abs(current_node.pathLen - pathLenLimit) <= 10: # terminating condition
path = []
current = current_node
while current is not None: # backtracking to find the d
path.append(current.position)
cost.append(current.cost)
habitats_time_spent += self.habitats_time_spent(current)
current = current.parent
path_mps = []
for point in path:
mps = Motion_plan_state(point[0], point[1])
path_mps.append(mps)
trajectory = path_mps[::-1]
# smoothPath = self.smoothPath(trajectory, habitats)
return ({"path" : trajectory, "cost list" : cost, "cost" : cost[0]})
current_neighbors = self.curr_neighbors(current_node, boundary_list)
children = []
for neighbor in current_neighbors: # create new node if the neighbor is collision-free
if self.collision_free(neighbor, obs_lst):
new_node = Node(current_node, neighbor)
self.update_habitat_coverage(new_node, self.habitat_open_list, self.habitat_closed_list)
cost_of_edge = cal_cost.cost_of_edge(new_node, self.habitat_open_list, self.habitat_closed_list, weights)
new_node.cost = new_node.parent.cost + cost_of_edge[0]
children.append(new_node)
for child in children:
if child in closed_list:
continue
habitatInfo = cal_cost.cost_of_edge(child, self.habitat_open_list, self.habitat_closed_list, weights)
insideAnyHabitats = habitatInfo[1]
insideOpenHabitats = habitatInfo[2]
child.g = child.parent.cost - w2 * insideAnyHabitats - w3 * insideOpenHabitats
child.cost = child.g
child.h = - w2 * abs(pathLenLimit - child.pathLen) - w3 * len(self.habitat_open_list)
child.f = child.g + child.h
child.pathLen = child.parent.pathLen + euclidean_dist(child.parent.position, child.position)
child.time_stamp = int(child.pathLen/self.velocity)
# check if child exists in the open list and have bigger g
for open_node in open_list:
if child == open_node and child.g > open_node.g:
continue
x_pos, y_pos = self.get_indices(child.position[0], child.position[1])
if self.visited_nodes[x_pos, y_pos] == 0:
open_list.append(child)
self.visited_nodes[x_pos, y_pos] = 1
def astar(self, habitat_list, obs_lst, boundary_list, start, goal,
weights):
"""
Find the optimal path from start to goal avoiding given obstacles
Parameter:
obs_lst - a list of motion_plan_state objects that represent obstacles
start - a tuple of two elements: x and y coordinates
goal - a tuple of two elements: x and y coordinates
"""
habitats_time_spent = 0
cal_cost = Cost()
path_length = 0
start_node = Node(None, start)
start_node.g = start_node.h = start_node.f = 0
end_node = Node(None, goal)
end_node.g = end_node.h = end_node.f = 0
dynamic_path = [] # append new node as a* search goes
cost = []
open_list = [] # hold neighbors of the expanded nodes
closed_list = [] # hold all the exapnded nodes
habitat_open_list = habitat_list # hold haibitats that have not been explored
habitat_closed_list = [] # hold habitats that have been explored
open_list.append(start_node)
while len(open_list) > 0:
current_node = open_list[0] # initialize the current node
current_index = 0
for index, item in enumerate(
open_list
): # find the current node with the smallest f(f=g+h)
if item.f < current_node.f:
current_node = item
current_index = index
open_list.pop(current_index)
closed_list.append(current_node)
dynamic_path.append(current_node.position)
print("dynamic path: ", dynamic_path)
# print ('dynamic_path: ', dynamic_path)
if self.near_goal(current_node.position, end_node.position):
path = []
current = current_node
while current is not None: # backtracking to find the path
path.append(current.position)
cost.append(current.cost)
# if current.parent is not None:
# current.cost = current.parent.cost + cal_cost.cost_of_edge(current, path, habitat_open_list, habitat_closed_list, weights=[1, 1, 1])
path_length += 10
habitats_time_spent += self.habitats_time_spent(current)
# print ("\n", "habitats_time_spent: ", habitats_time_spent)
current = current.parent
path_mps = []
for point in path:
mps = Motion_plan_state(point[0], point[1])
path_mps.append(mps)
# print ("\n", 'actual path length: ', path_length)
# print ("\n", 'time spent in habitats: ', habitats_time_spent)
# print ("\n", "cost list: ", cost)
# print ("\n", 'cost list length: ', len(cost))
# print ("\n", 'path list length: ', len(path_mps))
return ([path_mps[::-1], cost])
# find close neighbors of the current node
current_neighbors = self.curr_neighbors(current_node,
boundary_list)
print("\n", "current neighbors: ", current_neighbors)
# make current neighbors Nodes
children = []
grid = [] # holds a list of tuples (neighbor, grid value)
for neighbor in current_neighbors: # create new node if the neighbor is collision-free
if self.check_collision(neighbor, obs_lst):
"""
SELECTION
"""
grid_val = self.create_grid_map(current_node, neighbor)
grid.append((neighbor, grid_val))
print("\n", "grid value: ", grid_val)
print("\n", 'grid list: ', grid)
# remove two elements with the lowest grid values
grid.remove(min(grid))
print("selected grid list: ", grid)
# grid.remove(min(grid))
# append the selected neighbors to children
for neighbor in grid:
new_node = Node(current_node, neighbor[0])
children.append(new_node)
# update habitat_open_list and habitat_closed_list
result = self.check_cover_habitats(new_node, habitat_open_list,
habitat_closed_list)
habitat_open_list = result[0]
# print ("habitat_open_list: ", habitat_open_list)
habitat_closed_list = result[1]
# print ("habitat_closed_list: ", habitat_closed_list)
for child in children:
if child in closed_list:
continue
# dist_child_current = self.euclidean_dist(child.position, current_node.position)
if child.parent is not None:
result = cal_cost.cost_of_edge(child,
dynamic_path,
habitat_open_list,
habitat_closed_list,
weights=weights)
child.cost = child.parent.cost + result[0]
# child.g = current_node.g + dist_child_current
child.g = current_node.cost
child.h = weights[0] * self.euclidean_dist(
child.position,
goal) - weights[1] * result[1] # result=(cost, d_2)
child.f = child.g + child.h
# check if child exists in the open list and have bigger g
for open_node in open_list:
if child == open_node and child.g > open_node.g:
continue
open_list.append(child)
def astar(self, habitat_list, obs_lst, boundary_list, start, pathLenLimit,
weights):
"""
Find the optimal path from start to goal avoiding given obstacles
Parameter:
obs_lst - a list of motion_plan_state objects that represent obstacles
start - a tuple of two elements: x and y coordinates
goal - a tuple of two elements: x and y coordinates
"""
w1 = weights[0]
w2 = weights[1]
w3 = weights[2]
habitats_time_spent = 0
cal_cost = Cost()
start_node = Node(None, start)
start_node.g = start_node.h = start_node.f = 0
cost = []
open_list = [] # hold neighbors of the expanded nodes
closed_list = [] # hold all the exapnded nodes
habitat_open_list = habitat_list # hold haibitats that have not been explored
habitat_closed_list = [] # hold habitats that have been explored
open_list.append(start_node)
while len(open_list) > 0:
current_node = open_list[0] # initialize the current node
current_index = 0
for index, item in enumerate(
open_list): # find the current node with the smallest f
if item.f < current_node.f:
current_node = item
current_index = index
open_list.pop(current_index)
closed_list.append(current_node)
if abs(current_node.pathLen -
pathLenLimit) <= 10: # terminating condition
path = []
current = current_node
while current is not None: # backtracking to find the d
path.append(current.position)
cost.append(current.cost)
habitats_time_spent += self.habitats_time_spent(current)
current = current.parent
path_mps = []
for point in path:
mps = Motion_plan_state(point[0], point[1])
path_mps.append(mps)
return ([path_mps[::-1], cost])
current_neighbors = self.curr_neighbors(current_node,
boundary_list)
children = []
grid = []
'''Old Cost Function'''
for neighbor in current_neighbors: # create new node if the neighbor is collision-free
if self.collision_free(neighbor, obs_lst):
new_node = Node(current_node, neighbor)
result = self.update_habitat_coverage(
new_node, habitat_open_list, habitat_closed_list
) # update habitat_open_list and habitat_closed_list
habitat_open_list = result[0]
habitat_closed_list = result[1]
cost_of_edge = cal_cost.cost_of_edge(
new_node, habitat_open_list, habitat_closed_list,
weights)
new_node.cost = new_node.parent.cost + cost_of_edge[0]
children.append(new_node)
'''New Cost Function'''
# for neighbor in current_neighbors: # create new node if the neighbor is collision-free
# if self.collision_free(neighbor, obs_lst):
# """
# SELECTION
# """
# grid_val = self.create_grid_map(current_node, neighbor)
# grid.append((neighbor, grid_val))
# grid.remove(min(grid))
# append the selected neighbors to children
for neighbor in grid:
new_node = Node(current_node, neighbor[0])
children.append(new_node)
result = self.update_habitat_coverage(
new_node, habitat_open_list, habitat_closed_list
) # update habitat_open_list and habitat_closed_list
habitat_open_list = result[0]
habitat_closed_list = result[1]
for child in children:
if child in closed_list:
continue
result = cal_cost.cost_of_edge(child, habitat_open_list,
habitat_closed_list, weights)
d_2 = result[1]
d_3 = result[2]
child.g = child.parent.cost - w2 * d_2 - w3 * d_3
child.cost = child.g
child.h = -w2 * abs(pathLenLimit - child.pathLen) - w3 * len(
habitat_open_list)
child.f = child.g + child.h
child.pathLen = child.parent.pathLen + euclidean_dist(
child.parent.position, child.position)
# check if child exists in the open list and have bigger g
for open_node in open_list:
if child == open_node and child.g > open_node.g:
continue
x_pos, y_pos = self.get_indices(child.position[0],
child.position[1])
print('\n', "x_in_meter, y_in_meter: ", child.position[0],
child.position[1])
print('\n', "x_pos, y_pos", x_pos, y_pos)
if self.visited_nodes[x_pos, y_pos] == 0:
open_list.append(child)
self.visited_nodes[x_pos, y_pos] = 1
else:
print("False attempt : ", child.position)
# Find the neighbors of the current node
current_neighbors = self.curr_neighbors(current_node, self.boundary_list)
children = []
# Make the neighbor the child node of the current node if collision-free
for neighbor in current_neighbors:
if self.collision_free(neighbor, self.obstacle_list):
new_node = Node(current_node, neighbor)
children.append(new_node)
for child in children:
# Continue to beginning of for loop if child is in closed list
if child in closed_list:
continue
# Compute the attributes if the child node is new
habitatInfo = cal_cost.cost_of_edge(child, self.habitat_open_list, self.habitat_closed_list, weights)
<<<<<<< HEAD
# print("Num Closed: ", len(self.habitat_closed_list))
insideAnyHabitats = habitatInfo[1]
insideOpenHabitats = habitatInfo[2]
child.g = child.parent.cost - w2 * insideAnyHabitats - w3 * insideOpenHabitats
child.cost = child.g
child.h = - w2 * abs(pathLenLimit - child.pathLen) - w3 * len(self.habitat_open_list)
# - w4 * len(habitats in range)
=======
insideClosedHabitats = habitatInfo[1]
insideOpenHabitats = habitatInfo[2]
child.g = child.parent.cost - weight_10 * insideClosedHabitats - weight_1000 * insideOpenHabitats
child.cost = child.g
NumHabitatInRange = self.NumHabitatInRange(child, self.habitat_open_list, pathLenLimit)
