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,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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue