Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

removed unnecessary code and fixed formatting

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-08-14 14:49:11 +02:00
parent cd2fc8cc3b
commit 86b9ad659d
1 changed files with 47 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
remote_control/class_control_joystick.py
View File

@ -17,8 +17,6 @@ from copy import deepcopy
import time import time
# for debugging: # for debugging:
# rosrun image_view image_view image:=/fiducial_images # rosrun image_view image_view image:=/fiducial_images
@ -37,14 +35,6 @@ class race_track:
self.robot_1 = robot_1 self.robot_1 = robot_1
self.robot_2 = robot_2 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_1 = None
self.robot_pos_2 = None self.robot_pos_2 = None
self.start_line = None self.start_line = None
@ -74,7 +64,6 @@ class race_track:
self.time_circle_robot_1 = 0 self.time_circle_robot_1 = 0
self.time_circle_robot_2 = 0 self.time_circle_robot_2 = 0
self.print_GO = False self.print_GO = False
self.press_b_phrase = True self.press_b_phrase = True
@ -114,12 +103,14 @@ class race_track:
# print ("{}->ID MARKER, {}->POSITION".format(data.id, self.robot_pos_1)) # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.robot_pos_1))
def check_number_marker(self): 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): def debug_output(self, screen):
(screenwidth, screenheight) = screen.get_size() (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} outside = {k: v for k, v in self.outside_markers.iteritems() if v is not None}
markers = dict(inside, **outside) markers = dict(inside, **outside)
@ -135,12 +126,13 @@ class race_track:
if len(outside) >= 3: if len(outside) >= 3:
outside_transformed = [((x - minx) / (maxx - minx) * screenwidth, (y - miny) / (maxy - miny) * screenheight) 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) pygame.draw.polygon(screen, (0, 255, 0), outside_transformed)
if len(inside) >= 3: 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) 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 = pygame.freetype.SysFont(None, 20)
marker_font.render_to(screen, (centerx, centery), "{}".format(k), (0, 0, 255)) 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): def polygon_rectangle(self):
if self.check_number_markers: if self.check_number_markers:
self.inside_track = Polygon(self.inside_markers.values()) self.inside_track = Polygon(self.inside_markers.values())
@ -168,19 +153,19 @@ class race_track:
first_in = self.inside_markers.values()[0] first_in = self.inside_markers.values()[0]
first_out = self.outside_markers.values()[0] first_out = self.outside_markers.values()[0]
self.start_strength_line = Polygon([(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_out[0]+0.08, first_out[1]), (first_out[0] + 0.08, first_out[1]),
(first_in[0]+0.08, first_in[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_in = self.inside_markers.values()[n]
middle_out = self.outside_markers.values()[n] middle_out = self.outside_markers.values()[n]
self.checkpoint_strength_line = Polygon([(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_out[0]+0.08, middle_out[1]), (middle_out[0] + 0.08, middle_out[1]),
(middle_in[0]+0.08, middle_in[1])]) (middle_in[0] + 0.08, middle_in[1])])
self.robot_pos_1_point = Point(self.robot_pos_1) self.robot_pos_1_point = Point(self.robot_pos_1)
self.robot_pos_2_point = Point(self.robot_pos_2) self.robot_pos_2_point = Point(self.robot_pos_2)
@ -189,14 +174,12 @@ class race_track:
plt.plot(x, y) plt.plot(x, y)
plt.show() plt.show()
if self.outside_track.contains(self.robot_pos_1_point) and not \ if self.outside_track.contains(self.robot_pos_1_point) and not \
self.inside_track.contains(self.robot_pos_1_point): self.inside_track.contains(self.robot_pos_1_point):
self.robot_1.controlling_allowed = True self.robot_1.controlling_allowed = True
else: else:
#print("robot outside track") # print("robot outside track")
self.robot_1.controlling_allowed = False self.robot_1.controlling_allowed = False
if self.outside_track.contains(self.robot_pos_2_point) and not \ 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 self.robot_2.controlling_allowed = True
else: else:
#print("robot_2 outside track") # print("robot_2 outside track")
self.robot_2.controlling_allowed = False self.robot_2.controlling_allowed = False
else: else:
print("couldn't find the all ARUCO markers") 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_1 = robot_1.joystick.get_button(1)
button_robot_2 = robot_2.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") print("put the BMW X6 on the start line")
self.robot_2.controlling_allowed_2 = False self.robot_2.controlling_allowed_2 = False
def start_countdown(self): def start_countdown(self):
if self.countdown and self.countdown_2: if self.countdown and self.countdown_2:
self.count_down_start = time.time() self.count_down_start = time.time()
@ -253,7 +236,7 @@ class race_track:
# prints countdown to screen # prints countdown to screen
if self.time_start: if self.time_start:
countdown_font = pygame.font.Font(None, 80) 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)) ready_font.render_to(screen, (170, 200), "ARE YOU READY?", (255, 160, 30))
output_string = "{: 6.2f}".format(time.time() - self.count_down_start) output_string = "{: 6.2f}".format(time.time() - self.count_down_start)
text = countdown_font.render(output_string, True, (0, 255, 0)) 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.time_circle_robot_2 = time.time()
self.print_robot_1_time = True self.print_robot_1_time = True
self.print_robot_2_time = True self.print_robot_2_time = True
def print_time_robot_1_2(self): def print_time_robot_1_2(self):
if self.print_robot_1_time: if self.print_robot_1_time:
time_robot_2_font_1 = pygame.freetype.SysFont(None, 42) 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_2.controlling_allowed_2 = True
self.robot_1_time = True self.robot_1_time = True
def checkpoint(self): def checkpoint(self):
if self.check_number_markers: if self.check_number_markers:
if self.checkpoint_strength_line.contains(self.robot_pos_1_point): 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 = True
self.controlling_allowed_2 = False self.controlling_allowed_2 = False
self.u1 = 0 self.u1 = 0
self.u2 = 0 self.u2 = 0
try: try:
@ -416,6 +398,7 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
sys.exit(1) sys.exit(1)
self.check_joystick = False self.check_joystick = False
def joystick_init(self): # Joystick's initialisation def joystick_init(self): # Joystick's initialisation
joystick_count = pygame.joystick.get_count() joystick_count = pygame.joystick.get_count()
for count in range(joystick_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") print("no joysticks connected")
self.check_joystick = False self.check_joystick = False
def check_control(self): def check_control(self):
if not self.controlling_allowed: if not self.controlling_allowed:
if self.robot_stopped: if self.robot_stopped:
pass pass
#print("check control -> already stopped") # print("check control -> already stopped")
else: else:
self.u1 = self.u2 = 0 self.u1 = self.u2 = 0
self.robot_stopped = True 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") print("check control -> stop")
def control_alt(self): def control_alt(self):
if self.controlling_allowed and self.controlling_allowed_2: if self.controlling_allowed and self.controlling_allowed_2:
ax1 = self.joystick.get_axis(1) * 0.75 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) u2_old = deepcopy(self.u2)
threshold = 0.2 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 self.robot_stopped = True
#print("all axis in neutral -> stopping") # print("all axis in neutral -> stopping")
self.u1 = self.u2 = 0 self.u1 = self.u2 = 0
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) # self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
else: else:
self.robot_stopped = False self.robot_stopped = False
if abs(ax1) > threshold and abs(ax3) > threshold: if abs(ax1) > threshold and abs(ax3) > threshold:
self.u1 = (-ax1 + ax3)/2.0 self.u1 = (-ax1 + ax3) / 2.0
self.u2 = (-ax1 - ax3)/2.0 self.u2 = (-ax1 - ax3) / 2.0
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) # self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif abs(ax1) > threshold: elif abs(ax1) > threshold:
self.u1 = self.u2 = -ax1 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: elif abs(ax3) > threshold:
self.u1 = ax3 self.u1 = ax3
self.u2 = -ax3 self.u2 = -ax3
elif ax2 > threshold: elif ax2 > threshold:
self.u1 = ax2 self.u1 = ax2
self.u2 = ax2*0.75 self.u2 = ax2 * 0.75
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) # self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif ax5 > threshold: elif ax5 > threshold:
self.u1 = ax5*0.75 self.u1 = ax5 * 0.75
self.u2 = ax5 self.u2 = ax5
if u1_old != self.u1 or u2_old != self.u2: if u1_old != self.u1 or u2_old != self.u2:
#print("update_control = True") # print("update_control = True")
self.update_control = True self.update_control = True
def send_control(self): def send_control(self):
if self.update_control: 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()) self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
def control_keyboard(self, event): # keyboard control for robot1 def control_keyboard(self, event): # keyboard control for robot1
u1_old = deepcopy(self.u1) 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]: if pressed[pygame.K_LEFT]:
self.u1 = -1.0 self.u1 = -1.0
self.u2 = 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]: elif pressed[pygame.K_RIGHT]:
self.u1 = 1.0 self.u1 = 1.0
self.u2 = -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]: elif pressed[pygame.K_UP]:
self.u1 = 1.0 self.u1 = 1.0
self.u2 = 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]: elif pressed[pygame.K_DOWN]:
self.u1 = -1.0 self.u1 = -1.0
self.u2 = -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: elif event.type == pygame.KEYUP:
self.u1 = 0.0 self.u1 = 0.0
self.u2 = 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: if u1_old != self.u1 or u2_old != self.u2:
print("update_control = True") 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]: if pressed[pygame.K_a]:
self.u1 = -1.0 self.u1 = -1.0
self.u2 = 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]: elif pressed[pygame.K_d]:
self.u1 = 1.0 self.u1 = 1.0
self.u2 = -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]: elif pressed[pygame.K_w]:
self.u1 = 1.0 self.u1 = 1.0
self.u2 = 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]: elif pressed[pygame.K_s]:
self.u1 = -1.0 self.u1 = -1.0
self.u2 = -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: elif event.type == pygame.KEYUP:
self.u1 = 0.0 self.u1 = 0.0
self.u2 = 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: if u1_old != self.u1 or u2_old != self.u2:
print("update_control = True") 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(): def main():
marker_config = 'track.cfg' marker_config = 'track.cfg'
global screen global screen
@ -580,10 +556,6 @@ def main():
robot_track = race_track(robot_1, robot_2, marker_config) robot_track = race_track(robot_1, robot_2, marker_config)
time.sleep(1.0) time.sleep(1.0)
while True: while True:
screen.fill((0, 0, 0)) screen.fill((0, 0, 0))
@ -595,15 +567,11 @@ def main():
robot_track.checkpoint() robot_track.checkpoint()
robot_track.finish_line() robot_track.finish_line()
robot_1.check_control() robot_1.check_control()
robot_2.check_control() robot_2.check_control()
robot_track.check_countdown() robot_track.check_countdown()
events = pygame.event.get() events = pygame.event.get()
global pressed global pressed
pressed = pygame.key.get_pressed() pressed = pygame.key.get_pressed()