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
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)]
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
def test_nodes_on_obstacles():
scenario_3d_data = {
"size": 16,
"x": [4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7],
"y": [6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6],
"z": [2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5]
}
obstacle_3D_array = Model.create_obstacle_array(scenario_3d_data)
test_good_waypoint_array = [Node(5, 9, 2), Node(5, 0, 4)]
assert Model.nodes_on_obstacles(obstacle_3D_array,
test_good_waypoint_array) == False
test_error_waypoint_array = [Node(5, 9, 2), Node(6, 6, 5)]
Model.nodes_on_obstacles(obstacle_3D_array,
test_error_waypoint_array) == True
def __init__(self, scenario):
dimension = scenario["dimension"]
self.is_2d = Model.is_two_dimensional(dimension)
if "data" in scenario:
self.obstacle_array = Model.create_obstacle_array(scenario["data"])
else:
self.obstacle_array = []
self.num_obstacles = len(self.obstacle_array)
self.waypoint = scenario["waypoint"]
start = self.waypoint["start"]
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()
stop = self.waypoint["stop"]
self.stop_node = Node(stop["x"], stop["y"]) if self.is_2d else Node(stop["x"], stop["y"], stop["z"])
self.last_node_key = str(self.stop_node)
self.allow_diagonal = bool(self.waypoint["allowDiagonal"]) if "allowDiagonal" in self.waypoint else False
model = Model(dimension, self.obstacle_array, self.waypoint, True)
self.Q = model.create_initial_Q(False)
self.open_set = dict()
self.open_set[str(self.start_node)] = self.Q.get(str(self.start_node))
if Model.nodes_on_obstacles(self.obstacle_array, [self.start_node, self.stop_node]):
message = "[Waypoint Error] start position or stop position is on the obstacle."
print(message)
self.message = message
self.boundary = scenario["boundary"] if "boundary" in scenario else None
if not self.is_2d:
self.z_ceil = int(self.boundary["zCeil"]) if (self.boundary and "zCeil" in self.boundary) else inf
self.z_floor = int(self.boundary["zFloor"]) if (self.boundary and "zFloor" in self.boundary) else -inf
# print("z_ceil: {}, z_floor: {}".format(self.z_ceil, self.z_floor))
if not Model.is_boundary_available(self.z_floor, self.start_node.z, self.z_ceil):
message = "[Boundary Error] start position is out of boundary."
print(message)
self.message = message
self.message = "[Ready] No Results."
def intersect(group_center, obstacle_node, radius=1, is_flat=True) -> bool:
[box_min_x, box_max_x, box_min_y, box_max_y] = [
obstacle_node.x - 0.5, obstacle_node.x + 0.5,
obstacle_node.y - 0.5, obstacle_node.y + 0.5
]
x = max(box_min_x, min(group_center.x, box_max_x))
y = max(box_min_y, min(group_center.y, box_max_y))
if is_flat:
flat_center_node = Node(group_center.x, group_center.y)
closest_point = Node(x, y)
# distance = sqrt(pow(x - group_center.x, 2) + pow(y - group_center.y, 2))
distance = flat_center_node.distance_to(closest_point)
return distance <= radius
else:
[boxMinZ, boxMaxZ] = [obstacle_node.z - 0.5, obstacle_node.z + 0.5]
z = max(boxMinZ, min(group_center.z, boxMaxZ))
closest_point = Node(x, y, z)
distance = group_center.distance_to(closest_point)
return distance <= radius
def test_object_collision():
scenario = {
"waypoint": {
"start": {"x": 5, "y": 9, "z": 2},
"stop": {"x": 5, "y": 0, "z": 4},
"allowDiagonal": False
},
"data": {
"size": 16,
"x": [4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7],
"y": [6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6],
"z": [2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5]
}
}
start = scenario["waypoint"]["start"]
start_node = Node(start["x"], start["y"], start["z"])
data = scenario["data"]
index_obstacle = 0
obstacle_node = Node(data["x"][index_obstacle], data["y"][index_obstacle], data["z"][index_obstacle])
test_node = Node(5, 9, 2)
assert start_node != obstacle_node
assert start_node == test_node
def test_equal():
assert (node_2D == Node(1, 2)) == True
assert (node_2D == Node(1, 2, 0)) == False
assert (node_3D120 == Node(1, 2, 0)) == True
assert (node_3D120 == Node(1, 2)) == False
assert (node_3D123 == Node(1, 2, 3)) == True
assert (node_3D123 == Node(1, 2, 4)) == False
def test_intersect():
assert Tools.intersect(Node(1, 1), Node(1, 2.5)) == True
group_center_2D = Node(2.999, 5)
obstacle_2D = Node(5, 5)
critical_distance_2D = 1.5
assert Tools.intersect(group_center_2D, obstacle_2D, critical_distance_2D,
True) == False
intersected_group_center_2D = Node(3, 5)
assert Tools.intersect(intersected_group_center_2D, obstacle_2D,
critical_distance_2D, True) == True
group_center_3D = Node(2.999, 5, 4)
obstacle_3D = Node(5, 5, 5)
critical_distance_3D = 1.582
assert Tools.intersect(group_center_3D, obstacle_3D, critical_distance_3D,
False) == False
intersected_group_center_3D = Node(3, 5, 4)
assert Tools.intersect(intersected_group_center_3D, obstacle_3D,
critical_distance_3D, False) == True
def test_create_path_from_finalQ():
start_node = Node(12, 0)
stop_node = Node(1, 11)
dict_2D = dict()
no_result_2D = Tools.create_path_from_final_Q(dict_2D, start_node)
assert int(no_result_2D["x"][0]) == int(start_node.x)
assert int(no_result_2D["y"][0]) == int(start_node.y)
assert len(no_result_2D["z"]) == int(0)
last_node = Node(6, 6)
start_node.prev = last_node
dict_2D[str(last_node)] = last_node
partial_result_2D = Tools.create_path_from_final_Q(dict_2D, last_node)
assert int(partial_result_2D["x"][0]) == int(last_node.x)
assert int(partial_result_2D["y"][0]) == int(last_node.y)
last_node.prev = stop_node
great_result_2D = Tools.create_path_from_final_Q(dict_2D, stop_node)
assert int(great_result_2D["x"][0]) == int(stop_node.x)
assert int(great_result_2D["y"][0]) == int(stop_node.y)
def test_shift():
assert (node_2D.shift(2, 1) == Node(3, 3)) == True
assert (node_2D.shift(2, 1, 1) == Node(3, 3)) == True
assert (node_3D120.shift(2, 1) == Node(3, 3, 0)) == True
assert (node_3D120.shift(2, 1, 3) == Node(3, 3, 3)) == True
from math import inf
from pyhpp.node import Node
node_2D = Node(1, 2)
node_3D120 = Node(1, 2, 0)
node_3D123 = Node(1, 2, 3)
def test_is2d():
assert node_2D.is_2d == True
assert node_3D120.is_2d == False
assert node_3D123.is_2d == False
def test_str():
assert str(node_2D) == '1,2'
assert str(node_3D120) == '1,2,0'
assert str(node_3D123) == '1,2,3'
def test_equal():
assert (node_2D == Node(1, 2)) == True
assert (node_2D == Node(1, 2, 0)) == False
assert (node_3D120 == Node(1, 2, 0)) == True
assert (node_3D120 == Node(1, 2)) == False
assert (node_3D123 == Node(1, 2, 3)) == True
assert (node_3D123 == Node(1, 2, 4)) == False
def test_get_crux():
assert (node_2D.get_crux('dist') == inf) == True
assert (node_2D.get_crux('f') == inf) == True
class AStar:
def __init__(self, scenario, options=None):
self.dimension = scenario["dimension"]
self.is_2d = Model.is_two_dimensional(self.dimension)
if "data" in scenario:
self.obstacle_array = Model.create_obstacle_array(scenario["data"])
else:
self.obstacle_array = []
self.num_obstacles = len(self.obstacle_array)
self.waypoint = scenario["waypoint"]
start = self.waypoint["start"]
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()
stop = self.waypoint["stop"]
self.stop_node = Node(stop["x"], stop["y"]) if self.is_2d else Node(
stop["x"], stop["y"], stop["z"])
self.last_node_key = str(self.stop_node)
self.allow_diagonal = bool(
self.waypoint["allowDiagonal"]
) if "allowDiagonal" in self.waypoint else False
if options is None:
self.debug_mode = False
self.is_fast = True
else:
self.debug_mode = True if 'debug_mode' in options and options[
'debug_mode'] is True else False
self.is_fast = False if 'type' in options and options[
'type'] == 'original' else True
if self.debug_mode:
print("A* Path Finding (2D)") if self.is_2d else print(
"A* Path Finding (3D)")
model = Model(self.dimension, self.obstacle_array, self.waypoint,
self.debug_mode)
self.Q = model.create_initial_Q(self.is_fast)
self.open_set = dict()
self.open_set[str(self.start_node)] = self.Q.get(str(self.start_node))
self.message = "[Done] no results."
if Model.nodes_on_obstacles(self.obstacle_array,
[self.start_node, self.stop_node]):
message = "[Waypoint Error] start position or stop position is on the obstacle."
print(message)
self.message = message
self.boundary = scenario["boundary"] if "boundary" in scenario else None
if not self.is_2d:
self.z_ceil = int(self.boundary["zCeil"]) if (
self.boundary and "zCeil" in self.boundary) else inf
self.z_floor = int(self.boundary["zFloor"]) if (
self.boundary and "zFloor" in self.boundary) else -inf
# print("z_ceil: {}, z_floor: {}".format(self.z_ceil, self.z_floor))
if not Model.is_boundary_available(self.z_floor, self.start_node.z,
self.z_ceil):
message = "[Boundary Error] start position is out of boundary."
print(message)
self.message = message
grouping = scenario["grouping"] if "grouping" in scenario else None
# Only for integer type
# self.is_grouping = True if grouping is not None and "radius" in grouping and str(grouping["radius"]).isnumeric() else False
# For integer and float type
self.is_grouping = True if grouping is not None and "radius" in grouping and Tools.is_number(
str(grouping["radius"])) else False
self.group_radius = float(
grouping["radius"]) if self.is_grouping else 0
self.is_group_flat = True if self.is_2d or "boundary" in scenario else False
self.num_obstacles_in_start_group = 0
self.num_obstacles_in_stop_group = 0
if self.is_grouping:
grouping_style = 'circle' if self.is_group_flat else 'sphere'
print('[Grouping] radius', (self.group_radius + 0.6), 'of',
grouping_style)
for obstacle in self.obstacle_array:
if Tools.intersect(self.start_node, obstacle,
self.group_radius, self.is_group_flat):
# message = f'[Grouping Error] obstacle is in the start {grouping_style}'
# print(message)
self.num_obstacles_in_start_group = self.num_obstacles_in_start_group + 1
if Tools.intersect(self.stop_node, obstacle, self.group_radius,
self.is_group_flat):
# message = f'[Grouping Error] obstacle is in the stop {grouping_style}'
# print(message)
self.num_obstacles_in_stop_group = self.num_obstacles_in_stop_group + 1
if self.num_obstacles_in_start_group > 0:
print(f'[Grouping Error] {self.num_obstacles_in_start_group} obstacle is in the start {grouping_style}') if self.num_obstacles_in_start_group == 1 \
else print(f'[Grouping Error] {self.num_obstacles_in_start_group} obstacles are in the start {grouping_style}')
if self.num_obstacles_in_stop_group > 0:
print(f'[Grouping Error] {self.num_obstacles_in_stop_group} obstacle is in the stop {grouping_style}') if self.num_obstacles_in_stop_group == 1 \
else print(f'[Grouping Error] {self.num_obstacles_in_stop_group} obstacles are in the stop {grouping_style}')
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
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
}
def calculate_path(self):
# print("A* Path Finding (2D)") if self.is_2d else print("A* Path Finding (3D)")
final_Q = dict()
visited_Q = dict()
calculate_start_time = time.time()
size = len(self.open_set)
while size > 0:
obj = Tools.find_the_minimum(self.open_set, 'dist')
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)
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.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
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.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)
return {
"visited_Q": visited_Q,
"final_Q": final_Q,
"elapsed_ms": elapsed_ms,
"path": path,
"message": self.message
}
def __init__(self, scenario, options=None):
self.dimension = scenario["dimension"]
self.is_2d = Model.is_two_dimensional(self.dimension)
if "data" in scenario:
self.obstacle_array = Model.create_obstacle_array(scenario["data"])
else:
self.obstacle_array = []
self.num_obstacles = len(self.obstacle_array)
self.waypoint = scenario["waypoint"]
start = self.waypoint["start"]
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()
stop = self.waypoint["stop"]
self.stop_node = Node(stop["x"], stop["y"]) if self.is_2d else Node(
stop["x"], stop["y"], stop["z"])
self.last_node_key = str(self.stop_node)
self.allow_diagonal = bool(
self.waypoint["allowDiagonal"]
) if "allowDiagonal" in self.waypoint else False
if options is None:
self.debug_mode = False
self.is_fast = True
else:
self.debug_mode = True if 'debug_mode' in options and options[
'debug_mode'] is True else False
self.is_fast = False if 'type' in options and options[
'type'] == 'original' else True
if self.debug_mode:
print("A* Path Finding (2D)") if self.is_2d else print(
"A* Path Finding (3D)")
model = Model(self.dimension, self.obstacle_array, self.waypoint,
self.debug_mode)
self.Q = model.create_initial_Q(self.is_fast)
self.open_set = dict()
self.open_set[str(self.start_node)] = self.Q.get(str(self.start_node))
self.message = "[Done] no results."
if Model.nodes_on_obstacles(self.obstacle_array,
[self.start_node, self.stop_node]):
message = "[Waypoint Error] start position or stop position is on the obstacle."
print(message)
self.message = message
self.boundary = scenario["boundary"] if "boundary" in scenario else None
if not self.is_2d:
self.z_ceil = int(self.boundary["zCeil"]) if (
self.boundary and "zCeil" in self.boundary) else inf
self.z_floor = int(self.boundary["zFloor"]) if (
self.boundary and "zFloor" in self.boundary) else -inf
# print("z_ceil: {}, z_floor: {}".format(self.z_ceil, self.z_floor))
if not Model.is_boundary_available(self.z_floor, self.start_node.z,
self.z_ceil):
message = "[Boundary Error] start position is out of boundary."
print(message)
self.message = message
grouping = scenario["grouping"] if "grouping" in scenario else None
# Only for integer type
# self.is_grouping = True if grouping is not None and "radius" in grouping and str(grouping["radius"]).isnumeric() else False
# For integer and float type
self.is_grouping = True if grouping is not None and "radius" in grouping and Tools.is_number(
str(grouping["radius"])) else False
self.group_radius = float(
grouping["radius"]) if self.is_grouping else 0
self.is_group_flat = True if self.is_2d or "boundary" in scenario else False
self.num_obstacles_in_start_group = 0
self.num_obstacles_in_stop_group = 0
if self.is_grouping:
grouping_style = 'circle' if self.is_group_flat else 'sphere'
print('[Grouping] radius', (self.group_radius + 0.6), 'of',
grouping_style)
for obstacle in self.obstacle_array:
if Tools.intersect(self.start_node, obstacle,
self.group_radius, self.is_group_flat):
# message = f'[Grouping Error] obstacle is in the start {grouping_style}'
# print(message)
self.num_obstacles_in_start_group = self.num_obstacles_in_start_group + 1
if Tools.intersect(self.stop_node, obstacle, self.group_radius,
self.is_group_flat):
# message = f'[Grouping Error] obstacle is in the stop {grouping_style}'
# print(message)
self.num_obstacles_in_stop_group = self.num_obstacles_in_stop_group + 1
if self.num_obstacles_in_start_group > 0:
print(f'[Grouping Error] {self.num_obstacles_in_start_group} obstacle is in the start {grouping_style}') if self.num_obstacles_in_start_group == 1 \
else print(f'[Grouping Error] {self.num_obstacles_in_start_group} obstacles are in the start {grouping_style}')
if self.num_obstacles_in_stop_group > 0:
print(f'[Grouping Error] {self.num_obstacles_in_stop_group} obstacle is in the stop {grouping_style}') if self.num_obstacles_in_stop_group == 1 \
else print(f'[Grouping Error] {self.num_obstacles_in_stop_group} obstacles are in the stop {grouping_style}')
class Dijkstra:
def __init__(self, scenario):
dimension = scenario["dimension"]
self.is_2d = Model.is_two_dimensional(dimension)
if "data" in scenario:
self.obstacle_array = Model.create_obstacle_array(scenario["data"])
else:
self.obstacle_array = []
self.num_obstacles = len(self.obstacle_array)
self.waypoint = scenario["waypoint"]
start = self.waypoint["start"]
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()
stop = self.waypoint["stop"]
self.stop_node = Node(stop["x"], stop["y"]) if self.is_2d else Node(stop["x"], stop["y"], stop["z"])
self.last_node_key = str(self.stop_node)
self.allow_diagonal = bool(self.waypoint["allowDiagonal"]) if "allowDiagonal" in self.waypoint else False
model = Model(dimension, self.obstacle_array, self.waypoint, True)
self.Q = model.create_initial_Q(False)
self.open_set = dict()
self.open_set[str(self.start_node)] = self.Q.get(str(self.start_node))
if Model.nodes_on_obstacles(self.obstacle_array, [self.start_node, self.stop_node]):
message = "[Waypoint Error] start position or stop position is on the obstacle."
print(message)
self.message = message
self.boundary = scenario["boundary"] if "boundary" in scenario else None
if not self.is_2d:
self.z_ceil = int(self.boundary["zCeil"]) if (self.boundary and "zCeil" in self.boundary) else inf
self.z_floor = int(self.boundary["zFloor"]) if (self.boundary and "zFloor" in self.boundary) else -inf
# print("z_ceil: {}, z_floor: {}".format(self.z_ceil, self.z_floor))
if not Model.is_boundary_available(self.z_floor, self.start_node.z, self.z_ceil):
message = "[Boundary Error] start position is out of boundary."
print(message)
self.message = message
self.message = "[Ready] No Results."
def calculate_path(self):
# print("A* Path Finding (2D)") if self.is_2d else print("A* Path Finding (3D)")
final_Q = dict()
visited_Q = dict()
calculate_start_time = time.time()
size = len(self.open_set)
while size > 0:
obj = Tools.find_the_minimum(self.open_set, 'dist')
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)
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.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
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.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)
return {
"visited_Q": visited_Q,
"final_Q": final_Q,
"elapsed_ms": elapsed_ms,
"path": path,
"message": self.message
}
