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_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_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)
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_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
hsv_img = np.zeros((graph.matrix_width, graph.matrix_height, 3), np.uint8)
hsv_img[:, :] = (255, 255, 255)
for i in range(graph.matrix_width):
for j in range(graph.matrix_height):
if graph.matrix[i][j] == math.inf:
hsv_img[i, j] = (0, 0, 0)
else:
hsv_img[i, j] = (120 - (120 / MAXIMUM_GRID_NODE_HEIGHT) * graph.matrix[i][j], 255, 255)
img = cv2.cvtColor(hsv_img, cv2.COLOR_HSV2BGR)
pathfinder = Pathfinder(None)
robotStatus = RobotStatus(starting_point, 90)
pathfinder.graph = graph
pathfinder.robot_status = robotStatus
print("Calculating path")
try:
pathfinder.generate_path_to_checkpoint(destination)
for i in range(len(pathfinder.nodes_queue_to_checkpoint)):
x, y = pathfinder.graph.get_grid_element_index_from_position(pathfinder.nodes_queue_to_checkpoint[i])
cv2.rectangle(img, (y, x), (y + 1, x + 1), (114, 37, 116), 1)
if i < len(pathfinder.nodes_queue_to_checkpoint) - 1:
x2, y2 = pathfinder.graph.get_grid_element_index_from_position(
pathfinder.nodes_queue_to_checkpoint[i + 1])
cv2.line(img, (y, x), (y2, x2), (114, 37, 116), 1)
except CheckpointNotAccessibleError:
print("Checkpoint is not accessible!")
resized = cv2.resize(img, None, fx=7, fy=7, interpolation=cv2.INTER_LINEAR)