def test_current_heading_is_within_threshold_returns_true():
robot_status = RobotStatus((20, 20), 90)
robot_status.target_heading = 90
robot_status.heading = 89.95
assert robot_status.heading_has_reached_target_heading_threshold() is True
def set_game_map(self, game_map_data):
robot_information = game_map_data.get("robot")
if robot_information:
self.logger.log(
"Pathfinder - Assigning start position = {0} and orientation = {1}"
.format(robot_information[0], robot_information[1]))
self.robot_status = RobotStatus(robot_information[0],
robot_information[1])
else:
self.robot_status = RobotStatus((20, 20), 90)
table_corners_positions = game_map_data.get("table_corners")
if table_corners_positions:
self.logger.log(
"Pathfinding - Assigning table corner positions: {0}".format(
table_corners_positions))
self.figures.compute_positions(table_corners_positions[0],
table_corners_positions[1],
table_corners_positions[2],
table_corners_positions[3])
else:
self.figures.compute_positions((0, 0), (0, 231), (112, 231),
(112, 0))
obstacles = game_map_data.get("obstacles")
if obstacles:
self.logger.log(
"Pathfinder - Assigning obstacles: {0}".format(obstacles))
self.graph = Graph()
southeastern_x = int(table_corners_positions[0][0])
southeastern_y = int(table_corners_positions[0][1])
northwestern_x = int(table_corners_positions[2][0])
northwestern_y = int(table_corners_positions[2][1])
int_obstacles = []
for obstacle in obstacles:
x, y = obstacle[0]
int_obstacles.append([(int(x), int(y)), obstacle[1]])
self.graph.initialize_graph_matrix(
(southeastern_x, southeastern_y),
(northwestern_x, northwestern_y), int_obstacles)
else:
self.graph.initialize_graph_matrix((0, 0), (112, 231), [])
drawing_zone_corners = game_map_data.get("drawing_zone")
if drawing_zone_corners:
self.logger.log(
"Pathfinding - Assigning drawing zone corners: {0}".format(
drawing_zone_corners))
self.game_map.set_drawing_zone_borders(
game_map_data.get("drawing_zone"))
self.game_map.set_antenna_search_points(table_corners_positions[3])
else:
self.game_map.set_drawing_zone_borders(
((26, 27), (26, 87), (86, 87), (86, 27)))
self.game_map.set_antenna_search_points((112, 0))
def test_given_finishing_rotation_movement_then_rotation_status_is_reinitialized_and_new_robot_position_is_assigned(
):
position = (48, 37)
robot_status = RobotStatus((50, 50), 90)
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
servo_wheels_manager.finish_rotation_and_set_new_robot_position(
position, robot_status)
assert servo_wheels_manager.rotation_status == RotationStatus.ROTATING
assert robot_status.get_position() == position
def test_given_start_heading_correction_for_rotation_then_rotation_status_is_updated_and_command_is_sent_to_wheels(
):
heading = 75
robot_status = RobotStatus((50, 50), 90)
robot_status.target_heading = 79
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
wheels_controller = SimulatedWheelsController()
servo_wheels_manager.rotating_start_heading_correction(
heading, robot_status, wheels_controller)
assert servo_wheels_manager.rotation_status == RotationStatus.CORRECTING_HEADING
assert int(wheels_controller.last_degrees_of_rotation_given) == 4
def test_given_at_checkpoint_when_finishing_translation_then_translation_status_is_updated_and_no_new_vector_is_generated(
):
position = (50.1, 54.9)
robot_status = RobotStatus((45, 50), 90)
robot_status.target_position = (50, 55)
pathfinder = Pathfinder(ExecutionLogger())
pathfinder.robot_status = robot_status
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
path_status, new_vector = servo_wheels_manager.finish_translation_and_get_new_path_status_and_vector(
position, pathfinder)
assert servo_wheels_manager.translation_status == TranslationStatus.MOVING
assert new_vector is None
assert path_status == PathStatus.CHECKPOINT_REACHED
def test_given_start_of_position_correction_for_translation_change_translation_status_and_send_translation_vector_towards_current_target_node(
):
position = (40, 50)
robot_status = RobotStatus((30, 30), 90)
robot_status.target_position = (60, 65)
wheels_controller = SimulatedWheelsController()
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
servo_wheels_manager.translating_start_position_correction(
position, robot_status, wheels_controller)
vector_x, vector_y = robot_status.get_translation_vector()
assert int(vector_x) == 20
assert int(vector_y) == 15
assert servo_wheels_manager.translation_status == TranslationStatus.CORRECTING_POSITION
position = (50.1, 54.9)
robot_status = RobotStatus((45, 50), 90)
robot_status.target_position = (50, 55)
pathfinder = Pathfinder(ExecutionLogger())
pathfinder.robot_status = robot_status
pathfinder.nodes_queue_to_checkpoint.append((100, 150))
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
def test_given_at_intermediary_node_when_finishing_translation_then_translation_status_is_updated_and_new_vector_towards_next_node_is_generated(
):
position = (50.1, 54.9)
robot_status = RobotStatus((45, 50), 90)
robot_status.target_position = (50, 55)
pathfinder = Pathfinder(ExecutionLogger())
pathfinder.robot_status = robot_status
pathfinder.nodes_queue_to_checkpoint.append((100, 150))
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
path_status, new_vector = servo_wheels_manager.finish_translation_and_get_new_path_status_and_vector(
position, pathfinder)
new_vector_x, new_vector_y = new_vector
assert servo_wheels_manager.translation_status == TranslationStatus.MOVING
assert round(new_vector_x, 1) == 49.9
assert round(new_vector_y, 1) == 95.1
assert path_status == PathStatus.INTERMEDIATE_NODE_REACHED
def test_generate_new_vector_returns_correct_vector():
pathfinder = Pathfinder(ExecutionLogger())
pathfinder.robot_status = RobotStatus((20, 20), 90)
pathfinder.robot_status.target_position = (20, 20)
real_robot_position = (30, 30)
assert (pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
real_robot_position) == (-10, -10))
def test_given_start_of_heading_correction_for_translation_change_translation_status_and_send_rotation_vector_towards_standard_heading(
):
heading = 80
robot_status = RobotStatus((20, 20), 80)
wheels_controller = SimulatedWheelsController()
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
servo_wheels_manager.translating_start_heading_correction(
heading, robot_status, wheels_controller)
assert int(wheels_controller.last_degrees_of_rotation_given) == 10
assert servo_wheels_manager.translation_status == TranslationStatus.CORRECTING_HEADING
def test_if_not_close_enough_to_next_node_carry_on_to_next_node_without_changing_anything(
):
pathfinder = Pathfinder(ExecutionLogger())
pathfinder.robot_status = RobotStatus((20, 20), 90)
pathfinder.robot_status.target_position = (30, 30)
pathfinder.nodes_queue_to_checkpoint.append((20, 40))
path_status, new_vector = pathfinder.get_vector_to_next_node((25, 25))
assert path_status == PathStatus.MOVING_TOWARDS_TARGET
assert new_vector is None
assert pathfinder.robot_status.target_position == (30, 30)
def test_if_close_enough_to_next_node_and_it_is_checkpoint_return_checkpoint_reached_status(
):
pathfinder = Pathfinder(ExecutionLogger())
pathfinder.robot_status = RobotStatus((20, 20), 90)
pathfinder.robot_status.target_position = (30, 30)
pathfinder.nodes_queue_to_checkpoint.clear()
path_status, new_vector = pathfinder.get_vector_to_next_node(
(29.91, 29.91))
assert path_status == PathStatus.CHECKPOINT_REACHED
assert new_vector is None
assert pathfinder.robot_status.target_position == (29.91, 29.91)
def test_if_close_enough_to_next_node_switch_to_new_vector_and_return_moving_to_checkpoint_status(
):
pathfinder = Pathfinder(ExecutionLogger())
pathfinder.robot_status = RobotStatus((20, 20), 90)
pathfinder.robot_status.target_position = (30, 30)
pathfinder.nodes_queue_to_checkpoint.append((20, 40))
path_status, new_vector = pathfinder.get_vector_to_next_node(
(29.91, 29.91))
assert path_status == PathStatus.INTERMEDIATE_NODE_REACHED
assert new_vector == (-9.91, 10.09)
assert pathfinder.robot_status.target_position == (20, 40)
class Pathfinder():
def __init__(self, logger):
self.logger = logger
self.game_map = GameMap()
self.figures = FiguresInformation()
self.robot_status = None
self.graph = None
self.nodes_queue_to_checkpoint = deque() # in cm
self.filtered_nodes_queue_to_checkpoint = deque() # TODO: remove this
def reinitialize(self):
self.robot_status = None
self.graph = None
self.nodes_queue_to_checkpoint = deque()
self.figures.compute_positions((0, 0), (0, 231), (112, 231), (112, 0))
self.game_map = GameMap()
def set_game_map(self, game_map_data):
robot_information = game_map_data.get("robot")
if robot_information:
self.logger.log(
"Pathfinder - Assigning start position = {0} and orientation = {1}"
.format(robot_information[0], robot_information[1]))
self.robot_status = RobotStatus(robot_information[0],
robot_information[1])
else:
self.robot_status = RobotStatus((20, 20), 90)
table_corners_positions = game_map_data.get("table_corners")
if table_corners_positions:
self.logger.log(
"Pathfinding - Assigning table corner positions: {0}".format(
table_corners_positions))
self.figures.compute_positions(table_corners_positions[0],
table_corners_positions[1],
table_corners_positions[2],
table_corners_positions[3])
else:
self.figures.compute_positions((0, 0), (0, 231), (112, 231),
(112, 0))
obstacles = game_map_data.get("obstacles")
if obstacles:
self.logger.log(
"Pathfinder - Assigning obstacles: {0}".format(obstacles))
self.graph = Graph()
southeastern_x = int(table_corners_positions[0][0])
southeastern_y = int(table_corners_positions[0][1])
northwestern_x = int(table_corners_positions[2][0])
northwestern_y = int(table_corners_positions[2][1])
int_obstacles = []
for obstacle in obstacles:
x, y = obstacle[0]
int_obstacles.append([(int(x), int(y)), obstacle[1]])
self.graph.initialize_graph_matrix(
(southeastern_x, southeastern_y),
(northwestern_x, northwestern_y), int_obstacles)
else:
self.graph.initialize_graph_matrix((0, 0), (112, 231), [])
drawing_zone_corners = game_map_data.get("drawing_zone")
if drawing_zone_corners:
self.logger.log(
"Pathfinding - Assigning drawing zone corners: {0}".format(
drawing_zone_corners))
self.game_map.set_drawing_zone_borders(
game_map_data.get("drawing_zone"))
self.game_map.set_antenna_search_points(table_corners_positions[3])
else:
self.game_map.set_drawing_zone_borders(
((26, 27), (26, 87), (86, 87), (86, 27)))
self.game_map.set_antenna_search_points((112, 0))
def get_vector_to_next_node(self, current_robot_position=None):
if current_robot_position:
self.robot_status.set_position(current_robot_position)
if self.robot_status.position_has_reached_target_position_within_threshold(
):
if self.nodes_queue_to_checkpoint:
new_vector = self.robot_status.generate_new_translation_vector_towards_new_target(
self.nodes_queue_to_checkpoint.popleft())
return PathStatus.INTERMEDIATE_NODE_REACHED, new_vector
else:
self.robot_status.generate_new_translation_vector_towards_new_target(
self.robot_status.get_position())
return PathStatus.CHECKPOINT_REACHED, None
else:
return PathStatus.MOVING_TOWARDS_TARGET, None
def get_point_of_interest(self, point_of_interest):
return self.game_map.get_point_of_interest(point_of_interest)
def get_current_path(self):
path = deque()
for node in self.nodes_queue_to_checkpoint:
path.append(node)
path.appendleft(self.robot_status.target_position)
path.appendleft(self.robot_status.get_position())
return path
def generate_path_to_checkpoint_a_to_b(self, checkpoint_position):
self.nodes_queue_to_checkpoint.clear()
self.nodes_queue_to_checkpoint.append(checkpoint_position)
self.robot_status.generate_new_translation_vector_towards_new_target(
self.nodes_queue_to_checkpoint.popleft())
def generate_path_to_checkpoint(self, checkpoint_position):
self.nodes_queue_to_checkpoint.clear()
checkpoint_i, checkpoint_j = self.graph.get_grid_element_index_from_position(
checkpoint_position)
if self.graph.matrix[checkpoint_i][checkpoint_j] == math.inf:
raise CheckpointNotAccessibleError(
"This checkpoint is not accessible.")
source_vertex = self.graph.get_grid_element_index_from_position(
self.robot_status.get_position())
current_vertex = source_vertex
destination_vertex = self.graph.get_grid_element_index_from_position(
checkpoint_position)
visited_vertices = []
unsolved_vertices = dict(
((i, j), math.inf) for j in range(self.graph.matrix_height)
for i in range(self.graph.matrix_width)
if self.graph.get_weight_of_element((i, j)) != math.inf)
parent_of_vertices = defaultdict()
# Initialize weight of source vertex to 0
unsolved_vertices[current_vertex] = 0
vertices_weights = dict(unsolved_vertices)
# Generate vertices' parents dictionary
while current_vertex != destination_vertex:
current_vertex = min(unsolved_vertices, key=unsolved_vertices.get)
unsolved_vertices.pop(current_vertex)
visited_vertices.append(current_vertex)
for neighbour in self.graph.get_four_neighbours_indexes_from_element_index(
current_vertex):
if neighbour in unsolved_vertices:
new_weight = vertices_weights[
current_vertex] + self.graph.get_edge_distance(
current_vertex, neighbour)
if new_weight < vertices_weights[neighbour]:
vertices_weights[neighbour] = new_weight
unsolved_vertices[neighbour] = new_weight
parent_of_vertices[neighbour] = current_vertex
if vertices_weights[destination_vertex] == math.inf:
raise CheckpointNotAccessibleError(
"This checkpoint is not accessible.")
# Rebuild path
while current_vertex != source_vertex:
self.nodes_queue_to_checkpoint.appendleft(
self.graph.get_position_from_grid_element_index(
*current_vertex))
current_vertex = parent_of_vertices[current_vertex]
# Remove superfluous nodes
self.nodes_queue_to_checkpoint = self.filter_path(
self.nodes_queue_to_checkpoint, PATH_FILTER_WIDTH)
self.nodes_queue_to_checkpoint = self.filter_path(
self.nodes_queue_to_checkpoint, 2)
self.robot_status.generate_new_translation_vector_towards_new_target(
self.nodes_queue_to_checkpoint.popleft())
def is_checkpoint_accessible(self, checkpoint_position):
checkpoint_i, checkpoint_j = self.graph.get_grid_element_index_from_position(
checkpoint_position)
return not self.graph.matrix[checkpoint_i][checkpoint_j] == math.inf
def filter_path(self, nodes_queue, filter_width):
points_of_discontinuity = deque([nodes_queue[0]])
current_point_index = 0
slope = self.compute_slope(nodes_queue, current_point_index,
filter_width)
while current_point_index < (len(nodes_queue) - filter_width):
current_point_index += 1
new_slope = self.compute_slope(nodes_queue, current_point_index,
filter_width)
if math.fabs(new_slope - slope) >= PATH_SLOPE_THRESHOLD:
points_of_discontinuity.append(
nodes_queue[current_point_index + filter_width - 2])
current_point_index = current_point_index + filter_width - 2
slope = new_slope
points_of_discontinuity.append(nodes_queue[-1])
return points_of_discontinuity
def compute_slope(self, nodes_queue, start_index, length_to_consider):
try:
dx = nodes_queue[start_index + length_to_consider -
1][0] - nodes_queue[start_index][0]
dy = nodes_queue[start_index + length_to_consider -
1][1] - nodes_queue[start_index][1]
slope = dy / dx
except ZeroDivisionError:
slope = math.inf
return slope
""" Unit tests for servo wheel manager """
from design.interfacing.simulated_controllers import SimulatedWheelsController
from design.pathfinding.constants import TranslationStatus, RotationStatus
from design.pathfinding.pathfinder import Pathfinder, PathStatus
from design.pathfinding.robot_status import RobotStatus
from design.pathfinding.servo_wheels_manager import ServoWheelsManager
from design.utils.execution_logger import ExecutionLogger
heading = 80
robot_status = RobotStatus((20, 20), 80)
wheels_controller = SimulatedWheelsController()
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
def test_given_start_of_heading_correction_for_translation_change_translation_status_and_send_rotation_vector_towards_standard_heading(
):
heading = 80
robot_status = RobotStatus((20, 20), 80)
wheels_controller = SimulatedWheelsController()
servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
servo_wheels_manager.translating_start_heading_correction(
heading, robot_status, wheels_controller)
assert int(wheels_controller.last_degrees_of_rotation_given) == 10
assert servo_wheels_manager.translation_status == TranslationStatus.CORRECTING_HEADING
def test_given_start_of_position_correction_for_translation_change_translation_status_and_send_translation_vector_towards_current_target_node(
):
position = (40, 50)