From 034d88394c7a02a8c1d345827db0ebb2d077dd75 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Fri, 12 Jul 2019 18:09:42 +0200 Subject: [PATCH] timer for game --- remote_control/class_control_joystick.py | 352 ++++++++++++++--------- 1 file changed, 220 insertions(+), 132 deletions(-) diff --git a/remote_control/class_control_joystick.py b/remote_control/class_control_joystick.py index 268a403..6574497 100644 --- a/remote_control/class_control_joystick.py +++ b/remote_control/class_control_joystick.py @@ -29,7 +29,6 @@ class race_track: def __init__(self, robot_1, robot_2): marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback) - self.robot_1 = robot_1 self.robot_2 = robot_2 @@ -55,8 +54,30 @@ class race_track: self.checkpoint_count_2 = 0 self.finish_line_count_2 = 0 + self.countdown = False + self.countdown_2 = False + self.time_start = False + self.countdown_check = False + self.robot_1_time = False + + self.print_robot_1_time = False + self.print_robot_2_time = False + + self.count_down_start = 0 + + self.time_circle_robot_1 = 0 + self.time_circle_robot_2 = 0 + self.print_GO = False + + self.press_b_phrase = True + self.robot_1_second_circle = None + self.robot_1_first_circle = None + self.robot_2_second_circle = None + self.robot_2_first_circle = None + self.final_time = None + self.final_time_2 = None def measurement_callback(self, data): # Aruco marker number 14 it's robot 102; Aruco 1,5,7,8 are drawn small rectangle Polygon # Aruco 2,4,0,11 are assembled big rectangle Polygon @@ -173,57 +194,186 @@ class race_track: else: print("couldn't find the all ARUCO markers") - def check_starting_position(self): # when you pressing B robot must staying on the start line for beginning of race - button_robot_1 = self.robot_1.joystick.get_button(1) - button_robot_2 = self.robot_2.joystick.get_button(1) + def check_starting_position(self, robot_1, robot_2): # when you pressing B robot must staying on the start line for beginning of race + + button_robot_1 = robot_1.joystick.get_button(1) + button_robot_2 = robot_2.joystick.get_button(1) if button_robot_1: if self.start_strength_line.contains(self.robot_pos_1_point): - print("player 1 is on the start line") + print("MLS is on the start line") self.robot_1.controlling_allowed_2 = True + self.countdown = True else: - print("put the player 1 on the start line") + print("put the MLS on the start line") self.robot_1.controlling_allowed_2 = False if button_robot_2: if self.start_strength_line.contains(self.robot_pos_2_point): - print("player 2 is on the start line") + print("BMW X6 is on the start line") self.robot_2.controlling_allowed_2 = True + self.countdown_2 = True else: - print("put the player 2 on the start line") + print("put the BMW X6 on the start line") self.robot_2.controlling_allowed_2 = False - else: - print("no button pressed") + + + def start_countdown(self): + if self.countdown and self.countdown_2: + self.count_down_start = time.time() + self.time_start = True + self.countdown_check = True + self.press_b_phrase = False + self.countdown = False + + def check_countdown(self): + # if countdown reaches 0 -> allow control + if self.countdown_check: + if time.time() - self.count_down_start < 5: + self.robot_1.controlling_allowed_2 = False + self.robot_2.controlling_allowed_2 = False + + else: + self.print_GO = True + + def print_countdown(self): + # prints countdown to screen + if self.time_start: + countdown_font = pygame.font.Font(None, 80) + ready_font= pygame.freetype.SysFont(None, 42) + ready_font.render_to(screen, (170, 200), "ARE YOU READY?", (255, 160, 30)) + output_string = "{: 6.2f}".format(time.time() - self.count_down_start) + text = countdown_font.render(output_string, True, (0, 255, 0)) + screen.blit(text, [260, 270]) + + def time_robot_1_2(self): + if self.robot_1_time and time.time() - self.count_down_start < 7.1: + self.time_circle_robot_1 = time.time() + self.time_circle_robot_2 = time.time() + self.print_robot_1_time = True + self.print_robot_2_time = True + def print_time_robot_1_2(self): + if self.print_robot_1_time: + time_robot_2_font_1 = pygame.freetype.SysFont(None, 42) + time_robot_2_font_1.render_to(screen, (30, 20), "TIME MLS", (255, 0, 0)) + time_robot_1_font = pygame.font.Font(None, 52) + output_time_robot_1 = "{: 6.2f}".format(time.time() - self.time_circle_robot_1) + text = time_robot_1_font.render(output_time_robot_1, True, (255, 0, 0)) + screen.blit(text, [120, 90]) + if not self.final_time is None: + self.print_robot_1_time = False + + if self.print_robot_2_time: + time_robot_2_font_2 = pygame.freetype.SysFont(None, 42) + time_robot_2_font_2.render_to(screen, (380, 20), "TIME BMW X6", (150, 50, 255)) + time_robot_2_font = pygame.font.Font(None, 52) + output_time_robot_2 = "{: 6.2f}".format(time.time() - self.time_circle_robot_2) + text = time_robot_2_font.render(output_time_robot_2, True, (150, 50, 255)) + screen.blit(text, [470, 90]) + if not self.final_time_2 is None: + self.print_robot_2_time = False + + def print_result(self): + if not self.final_time is None: + final_time_robot_1_font = pygame.font.Font(None, 36) + final_time_robot_1 = "FINAL TIME MLS {: 6.2f}".format(self.final_time) + text = final_time_robot_1_font.render(final_time_robot_1, True, (0, 255, 0)) + screen.blit(text, [30, 300]) + + if not self.robot_1_second_circle is None: + second_circle_robot_1_font = pygame.font.Font(None, 32) + second_circle_robot_1 = "SECOND CIRCLE {: 6.2f}".format(self.robot_1_second_circle) + text = second_circle_robot_1_font.render(second_circle_robot_1, True, (255, 0, 0)) + screen.blit(text, [30, 200]) + + if not self.robot_1_first_circle is None: + first_circle_robot_1_font = pygame.font.Font(None, 32) + first_circle_robot_1 = "FIRST CIRCLE {: 6.2f}".format(self.robot_1_first_circle) + text = first_circle_robot_1_font.render(first_circle_robot_1, True, (255, 0, 0)) + screen.blit(text, [30, 150]) + + if not self.final_time_2 is None: + final_time_robot_2_font = pygame.font.Font(None, 36) + final_time_robot_2 = "FINAL TIME BMW X6 {: 6.2f}".format(self.final_time_2) + text = final_time_robot_2_font.render(final_time_robot_2, True, (0, 255, 0)) + screen.blit(text, [380, 300]) + + if not self.robot_2_second_circle is None: + second_circle_robot_2_font = pygame.font.Font(None, 32) + second_circle_robot_2 = "SECOND CIRCLE {: 6.2f}".format(self.robot_2_second_circle) + text = second_circle_robot_2_font.render(second_circle_robot_2, True, (150, 50, 255)) + screen.blit(text, [380, 200]) + + if not self.robot_2_first_circle is None: + first_circle_robot_2_font = pygame.font.Font(None, 32) + first_circle_robot_2 = "FIRST CIRCLE {: 6.2f}".format(self.robot_2_first_circle) + text = first_circle_robot_2_font.render(first_circle_robot_2, True, (150, 50, 255)) + screen.blit(text, [380, 150]) + + def first_phrase(self): + + if self.press_b_phrase: + robot_font_2 = pygame.freetype.SysFont(None, 30) + robot_font_2.render_to(screen, (240, 240), "BOTH PLAYERS!", (255, 160, 30)) + robot_font_2.render_to(screen, (30, 300), "PUT DOWN TWO ROBOTS ON THE START LINE", (255, 160, 30)) + robot_font_2.render_to(screen, (110, 360), "AND PRESS 'B' FOR THE BEGINNING", (255, 160, 30)) + if self.print_GO: + if time.time() - self.count_down_start > 5 and time.time() - self.count_down_start < 7: + print_go_font = pygame.freetype.SysFont(None, 152) + print_go_font.render_to(screen, (230, 200), "GO!", (255, 160, 30)) + self.time_start = False + else: + self.robot_1.controlling_allowed_2 = True + self.robot_2.controlling_allowed_2 = True + self.robot_1_time = True + def checkpoint(self): - if self.checkpoint_strength_line.contains(self.robot_pos_1_point): - if self.finish_line_count == self.checkpoint_count: - self.checkpoint_count += 1 - print("Player 1 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count)) - if self.checkpoint_strength_line.contains(self.robot_pos_2_point): - if self.finish_line_count_2 == self.checkpoint_count_2: - self.checkpoint_count_2 += 1 - print("Player 2 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count_2)) + if self.check_number_markers: + if self.checkpoint_strength_line.contains(self.robot_pos_1_point): + if self.finish_line_count == self.checkpoint_count: + self.checkpoint_count += 1 + print("Player 1 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count)) + if self.checkpoint_strength_line.contains(self.robot_pos_2_point): + if self.finish_line_count_2 == self.checkpoint_count_2: + self.checkpoint_count_2 += 1 + print("Player 2 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count_2)) def finish_line(self): - if self.start_strength_line.contains(self.robot_pos_1_point): - if self.finish_line_count < self.checkpoint_count: - self.finish_line_count += 1 - print("reached finish line, finish line count = {}".format(self.finish_line_count)) + if self.check_number_markers: + if self.start_strength_line.contains(self.robot_pos_1_point): + if self.finish_line_count < self.checkpoint_count: + self.finish_line_count += 1 - elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 3: - print("Player1 finished") - self.robot_1.controlling_allowed = False + elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 3: + if self.final_time is None: + self.final_time = time.time() - self.time_circle_robot_1 + self.robot_1.controlling_allowed = False - if self.start_strength_line.contains(self.robot_pos_2_point): - if self.finish_line_count_2 < self.checkpoint_count_2: - self.finish_line_count_2 += 1 - print("Player 2 reached finish line, finish line count = {}".format(self.finish_line_count_2)) + elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 2: + if self.robot_1_second_circle is None: + self.robot_1_second_circle = time.time() - self.time_circle_robot_1 - elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 3: - self.robot_2.controlling_allowed = False - self.player_2 = False - if not self.player_2: - print("Player_2 win") - self.player_2 = True + elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 1: + if self.robot_1_first_circle is None: + self.robot_1_first_circle = time.time() - self.time_circle_robot_1 + + if self.start_strength_line.contains(self.robot_pos_2_point): + if self.finish_line_count_2 < self.checkpoint_count_2: + self.finish_line_count_2 += 1 + print("Player 2 reached finish line, finish line count = {}".format(self.finish_line_count_2)) + + elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 3: + if self.final_time_2 is None: + self.final_time_2 = time.time() - self.time_circle_robot_2 + self.robot_2.controlling_allowed = False + self.player_2 = False + + elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 2: + if self.robot_2_second_circle is None: + self.robot_2_second_circle = time.time() - self.time_circle_robot_2 + + elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 1: + if self.robot_2_first_circle is None: + self.robot_2_first_circle = time.time() - self.time_circle_robot_2 class robot: # we have 4 arg for this class, because joysticks get the same (value, axis) events @@ -238,7 +388,7 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va self.controlling_allowed = True self.controlling_allowed_2 = False - self.joystick = pygame.joystick.Joystick(self.joy) + self.u1 = 0 self.u2 = 0 @@ -271,86 +421,17 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va def check_control(self): if not self.controlling_allowed: - if self.robot_stopped:# and self.robot0_stopped_2 and self.robot0_stopped_3 and self.robot0_stopped_4: + if self.robot_stopped: pass #print("check control -> already stopped") else: self.u1 = self.u2 = 0 self.robot_stopped = True - # self.robot0_stopped_2 = True - # self.robot0_stopped_3 = True - # self.robot0_stopped_4 = True self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) print("check control -> stop") - - - def control(self, event): # the control of two robots with joysticks - if self.controlling_allowed_2: - if self.controlling_allowed: - joy = event.joy - value = event.value*0.75 - axis = event.axis - - if joy == self.joy: - if axis == 1: - if abs(value) > 0.2: - u1 = u2 = -value - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot_stopped = False - - elif not self.robot_stopped: - u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot_stopped = True - elif axis == 3: - if abs(value) > 0.2: - u1 = value - u2 = -value - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_2 = False - - elif not self.robot0_stopped_2: - u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_2 = True - elif axis == 2: - if value > 0.2: - u1 = value/1.9 - u2 = value/1.2 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_3 = False - - elif not self.robot0_stopped_3: - u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_3 = True - elif axis == 5: - if value > 0.2: - u1 = value/1.2 - u2 = value/1.9 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_4 = False - - elif not self.robot0_stopped_4: - u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_4 = True - - else: # not self.robot_stopped and not self.robot0_stopped_2 - if self.robot_stopped and self.robot0_stopped_2 and self.robot0_stopped_3 and self.robot0_stopped_4: - pass - print("check control -> already stopped") - else: - u1 = u2 = 0 - self.robot_stopped = True - self.robot0_stopped_2 = True - self.robot0_stopped_3 = True - self.robot0_stopped_4 = True - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - def control_alt(self): if self.controlling_allowed and self.controlling_allowed_2: ax1 = self.joystick.get_axis(1) * 0.75 @@ -362,7 +443,7 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va u2_old = deepcopy(self.u2) threshold = 0.2 - if abs(ax1) < threshold and abs(ax2) < threshold and abs(ax3) < threshold and abs(ax5) < threshold and not self.robot_stopped: + if abs(ax1) < threshold and ax2 < threshold and abs(ax3) < threshold and ax5 < threshold and not self.robot_stopped: self.robot_stopped = True #print("all axis in neutral -> stopping") self.u1 = self.u2 = 0 @@ -379,7 +460,13 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va elif abs(ax3) > threshold: self.u1 = ax3 self.u2 = -ax3 - + elif ax2 > threshold: + self.u1 = ax2 + self.u2 = ax2*0.75 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + elif ax5 > threshold: + self.u1 = ax5*0.75 + self.u2 = ax5 if u1_old != self.u1 or u2_old != self.u2: #print("update_control = True") self.update_control = True @@ -407,13 +494,13 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_UP]: - self.u1 = -1.0 - self.u2 = -1.0 + self.u1 = 1.0 + self.u2 = 1.0 #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_DOWN]: - self.u1 = 1.0 - self.u2 = 1.0 + self.u1 = -1.0 + self.u2 = -1.0 #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif event.type == pygame.KEYUP: @@ -441,13 +528,13 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_w]: - self.u1 = -1.0 - self.u2 = -1.0 + self.u1 = 1.0 + self.u2 = 1.0 #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_s]: - self.u1 = 1.0 - self.u2 = 1.0 + self.u1 = -1.0 + self.u2 = -1.0 #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif event.type == pygame.KEYUP: @@ -461,20 +548,21 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va def main(): + global screen pygame.init() - screen= pygame.display.set_mode((640, 480)) - robot_font = pygame.freetype.SysFont("shut up and play", 32) - robot_font_2 = pygame.freetype.SysFont("shut up and play", 22) - clock = pygame.time.Clock() + screen = pygame.display.set_mode((720, 576)) rospy.init_node('game_node', anonymous=True) robot_1 = robot(0, '192.168.1.102', 1234) robot_1.joystick_init() robot_2 = robot(1, '192.168.1.101', 1234) robot_2.joystick_init() - robot_track = race_track(robot_1, robot_2) time.sleep(1.0) + + + + while True: robot_track.check_number_marker() robot_track.polygon_rectangle() @@ -484,6 +572,9 @@ def main(): robot_1.check_control() robot_2.check_control() + robot_track.check_countdown() + + screen.fill((0, 0, 0)) events = pygame.event.get() global pressed @@ -495,8 +586,8 @@ def main(): input_2_recorded = False for event in events: if event.type == pygame.JOYBUTTONDOWN: - robot_track.check_starting_position() - #robot_track.check_starting_position() + robot_track.check_starting_position(robot_1, robot_2) + robot_track.start_countdown() if event.type == pygame.JOYAXISMOTION: if event.joy == robot_2.joy: robot_2.control_alt() @@ -515,16 +606,13 @@ def main(): robot_2.send_control() if input_1_recorded: robot_1.send_control() - - - screen.fill((0, 0, 0)) - # You can use 'render' and then blit the text surface ... - - # or just 'render_to' the target surface. - robot_font.render_to(screen, (80, 50), "Become your weapon and fight!", (255, 255, 255)) - robot_font_2.render_to(screen, (280, 300), "PRESS 'B' FOR THE BEGINNING", (255, 130, 70)) + robot_track.time_robot_1_2() + robot_track.print_countdown() + robot_track.print_time_robot_1_2() + robot_track.first_phrase() + robot_track.print_result() pygame.display.flip() if __name__ == '__main__': - main() + main() \ No newline at end of file