removed unnecessary code and fixed formatting
parent
cd2fc8cc3b
commit
86b9ad659d
|
|
@ -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,7 +103,9 @@ 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()
|
||||
|
|
@ -140,7 +131,8 @@ class race_track:
|
|||
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())
|
||||
|
|
@ -189,8 +174,6 @@ 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):
|
||||
|
||||
|
|
@ -209,7 +192,8 @@ class race_track:
|
|||
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()
|
||||
|
|
@ -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,10 +416,6 @@ 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:
|
||||
|
|
@ -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,7 +439,8 @@ 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")
|
||||
self.u1 = self.u2 = 0
|
||||
|
|
@ -489,13 +468,11 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
|
|||
# print("update_control = True")
|
||||
self.update_control = True
|
||||
|
||||
|
||||
def send_control(self):
|
||||
if self.update_control:
|
||||
# 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)
|
||||
|
|
@ -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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue