def test_boundary():
scenario = {
"waypoint": {
"start": {
"x": 5,
"y": 9,
"z": 2
},
"stop": {
"x": 5,
"y": 0,
"z": 4
},
"allowDiagonal": False
},
"boundary": {
"zCeil": 6,
"zFloor": 1
}
}
zStart = int(scenario["waypoint"]["start"]["z"])
zCeil = int(scenario["boundary"]["zCeil"])
zFloor = int(scenario["boundary"]["zFloor"])
assert Model.is_boundary_available(zFloor, zStart, zCeil)
assert Model.is_boundary_available(zFloor, 0, zCeil) == False
assert Model.is_boundary_available(zFloor, 6, zCeil) == False
def test_check_num_obstacles():
obstacle_array = Model.create_obstacle_array()
assert len(obstacle_array) == 0
scenario_empty_data = {"size": 0, "x": [1, 2, 3], "y": [1, 2, 3]}
obstacle_empty_array = Model.create_obstacle_array(scenario_empty_data)
assert len(obstacle_empty_array) == 0
scenario_2d_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_2D_array = Model.create_obstacle_array(scenario_2d_data)
assert len(obstacle_2D_array) == int(scenario_2d_data["size"])
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)
assert len(obstacle_3D_array) == int(scenario_3d_data["size"])
def test_find_the_min_F():
obstacle_2D_array = Model.create_obstacle_array(scenario_2d["data"])
model_2D = Model(scenario_2d["dimension"], obstacle_2D_array,
scenario_2d["waypoint"])
Q_2D = model_2D.create_initial_Q()
min_node_2D = Tools.find_the_minimum(Q_2D, 'f')
assert min_node_2D["key"] == '12,0'
assert int(min_node_2D["value"].dist) == 0
obstacle_3D_array = Model.create_obstacle_array(scenario["data"])
model_3D = Model(scenario["dimension"], obstacle_3D_array,
scenario["waypoint"])
is_fast = True
fast_Q_3D = model_3D.create_initial_Q(is_fast)
min_node_3D = Tools.find_the_minimum(fast_Q_3D, 'f')
assert min_node_3D["key"] == '5,9,2'
assert int(min_node_3D["value"].dist) == 0
is_fast = False
original_Q_3D = model_3D.create_initial_Q(is_fast)
original_min_node_3D = Tools.find_the_minimum(original_Q_3D, 'f')
assert original_min_node_3D["key"] == '5,9,2'
assert int(original_min_node_3D["value"].dist) == 0
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 __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 test_create_initial_Q():
scenario_2d = {
"dimension": {
"x": 15,
"y": 15
},
"waypoint": {
"start": {
"x": 12,
"y": 0
},
"stop": {
"x": 1,
"y": 11
},
"allowDiagonal": False
},
"data": {
"size":
28,
"x": [
2, 2, 2, 2, 2, 2, 2, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 12,
12, 12, 12, 12, 12, 12, 12, 12, 12
],
"y": [
5, 6, 7, 8, 9, 10, 11, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12,
12, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
]
}
}
obstacle_2D_array = Model.create_obstacle_array(scenario_2d["data"])
model_2D = Model(scenario_2d["dimension"], obstacle_2D_array,
scenario_2d["waypoint"])
Q_2D = model_2D.create_initial_Q(False)
model_fast_2D = Model(scenario_2d["dimension"], obstacle_2D_array,
scenario_2d["waypoint"])
Q_fast_2D = model_fast_2D.create_initial_Q(True)
number_obstacle_nodes_2D = (int(scenario_2d["dimension"]["x"]) * int(
scenario_2d["dimension"]["y"])) - int(scenario_2d["data"]["size"])
assert len(Q_2D) == number_obstacle_nodes_2D
assert len(Q_fast_2D) == 1
scenario_3d = {
"dimension": {
"x": 10,
"y": 10,
"z": 10
},
"waypoint": {
"start": {
"x": 5,
"y": 9,
"z": 2
},
"stop": {
"x": 6,
"y": 6,
"z": 5
},
"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]
}
}
obstacle_3D_array = Model.create_obstacle_array(scenario_3d["data"])
model3D = Model(scenario_3d["dimension"], obstacle_3D_array,
scenario_3d["waypoint"])
isFast = False
original_Q_3D = model3D.create_initial_Q(isFast)
number_obstacle_nodes_3D = (int(scenario_3d["dimension"]["x"]) *
int(scenario_3d["dimension"]["y"]) *
int(scenario_3d["dimension"]["z"])) - int(
scenario_3d["data"]["size"])
assert len(original_Q_3D) == number_obstacle_nodes_3D
fast_Q_3D = model3D.create_initial_Q()
assert len(fast_Q_3D) == number_obstacle_nodes_3D
def test_is_2d():
assert Model.is_two_dimensional(dimension_2D1) is True
assert Model.is_two_dimensional(dimension_2D2) is True
assert Model.is_two_dimensional(dimension_2D3) is True
assert Model.is_two_dimensional(dimension_3D) is False