def next_position(self): self.target_orientation = (self.target.position - self.game_state.ball_position).angle self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: if time.time() - self.start_time >= self.rotate_time: self.rotation_sign = self._get_direction() if compare_angle(self.target_orientation, (self.game_state.ball_position - self.player.position).angle, VALID_DIFF_ANGLE): self.next_state = self.halt return self._go_to_final_position() elif time.time() - self.iter_time >= self.switch_time: self.iter_time = time.time() self._switch_rotation() if (self.player.pose.position - self.position).norm < VALID_DISTANCE: if self.start_time is None: self.start_time = time.time() self.iter_time = time.time() self.ball_collision = True self.speed = 1 self.offset_orientation += DIFF_ANGLE * self.rotation_sign self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: orientation = self.offset_orientation if time.time() - self.start_time < \ self.rotate_time else self.target_orientation else: orientation = self.target_orientation return CmdBuilder().addMoveTo(Pose(self.position, orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
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 _go_to_final_position(self): position = self.game_state.ball_position - Position.from_angle( self.target_orientation) * DISTANCE_FROM_BALL return CmdBuilder().addMoveTo( Pose(position, self.target_orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
def next_position(self): self.target_orientation = (self.target.position - self.game_state.ball_position).angle self.position = ( self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: if time.time() - self.start_time >= self.rotate_time: self.rotation_sign = self._get_direction() if compare_angle(self.target_orientation, (self.game_state.ball_position - self.player.position).angle, VALID_DIFF_ANGLE): self.next_state = self.halt return self._go_to_final_position() elif time.time() - self.iter_time >= self.switch_time: self.iter_time = time.time() self._switch_rotation() if (self.player.pose.position - self.position).norm < VALID_DISTANCE: if self.start_time is None: self.start_time = time.time() self.iter_time = time.time() self.ball_collision = True self.speed = 1 self.offset_orientation += DIFF_ANGLE * self.rotation_sign self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: orientation = self.offset_orientation if time.time() - self.start_time < \ self.rotate_time else self.target_orientation else: orientation = self.target_orientation return CmdBuilder().addMoveTo( Pose(self.position, orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
def _go_to_final_position(self): position = self.game_state.ball_position - Position.from_angle(self.target_orientation) * DISTANCE_FROM_BALL return CmdBuilder().addMoveTo(Pose(position, self.target_orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()