def _find_movements_to_command_panel_zone(self, robot_pose: RobotPose):
path = self._path_service.find_path_to_command_panel_zone(
robot_pose.get_position())
GameState.get_instance().set_current_planned_trajectory(path)
return self._movement_factory.create_movements(
path, robot_pose.get_orientation_in_degree())
def _read_resistance(self):
self._send_command_to_robot(Topic.READ_RESISTANCE, None)
resistance = self._wait_for_robot_confirmation(Topic.READ_RESISTANCE)
GameState.get_instance().set_resistance_value(resistance)
puck_colors = GameState.get_instance().get_resistance_value(
).get_colors()
print(puck_colors)
GameState.get_instance().set_puck_colors(puck_colors)
def _go_to_starting_zone_center(self, current_orientation: Orientation):
robot_pose = GameState.get_instance().get_robot_pose()
path = self._path_service.find_path_to_starting_zone_center(
robot_pose.get_position())
GameState.get_instance().set_current_planned_trajectory(path)
movements = self._movement_factory.create_movements(
path, current_orientation)
self._move_robot(movements)
def _add_puck_as_obstacle(self, starting_zone_corner_index: int, puck: Puck):
maze = GameState.get_instance().get_game_table().get_maze()
starting_zone = GameState.get_instance().get_game_table().get_starting_zone()
current_corner = GameState.get_instance().get_starting_zone_corners()[
starting_zone_corner_index
]
corner_position = starting_zone.find_corner_position_from_letter(current_corner)
maze.add_puck_as_obstacle(corner_position)
maze.remove_puck_as_obstacle(puck.get_position())
def _go_to_ohmmeter(self):
robot_pose = GameState.get_instance().get_robot_pose()
path = self._path_service.find_path_to_ohmmeter(
robot_pose.get_position())
GameState.get_instance().set_current_planned_trajectory(path)
movements = self._movement_factory.create_movements(
path, CardinalOrientation.EAST.value)
self._move_robot(movements)
def _return_to_starting_zone_from_resistance(self):
# TODO Maybe get this out of here to now look clanky at the beginning of the stage
robot_pose = GameState.get_instance().get_robot_pose()
path = self._path_service.find_path_to_starting_zone_center(
robot_pose.get_position())
GameState.get_instance().set_current_planned_trajectory(path)
movements = self._movement_factory.create_movements(
path, robot_pose.get_orientation_in_degree())
self._move_robot(movements)
def _analyze_command_panel(self):
resistance = GameState.get_instance().get_resistance_value()
self._send_command_to_robot(Topic.ANALYZE_COMMAND_PANEL, resistance)
read_corner = self._wait_for_robot_confirmation(
Topic.ANALYZE_COMMAND_PANEL)
next_corner = StartingZoneCorner.value_of_string(read_corner)
corners = [next_corner]
for _ in range(2):
next_corner = StartingZoneCorner.get_next_corner(next_corner)
corners.append(next_corner)
GameState.get_instance().set_starting_zone_corners(corners)
def _rotate_robot_towards_corner(self, starting_zone_corner_index):
starting_zone_corner_orientation = (
GameState.get_instance()
.get_starting_zone_corners()[starting_zone_corner_index]
.value
)
self._rotate_robot(starting_zone_corner_orientation)
def _go_forward_a_bit(self, starting_zone_index: int):
distance = Distance(0.25, unit_of_measure=UnitOfMeasure.METER)
starting_zone_corner = GameState.get_instance().get_starting_zone_corners()[
starting_zone_index
]
if (
starting_zone_corner == StartingZoneCorner.B
or starting_zone_corner == StartingZoneCorner.A
):
distance = Distance(0.22, unit_of_measure=UnitOfMeasure.METER)
movements = [Movement(Direction.FORWARD, distance)]
self._move_robot(movements)
def execute(self):
GameState.get_instance().set_current_stage(Stage.TRANSPORT_PUCK)
puck_colors = GameState.get_instance().get_puck_colors()
pucks_to_grab = [
GameState.get_instance().get_game_table().get_puck(color)
for color in puck_colors
]
for puck in pucks_to_grab:
print(puck.get_color(), puck.get_position())
self._send_command_to_robot(Topic.START_STAGE, Stage.TRANSPORT_PUCK)
self._wait_for_robot_confirmation(Topic.START_STAGE)
for starting_zone_corner_index, puck in enumerate(pucks_to_grab):
GameState.get_instance().set_current_puck(puck.get_color())
is_puck_in_a_corner = self._is_puck_in_a_corner(puck)
self._go_to_puck_zone()
self._go_to_puck_zone()
self._grab_puck(puck, is_puck_in_a_corner)
self._go_back_to_puck_zone()
self._go_to_starting_zone_center(first_movement=True)
self._go_to_starting_zone_center()
self._rotate_robot_towards_corner(starting_zone_corner_index)
self._go_forward_a_bit(starting_zone_corner_index)
self._drop_puck_on_corner()
self._add_puck_as_obstacle(starting_zone_corner_index, puck)
self._go_back_a_bit()
self._go_to_starting_zone_center()
self._send_command_to_robot(Topic.STAGE_COMPLETED, None)
self._wait_for_robot_confirmation(Topic.STAGE_COMPLETED)
def _create_straight_movement(
self,
direction: Direction,
goal_position: Position,
robot_pose: RobotPose,
):
# TODO Fix it so we calculate diagonal path to avoid it looking dumb in the UI
path = self._path_service.find_path(robot_pose.get_position(), goal_position)
GameState.get_instance().set_current_planned_trajectory(path)
position_to_use = (
robot_pose.get_gripper_position()
if direction is Direction.FORWARD
else robot_pose.get_position()
)
return [
self._movement_factory.create_movement_to_get_to_point_with_direction(
position_to_use,
goal_position,
direction,
)
]
def update_vision_state(vision_service: VisionService):
try:
table_image, robot_pose = vision_service.get_vision_state()
GameState.get_instance().set_table_image(table_image)
GameState.get_instance().set_robot_pose(robot_pose)
VisionWorker.last_known_pose = robot_pose
except RobotNotFoundException:
GameState.get_instance().set_robot_pose(
VisionWorker.last_known_pose)
def execute(self):
GameState.get_instance().set_current_stage(Stage.STOP)
self._start_stage()
self._move_to_starting_zone_center()
self._rotation_service.rotate(CardinalOrientation.WEST.value)
self._turn_on_led()
self._send_command_to_robot(Topic.STAGE_COMPLETED, None)
self._wait_for_robot_confirmation(Topic.STAGE_COMPLETED)
GameState.get_instance().end_game_cycle()
GameState.get_instance().set_current_stage(Stage.CYCLE_COMPLETED)
def _start_stage(self):
GameState.get_instance().set_current_stage(Stage.READ_COMMAND_PANEL)
self._send_command_to_robot(Topic.START_STAGE,
Stage.READ_COMMAND_PANEL)
self._wait_for_robot_confirmation(Topic.START_STAGE)
def _find_robot_pose(self):
return GameState.get_instance().get_robot_pose()
def _go_to_command_panel_zone(self):
self._rotation_service.rotate(CardinalOrientation.WEST.value)
robot_pose = GameState.get_instance().get_robot_pose()
movements_to_puck_zone = self._find_movements_to_command_panel_zone(
robot_pose)
self._move_robot(movements_to_puck_zone)
def start_game_cycle(self):
GameState.get_instance().start_game_cycle()
return {"is_game_started": True}
def _wait_for_robot_boot(self):
message = self._communication_service.receive_object()
if message.get_topic() == Topic.BOOT:
GameState.get_instance().set_robot_booted(True)
def run(self):
while True:
gripper_state = self.communication_service.receive_gripper_status()
GameState.get_instance().set_gripper_state(gripper_state)
battery_time_left = self.communication_service.receive_battery_time_left(
)
battery_percentage = self.communication_service.receive_battery_percentage(
)
power_consumption_first_wheel = (
self.communication_service.
receive_power_consumption_first_wheel())
power_consumption_second_wheel = (
self.communication_service.
receive_power_consumption_second_wheel())
power_consumption_third_wheel = (
self.communication_service.
receive_power_consumption_third_wheel())
power_consumption_fourth_wheel = (
self.communication_service.
receive_power_consumption_fourth_wheel())
power_consumption = self.communication_service.receive_power_consumption(
)
GameState.get_instance().set_battery_consumption(power_consumption)
GameState.get_instance().set_battery_time_left(battery_time_left)
GameState.get_instance().set_battery_percentage(battery_percentage)
GameState.get_instance().set_power_consumption_first_wheel(
power_consumption_first_wheel)
GameState.get_instance().set_power_consumption_second_wheel(
power_consumption_second_wheel)
GameState.get_instance().set_power_consumption_third_wheel(
power_consumption_third_wheel)
GameState.get_instance().set_power_consumption_fourth_wheel(
power_consumption_fourth_wheel)
time.sleep(1)
def test_whenInstantiate_thenInstancesAreEqual(self):
first_game_state = GameState.get_instance()
second_game_state = GameState.get_instance()
self.assertEqual(first_game_state, second_game_state)
if __name__ == "__main__":
context = TestContext()
server = ApplicationServer(MagicMock(), MagicMock(),
context._vision_worker, MagicMock())
server.run()
input("Ready to start, press enter to continue")
game_table = context._vision_service.create_game_table()
context._path_service.set_game_table(game_table)
puck_position = Position(1400, 600)
path = (context._shortest_path_algorithm.
find_shortest_path_with_cartesian_coordinates(
GameState.get_instance().get_robot_pose().get_position(),
puck_position))
movement_factory = MovementFactory()
robot_orientation = (
GameState.get_instance().get_robot_pose().get_orientation_in_degree())
movements = movement_factory.create_movements(path, robot_orientation)
maze_array = context._vision_service.create_game_table().get_maze()
table_image = context._world_camera.take_world_image()
for i, column in enumerate(maze_array):
for j, row in enumerate(column):
if maze_array[i][j] == 1:
table_image[i][j] = [255, 0, 255]
def _wait_for_input(self):
game_state: GameState = GameState.get_instance()
while True:
if game_state.is_game_cycle_started():
break
time.sleep(0.5)
def _start_stage(self):
GameState.get_instance().set_current_stage(Stage.GO_TO_OHMMETER)
self._send_command_to_robot(Topic.START_STAGE, Stage.GO_TO_OHMMETER)
self._wait_for_robot_confirmation(Topic.START_STAGE)
def assemble_from_game_state(self, game_state: GameState):
puck_colors = self._get_puck_colors(game_state.get_puck_colors())
current_puck = self._get_current_colors(game_state.get_current_puck())
current_stage = self._get_current_stage(game_state.get_current_stage())
gripper_state = self._get_gripper_state(game_state.get_gripper_state())
starting_zone_corner_order = self._get_starting_zone_corner_order(
game_state.get_starting_zone_corners()
)
robot_position = self._get_robot_position(game_state.get_robot_pose())
battery_consumption = self._get_battery_consumption(
game_state.get_battery_consumption()
)
is_game_started = game_state.is_game_cycle_started()
current_planned_trajectory = self._get_current_planned_trajectory(
game_state.get_current_planned_trajectory()
)
is_robot_booted = game_state.is_robot_booted()
battery_time_left = self._get_power_consumption_first_wheel(
game_state.get_battery_time_left()
)
battery_percentage = self._get_battery_time_left(
game_state.get_battery_percentage()
)
power_consumption_first_wheel = self._get_power_consumption_first_wheel(
game_state.get_power_consumption_first_wheel()
)
power_consumption_second_wheel = self._get_power_consumption_second_wheel(
game_state.get_power_consumption_second_wheel()
)
power_consumption_third_wheel = self._get_power_consumption_third_wheel(
game_state.get_power_consumption_third_wheel()
)
power_consumption_fourth_wheel = self._get_power_consumption_fourth_wheel(
game_state.get_power_consumption_fourth_wheel()
)
resistance_value = self._get_resistance_value(game_state.get_resistance_value())
return GameStateDto(
puck_colors,
current_puck,
current_stage,
gripper_state,
starting_zone_corner_order,
robot_position,
battery_consumption,
is_game_started,
is_robot_booted,
current_planned_trajectory,
battery_time_left,
battery_percentage,
power_consumption_first_wheel,
power_consumption_second_wheel,
power_consumption_third_wheel,
power_consumption_fourth_wheel,
resistance_value,
)
def get_robot_status(self):
game_state_dto = self._game_state_dto_assembler.assemble_from_game_state(
GameState.get_instance())
return jsonify(game_state_dto.__dict__)