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

54
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,7 +103,9 @@ 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()
@ -140,7 +131,8 @@ class race_track:
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())
@ -189,8 +174,6 @@ 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):
@ -209,7 +192,8 @@ class race_track:
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()
@ -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,10 +416,6 @@ 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:
@ -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,7 +439,8 @@ 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
@ -489,13 +468,11 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
# 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)
@ -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()