diff --git a/remote_control/class_control_joystick.py b/remote_control/class_control_joystick.py index 087336b..13a226a 100644 --- a/remote_control/class_control_joystick.py +++ b/remote_control/class_control_joystick.py @@ -17,8 +17,6 @@ from copy import deepcopy import time - - # for debugging: # rosrun image_view image_view image:=/fiducial_images @@ -37,14 +35,6 @@ class race_track: self.robot_1 = robot_1 self.robot_2 = robot_2 - self.rectangle_pos = None - self.rectangle_pos_2 = None - self.rectangle_pos_3 = None - self.rectangle_pos_4 = None - self.rectangle_pos_5 = None - self.rectangle_pos_6 = None - self.rectangle_pos_7 = None - self.rectangle_pos_8 = None self.robot_pos_1 = None self.robot_pos_2 = None self.start_line = None @@ -74,7 +64,6 @@ class race_track: self.time_circle_robot_1 = 0 self.time_circle_robot_2 = 0 - self.print_GO = False self.press_b_phrase = True @@ -114,12 +103,14 @@ class race_track: # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.robot_pos_1)) def check_number_marker(self): - self.check_number_markers = all(m is not None for m in self.outside_markers.values() + self.inside_markers.values() + [self.robot_pos_1] + [self.robot_pos_2]) + self.check_number_markers = all(m is not None for m in + self.outside_markers.values() + self.inside_markers.values() + [ + self.robot_pos_1] + [self.robot_pos_2]) def debug_output(self, screen): (screenwidth, screenheight) = screen.get_size() - inside = {k: v for k,v in self.inside_markers.iteritems() if v is not None} + inside = {k: v for k, v in self.inside_markers.iteritems() if v is not None} outside = {k: v for k, v in self.outside_markers.iteritems() if v is not None} markers = dict(inside, **outside) @@ -135,12 +126,13 @@ class race_track: if len(outside) >= 3: outside_transformed = [((x - minx) / (maxx - minx) * screenwidth, (y - miny) / (maxy - miny) * screenheight) - for (x, y) in outside.values()] + for (x, y) in outside.values()] pygame.draw.polygon(screen, (0, 255, 0), outside_transformed) if len(inside) >= 3: - inside_transformed = [((x-minx)/(maxx - minx) * screenwidth, (y-miny)/(maxy-miny) * screenheight) for (x,y) in inside.values()] + inside_transformed = [((x - minx) / (maxx - minx) * screenwidth, (y - miny) / (maxy - miny) * screenheight) + for (x, y) in inside.values()] pygame.draw.polygon(screen, (255, 0, 0), inside_transformed) @@ -152,13 +144,6 @@ class race_track: marker_font = pygame.freetype.SysFont(None, 20) marker_font.render_to(screen, (centerx, centery), "{}".format(k), (0, 0, 255)) - # if self.robot_pos_1 is not None: - # centerx = int((self.robot_pos_1[0] - minx) / (maxx - minx) * screenwidth) - # centery = int((self.robot_pos_1[1] - miny) / (maxy - miny) * screenheight) - # pygame.draw.circle(screen, (255, 0, 255), (centerx, centery), 10) - # marker_font = pygame.freetype.SysFont(None, 20) - # marker_font.render_to(screen, (centerx, centery), "{}".format(self.robot_1.id), (0, 0, 255)) - def polygon_rectangle(self): if self.check_number_markers: self.inside_track = Polygon(self.inside_markers.values()) @@ -168,19 +153,19 @@ class race_track: first_in = self.inside_markers.values()[0] first_out = self.outside_markers.values()[0] - self.start_strength_line = Polygon([(first_in[0]-0.08, first_in[1]), - (first_out[0]-0.08, first_out[1]), - (first_out[0]+0.08, first_out[1]), - (first_in[0]+0.08, first_in[1])]) + self.start_strength_line = Polygon([(first_in[0] - 0.08, first_in[1]), + (first_out[0] - 0.08, first_out[1]), + (first_out[0] + 0.08, first_out[1]), + (first_in[0] + 0.08, first_in[1])]) - n = len(self.inside_markers.keys())/2 + n = len(self.inside_markers.keys()) / 2 middle_in = self.inside_markers.values()[n] middle_out = self.outside_markers.values()[n] - self.checkpoint_strength_line = Polygon([(middle_in[0]-0.08, middle_in[1]), - (middle_out[0]-0.08, middle_out[1]), - (middle_out[0]+0.08, middle_out[1]), - (middle_in[0]+0.08, middle_in[1])]) + self.checkpoint_strength_line = Polygon([(middle_in[0] - 0.08, middle_in[1]), + (middle_out[0] - 0.08, middle_out[1]), + (middle_out[0] + 0.08, middle_out[1]), + (middle_in[0] + 0.08, middle_in[1])]) self.robot_pos_1_point = Point(self.robot_pos_1) self.robot_pos_2_point = Point(self.robot_pos_2) @@ -189,14 +174,12 @@ class race_track: plt.plot(x, y) plt.show() - - if self.outside_track.contains(self.robot_pos_1_point) and not \ self.inside_track.contains(self.robot_pos_1_point): self.robot_1.controlling_allowed = True else: - #print("robot outside track") + # print("robot outside track") self.robot_1.controlling_allowed = False if self.outside_track.contains(self.robot_pos_2_point) and not \ @@ -204,12 +187,13 @@ class race_track: self.robot_2.controlling_allowed = True else: - #print("robot_2 outside track") + # print("robot_2 outside track") self.robot_2.controlling_allowed = False else: print("couldn't find the all ARUCO markers") - def check_starting_position(self, robot_1, robot_2): # when you pressing B robot must staying on the start line for beginning of race + 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) @@ -230,7 +214,6 @@ class race_track: print("put the BMW X6 on the start line") self.robot_2.controlling_allowed_2 = False - def start_countdown(self): if self.countdown and self.countdown_2: self.count_down_start = time.time() @@ -253,7 +236,7 @@ class race_track: # prints countdown to screen if self.time_start: countdown_font = pygame.font.Font(None, 80) - ready_font= pygame.freetype.SysFont(None, 42) + 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)) @@ -265,6 +248,7 @@ class race_track: 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) @@ -340,7 +324,6 @@ class race_track: self.robot_2.controlling_allowed_2 = True self.robot_1_time = True - def checkpoint(self): if self.check_number_markers: if self.checkpoint_strength_line.contains(self.robot_pos_1_point): @@ -405,7 +388,6 @@ 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.u1 = 0 self.u2 = 0 try: @@ -416,6 +398,7 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va sys.exit(1) self.check_joystick = False + def joystick_init(self): # Joystick's initialisation joystick_count = pygame.joystick.get_count() for count in range(joystick_count): @@ -433,15 +416,11 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va print("no joysticks connected") self.check_joystick = False - - - - def check_control(self): if not self.controlling_allowed: if self.robot_stopped: pass - #print("check control -> already stopped") + # print("check control -> already stopped") else: self.u1 = self.u2 = 0 self.robot_stopped = True @@ -449,7 +428,6 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va print("check control -> stop") - def control_alt(self): if self.controlling_allowed and self.controlling_allowed_2: ax1 = self.joystick.get_axis(1) * 0.75 @@ -461,41 +439,40 @@ 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 ax2 < threshold and abs(ax3) < threshold and 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") + # print("all axis in neutral -> stopping") self.u1 = self.u2 = 0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) else: self.robot_stopped = False if abs(ax1) > threshold and abs(ax3) > threshold: - self.u1 = (-ax1 + ax3)/2.0 - self.u2 = (-ax1 - ax3)/2.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = (-ax1 + ax3) / 2.0 + self.u2 = (-ax1 - ax3) / 2.0 + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif abs(ax1) > threshold: self.u1 = self.u2 = -ax1 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) 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()) + self.u2 = ax2 * 0.75 + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif ax5 > threshold: - self.u1 = ax5*0.75 + self.u1 = ax5 * 0.75 self.u2 = ax5 if u1_old != self.u1 or u2_old != self.u2: - #print("update_control = True") + # print("update_control = True") self.update_control = True - def send_control(self): if self.update_control: - #print("sending..({},{})\n".format(self.u1, self.u2)) + # print("sending..({},{})\n".format(self.u1, self.u2)) self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) - def control_keyboard(self, event): # keyboard control for robot1 u1_old = deepcopy(self.u1) @@ -504,27 +481,27 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va if pressed[pygame.K_LEFT]: self.u1 = -1.0 self.u2 = 1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_RIGHT]: self.u1 = 1.0 self.u2 = -1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_UP]: self.u1 = 1.0 self.u2 = 1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_DOWN]: self.u1 = -1.0 self.u2 = -1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif event.type == pygame.KEYUP: self.u1 = 0.0 self.u2 = 0.0 - #selfd.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) + # selfd.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) if u1_old != self.u1 or u2_old != self.u2: print("update_control = True") @@ -538,27 +515,27 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va if pressed[pygame.K_a]: self.u1 = -1.0 self.u2 = 1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_d]: self.u1 = 1.0 self.u2 = -1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_w]: self.u1 = 1.0 self.u2 = 1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_s]: self.u1 = -1.0 self.u2 = -1.0 - #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + # self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif event.type == pygame.KEYUP: self.u1 = 0.0 self.u2 = 0.0 - #self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) + # self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) if u1_old != self.u1 or u2_old != self.u2: print("update_control = True") @@ -566,7 +543,6 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va def main(): - marker_config = 'track.cfg' global screen @@ -580,10 +556,6 @@ def main(): robot_track = race_track(robot_1, robot_2, marker_config) time.sleep(1.0) - - - - while True: screen.fill((0, 0, 0)) @@ -595,15 +567,11 @@ def main(): robot_track.checkpoint() robot_track.finish_line() - - robot_1.check_control() robot_2.check_control() robot_track.check_countdown() - - events = pygame.event.get() global pressed pressed = pygame.key.get_pressed()