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 find_movements_to_goal(self, robot_pose: RobotPose, goal_position):
if FIND_MOVEMENTS_FOR_REAL:
path = self._path_service.find_path(
robot_pose.get_position() + Position(140, 0),
goal_position,
)
return MovementFactory().create_movements(
path, robot_pose.get_orientation_in_degree())
else:
# TODO return hardcoded movements
pass
def _go_to_starting_zone_center(self, first_movement=False):
# TODO Maybe get this out of here to now look clanky at the beginning of the stage
self._rotation_service.rotate(CardinalOrientation.EAST.value)
robot_pose = self._find_robot_pose()
if first_movement:
pos_x = robot_pose.get_position().get_x_coordinate()
pos_y = robot_pose.get_position().get_y_coordinate() + 20
robot_pose = RobotPose(
Position(pos_x, pos_y), robot_pose.get_orientation_in_degree()
)
movements_to_starting_zone = self._find_movements_to_starting_zone(robot_pose)
self._move_robot(movements_to_starting_zone)
def find_movements_to_puck_zone(self, robot_pose: RobotPose):
path = self._path_service.find_path_to_puck_zone_center(
robot_pose.get_position())
return MovementFactory().create_movements(
path, robot_pose.get_orientation_in_degree())