def debug_cmd(self):
if not self.is_debug:
return
angle = None
additional_dbg = []
if self.current_state == self.go_behind_ball:
angle = 18
elif self.current_state == self.grab_ball:
angle = 25
elif self.current_state == self.kick:
angle = 45
additional_dbg = [DebugCommandFactory.circle(self.game_state.ball_position, KICK_DISTANCE, color=RED)]
if angle is not None:
angle *= np.pi/180.0
base_angle = (self.game_state.ball.position - self.target.position).angle
magnitude = 3000
ori = self.game_state.ball.position
upper = ori + Position.from_angle(base_angle + angle, magnitude)
lower = ori + Position.from_angle(base_angle - angle, magnitude)
ball_to_player = self.player.position - self.game_state.ball_position
behind_player = (ball_to_player.norm + 1000) * normalize(ball_to_player) + self.game_state.ball_position
return [DebugCommandFactory.line(ori, upper),
DebugCommandFactory.line(ori, lower),
DebugCommandFactory.line(self.game_state.ball_position, behind_player, color=CYAN)] + additional_dbg
return []
def debug_cmd(self):
if self.current_state == self.defense:
return DebugCommandFactory().line(self.game_state.ball.position,
self._best_target_into_goal(),
timeout=0.1)
elif self.current_state == self.intercept and self.last_intersection is not None:
return DebugCommandFactory().line(self.game_state.ball.position,
self.last_intersection,
timeout=0.1)
else:
return []
def debug_cmd(self):
if self.wall_segment is None or self.player_number_in_formation != 0:
return []
return [DebugCommandFactory().line(self.wall_segment.p1,
self.wall_segment.p2,
timeout=0.1),
DebugCommandFactory().line(self.center_formation,
self.bisect_inter,
timeout=0.1),
DebugCommandFactory().line(self.game_state.ball_position,
self.game_state.field.our_goal_line.p1,
timeout=0.1),
DebugCommandFactory().line(self.game_state.ball_position,
self.game_state.field.our_goal_line.p2,
timeout=0.1)
]
def debug_cmd(self):
cmds = []
[
cmds.extend(DebugCommandFactory().area(area, color=VIOLET))
for area in self.forbidden_areas
]
return cmds
def main_loop(self):
engine_cmds = self.get_engine_commands()
game_state = self.tracker.update()
self.game_state.update(game_state)
self.controller.update(self.game_state, engine_cmds)
robot_state = self.controller.execute(self.dt)
self.robot_cmd_sender.send_packet(robot_state)
self.tracker.predict(robot_state, self.dt)
if any(robot.path for robot in self.controller.robots):
self.ui_send_queue.put_nowait(DebugCommandFactory.paths(self.controller.robots))
self.ui_send_queue.put_nowait(DebugCommandFactory.game_state(blue=self.game_state['blue'],
yellow=self.game_state['yellow'],
balls=self.game_state['balls']))
def _send_auto_state(self):
if self.play_executor.ref_states:
ref_state = self.play_executor.ref_states[-1]
msg = DebugCommandFactory().auto_play_info(
ref_state.info, ref_state.team_info,
self.play_executor.auto_play.info,
self.play_executor.autonomous_flag)
self.ui_send_queue.put(msg)
def _send_books(self):
msg = DebugCommandFactory().books(
strategy_book=self.play_state.strategy_book.strategies_roles,
strategy_default=self.play_state.strategy_book.default_strategies,
tactic_book=self.play_state.tactic_book.tactics_name,
tactic_default=self.play_state.tactic_book.default_tactics)
self.ui_send_queue.put(msg)
def main_loop(self):
engine_cmds = self.get_engine_commands()
game_state = self.tracker.update()
self.game_state.update(game_state)
self.controller.update(self.game_state, engine_cmds)
robot_state = self.controller.execute(self.dt)
self.robot_cmd_sender.send_packet(robot_state)
self.tracker.predict(robot_state, self.dt)
if any(robot.path for robot in self.controller.robots):
self.ui_send_queue.put_nowait(
DebugCommandFactory.paths(self.controller.robots))
self.ui_send_queue.put_nowait(
DebugCommandFactory.game_state(blue=self.game_state['blue'],
yellow=self.game_state['yellow'],
balls=self.game_state['balls']))
def send_debug(self, commands: Dict[int, Pose]):
#if not commands:
# return
robot_id = 3
if robot_id not in commands:
return
self.ui_send_queue.put_nowait(DebugCommandFactory.plot_point('mm/s',
'robot {} cmd speed'.format(robot_id),
[time.time()],
[commands[robot_id].norm]))
self.ui_send_queue.put_nowait(DebugCommandFactory.plot_point('rad/s',
'robot {} cmd rotation speed'.format(robot_id),
[time.time()],
[commands[robot_id].orientation]))
self.ui_send_queue.put_nowait(DebugCommandFactory.plot_point('mm/s',
'robot {} Kalman speed'.format(robot_id),
[time.time()],
[self[robot_id].velocity.norm]))
def __init__(self,
game_state: GameState,
player: Player,
target: Pose = Pose(),
args: List[str] = None):
super().__init__(game_state, player, target, args)
self.current_state = self.get_behind_ball
self.next_state = self.get_behind_ball
self.debug_interface = DebugCommandFactory()
self.move_action = self._generate_move_to()
self.last_ball_position = self.game_state.ball_position
self.charge_time = 0
self.last_time = time.time()
self.orientation_target = 0
self.target = target
def _send_robots_status(self):
states = self.play_state.current_tactical_state
if len(states) > 0:
cmd = DebugCommandFactory.robots_strategic_state(states)
self.ui_send_queue.put(cmd)