def calculate_path(self):
final_Q = dict()
visited_Q = dict()
calculate_start_time = time.time()
if self.num_obstacles_in_stop_group > 0:
calculate_end_time = time.time()
elapsed_ms = 1000.0 * (calculate_end_time - calculate_start_time)
path = {
"x": [int(self.start_node.x)],
"y": [int(self.start_node.y)],
"z": [] if self.is_2d else [int(self.start_node.z)]
}
refined_path = Tools.refine_path_from_collinearity(path)
self.message = "[Path Error] no results due to obstacles in STOP area."
return {
"visited_Q": visited_Q,
"final_Q": final_Q,
"elapsed_ms": elapsed_ms,
"path": path,
"refined_path": refined_path,
"message": self.message
}
size = len(self.open_set)
while size > 0:
obj = Tools.find_the_minimum(self.open_set, 'f')
obj_key = obj["key"]
current_node = obj["value"]
final_Q[obj_key] = current_node
if obj_key is not None:
self.last_node_key = str(obj_key)
del self.open_set[obj_key]
if current_node == self.stop_node:
message = "[Done] Arrival! 🚀"
print(message)
self.message = message
break
shift_node = [-1, 0, 1]
for shift_row in shift_node:
for shift_col in shift_node:
if self.is_2d:
is_not_diagonal = (shift_row == 0 or shift_col
== 0) and (shift_row != shift_col)
is_diagonal = not (shift_row == 0 and shift_col == 0)
is_allowed = is_diagonal if self.allow_diagonal else is_not_diagonal
if is_allowed:
neighbor_node = current_node.shift(
shift_row, shift_col)
if neighbor_node.is_out_of_bound(
bound_x=[-1, self.dimension['x']],
bound_y=[-1, self.dimension['y']]):
continue
if self.is_grouping:
is_obstacle_found = False
for obstacle_node in self.obstacle_array:
if Tools.intersect(neighbor_node,
obstacle_node,
self.group_radius,
self.is_group_flat):
is_obstacle_found = True
# no more to check the other collisions
break
if is_obstacle_found:
# continue to find the next neighbor
continue
else:
is_obstacle_found = False
for index in range(self.num_obstacles):
obstacle_node = self.obstacle_array[index]
if obstacle_node == neighbor_node:
# Find out an obstacle on the neighbor node
is_obstacle_found = True
break
if is_obstacle_found:
continue
if str(neighbor_node) in final_Q:
continue
if self.is_fast is True:
neighbor = visited_Q.get(str(neighbor_node))
if neighbor is None:
neighbor = Node(neighbor_node.x,
neighbor_node.y)
visited_Q[str(neighbor_node)] = neighbor
if str(neighbor_node) not in self.open_set:
self.open_set[str(
neighbor_node)] = neighbor
dist = sqrt(shift_row**2 + shift_col**2)
alt = current_node.dist + dist
if alt < neighbor.dist:
neighbor.dist = alt
neighbor.f = alt + neighbor.manhattan_distance_to(
self.stop_node)
neighbor.prev = str(current_node)
self.open_set[str(
neighbor_node)] = neighbor
else:
neighbor = self.Q.get(str(neighbor_node))
if neighbor is not None and str(
neighbor_node) not in final_Q:
visited_Q[str(neighbor_node)] = neighbor
if str(neighbor_node) not in self.open_set:
self.open_set[str(
neighbor_node)] = neighbor
dist = sqrt(shift_row**2 + shift_col**2)
alt = current_node.dist + dist
if alt < neighbor.dist:
neighbor.dist = alt
neighbor.f = alt + neighbor.manhattan_distance_to(
self.stop_node)
neighbor.prev = str(current_node)
self.open_set[str(
neighbor_node)] = neighbor
else:
for shift_z in shift_node:
is_not_diagonal = ((shift_row == 0 or shift_col == 0) and (shift_row != shift_col)) \
or (shift_row == 0 and shift_col == 0)
is_diagonal = not (shift_row == 0 and shift_col
== 0 and shift_z == 0)
is_allowed = is_diagonal if self.allow_diagonal else is_not_diagonal
if is_allowed:
neighbor_node = current_node.shift(
shift_row, shift_col, shift_z)
if neighbor_node.is_out_of_bound(
bound_z=[self.z_floor, self.z_ceil]):
continue
# Full search (time-consuming) = 2D
# neighbor = self.Q.get(str(neighbor_node))
# if neighbor is not None and str(neighbor_node) not in final_Q:
# visited_Q[str(neighbor_node)] = neighbor
#
# dist = sqrt(shift_row ** 2 + shift_col ** 2 + shift_z ** 2)
# alt = current_obj["dist"] + dist
# # ...
# Fast search
if self.is_grouping:
is_obstacle_found = False
for obstacle_node in self.obstacle_array:
if Tools.intersect(
neighbor_node, obstacle_node,
self.group_radius,
self.is_group_flat):
is_obstacle_found = True
# no more to check the other collisions
break
if is_obstacle_found:
# continue to find the next neighbor
continue
else:
is_obstacle_found = False
for index in range(self.num_obstacles):
obstacle_node = self.obstacle_array[
index]
if obstacle_node == neighbor_node:
# Find out an obstacle on the neighbor node
is_obstacle_found = True
break
if is_obstacle_found:
continue
# has_key was removed in Python 3
# https://docs.python.org/3.0/whatsnew/3.0.html#builtins
# print(final_Q.has_key(neighborPosition))
if str(neighbor_node) in final_Q:
continue
neighbor = visited_Q.get(str(neighbor_node))
if neighbor is None:
neighbor = Node(neighbor_node.x,
neighbor_node.y,
neighbor_node.z)
visited_Q[str(neighbor_node)] = neighbor
if str(neighbor_node) not in self.open_set:
self.open_set[str(
neighbor_node)] = neighbor
dist = sqrt(shift_row**2 + shift_col**2 +
shift_z**2)
alt = current_node.dist + dist
if alt < neighbor.dist:
neighbor.dist = alt
neighbor.f = alt + neighbor.manhattan_distance_to(
self.stop_node)
neighbor.prev = str(current_node)
self.open_set[str(
neighbor_node)] = neighbor
size = len(self.open_set)
calculate_end_time = time.time()
elapsed_ms = 1000.0 * (calculate_end_time - calculate_start_time)
final_node = final_Q.get(str(self.last_node_key))
path = Tools.create_path_from_final_Q(final_Q, final_node)
refined_path = Tools.refine_path_from_collinearity(path)
if self.num_obstacles_in_start_group > 0 and len(path["x"]) == 1:
self.message = "[Path Error] no results due to obstacles in START area."
return {
"visited_Q": visited_Q,
"final_Q": final_Q,
"elapsed_ms": elapsed_ms,
"path": path,
"refined_path": refined_path,
"message": self.message
}
class Model:
def __init__(self, dimension, obstacle_array, waypoint, debug_mode=False):
self.dimension = dimension
self.is_2d = Model.is_two_dimensional(self.dimension)
self.obstacle_array = obstacle_array
start = waypoint["start"]
stop = waypoint["stop"]
self.start_node = Node(start["x"], start["y"]) if self.is_2d else Node(start["x"], start["y"], start["z"])
self.start_node.set_as_start_node()
self.stop_node = Node(stop["x"], stop["y"]) if self.is_2d else Node(stop["x"], stop["y"], stop["z"])
self.dist = self.start_node.manhattan_distance_to(self.stop_node)
self.init_Q = dict()
self.debug_mode = debug_mode
@staticmethod
def is_two_dimensional(dimension):
if "z" not in dimension or int(dimension["z"]) <= 0:
return True
return False
@staticmethod
def create_obstacle_array(data = None):
if data is None or len(data) == 0 or int(data["size"]) == 0:
return []
size = int(data["size"])
x_array = data["x"]
y_array = data["y"]
if "z" not in data:
return [Node(x_array[i], y_array[i]) for i in range(size)]
else:
z_array = data["z"]
return [Node(x_array[i], y_array[i], z_array[i]) for i in range(size)]
@staticmethod
def nodes_on_obstacles(array, nodes):
for _, restricted_node in enumerate(array):
for node in nodes:
if node == restricted_node:
print("the point is located on restricted region")
return True
return False
@staticmethod
def is_boundary_available(lower_bound, value, upper_bound):
lower_bound = -inf if lower_bound is None else lower_bound
upper_bound = inf if upper_bound is None else upper_bound
if value <= lower_bound:
print("value <= lower_bound")
return False
if value >= upper_bound:
print("value >= upper_bound")
return False
return lower_bound + 1 < upper_bound
def create_initial_Q(self, is_fast=True):
x = int(self.dimension["x"])
y = int(self.dimension["y"])
calculate_start_time = time.time()
if is_fast:
# f_value = abs(self.dist_x) + abs(self.dist_y) + abs(self.dist_z) # option 1
f_value = self.dist # option 2
self.start_node.f = f_value
self.init_Q[str(self.start_node)] = self.start_node
else:
if self.is_2d:
[self.update_init_Q(row, col, None) for row in range(x) for col in range(y)
if Node(row, col) not in self.obstacle_array]
else:
z = int(self.dimension["z"])
[self.update_init_Q(row, col, iz) for row in range(x) for col in range(y) for iz in range(z)
if Node(row, col, iz) not in self.obstacle_array]
calculate_end_time = time.time()
if self.debug_mode is True:
print(f'create_initial_Q time: {1000.0 * (calculate_end_time - calculate_start_time)} ms')
return self.init_Q
def update_init_Q(self, row, col, z = None):
cell_node = Node(row, col, z)
if cell_node == self.start_node:
cell_node.dist = 0
cell_node.f = cell_node.dist + self.dist
self.init_Q[str(cell_node)] = cell_node
