diff --git a/remote_control/class_control_joystick.py b/remote_control/class_control_joystick.py index 38814ad..087336b 100644 --- a/remote_control/class_control_joystick.py +++ b/remote_control/class_control_joystick.py @@ -1,3 +1,4 @@ +import sys import socket import pygame import pygame.joystick @@ -7,6 +8,9 @@ from fiducial_transform.msg import id_pos_angle from shapely.geometry import Polygon import matplotlib.pyplot as plt from shapely.geometry import Point +import numpy as np + +from collections import OrderedDict from copy import deepcopy @@ -26,9 +30,10 @@ class race_track: only keyboard control - we use it for came back of robots in there """ - def __init__(self, robot_1, robot_2): + def __init__(self, robot_1, robot_2, config_file): + + self.read_marker_config(config_file) - marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback) self.robot_1 = robot_1 self.robot_2 = robot_2 @@ -44,8 +49,9 @@ class race_track: self.robot_pos_2 = None self.start_line = None self.checkpoint_line = None - self.check_number_markers = True + self.check_number_markers = False + marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback) self.debug_plot = False @@ -78,94 +84,103 @@ class race_track: self.robot_2_first_circle = None self.final_time = None self.final_time_2 = None - def measurement_callback(self, data): - # Aruco marker number 14 it's robot 102; Aruco 1,5,7,8 are drawn small rectangle Polygon - # Aruco 2,4,0,11 are assembled big rectangle Polygon - if data.id == 14: + def read_marker_config(self, config_file): + self.inside_markers = OrderedDict() + self.outside_markers = OrderedDict() + + f = open(config_file, 'r') + for l in f: + if len(l) > 0 and l[0] != '#': + ind = l.find(' ') + first = int(l[0:ind]) + second = int(l[ind:]) + + self.inside_markers[first] = None + self.outside_markers[second] = None + + def measurement_callback(self, data): + if data.id in self.inside_markers.keys(): + self.inside_markers[data.id] = (data.x, data.y) + + if data.id in self.outside_markers.keys(): + self.outside_markers[data.id] = (data.x, data.y) + + if data.id == self.robot_1.id: self.robot_pos_1 = (data.x, data.y) # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.robot_pos_1)) - elif data.id == 15: + elif data.id == self.robot_2.id: self.robot_pos_2 = (data.x, data.y) # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.robot_pos_1)) - elif data.id == 9: - self.checkpoint_line = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.start_line)) - elif data.id == 0: - self.start_line = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.start_line)) - - elif data.id == 1: - self.rectangle_pos = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.rectangle_pos)) - - elif data.id == 2: - self.rectangle_pos_2 = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.rectangle_pos_2)) - - elif data.id == 3: - self.rectangle_pos_3 = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id, start_marker_pos_3)) - - elif data.id == 4: - self.rectangle_pos_4 = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id,start_marker_pos_4)) - elif data.id == 5: - self.rectangle_pos_5 = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.rectangle_pos_5)) - - elif data.id == 6: - self.rectangle_pos_6 = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id,self.rectangle_pos_6)) - - elif data.id == 7: - self.rectangle_pos_7 = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.rectangle_pos_7)) - - elif data.id == 8: - self.rectangle_pos_8 = (data.x, data.y) - # print ("{}->ID MARKER, {}->POSITION".format(data.id,self.rectangle_pos_8)) def check_number_marker(self): - if self.rectangle_pos is None: - self.check_number_markers = False - elif self.rectangle_pos_2 is None: - self.check_number_markers = False - elif self.rectangle_pos_3 is None: - self.check_number_markers = False - elif self.rectangle_pos_4 is None: - self.check_number_markers = False - elif self.rectangle_pos_5 is None: - self.check_number_markers = False - elif self.rectangle_pos_6 is None: - self.check_number_markers = False - elif self.rectangle_pos_7 is None: - self.check_number_markers = False - elif self.rectangle_pos_8 is None: - self.check_number_markers = False - elif self.start_line is None: - self.check_number_markers = False - elif self.checkpoint_line is None: - self.check_number_markers = False - else: - self.check_number_markers = True + 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} + outside = {k: v for k, v in self.outside_markers.iteritems() if v is not None} + + markers = dict(inside, **outside) + if self.robot_pos_1 is not None: + markers[self.robot_1.id] = self.robot_pos_1 + if self.robot_pos_2 is not None: + markers[self.robot_2.id] = self.robot_pos_2 + + maxx = np.max([x for (x, y) in markers.values()]) + minx = np.min([x for (x, y) in markers.values()]) + maxy = np.max([y for (x, y) in markers.values()]) + miny = np.min([y for (x, y) in markers.values()]) + + if len(outside) >= 3: + outside_transformed = [((x - minx) / (maxx - minx) * screenwidth, (y - miny) / (maxy - miny) * screenheight) + 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()] + + pygame.draw.polygon(screen, (255, 0, 0), inside_transformed) + + for k in markers: + m = markers[k] + centerx = int((m[0] - minx) / (maxx - minx) * screenwidth) + centery = int((m[1] - miny) / (maxy - miny) * screenheight) + pygame.draw.circle(screen, (255, 255, 255), (centerx, centery), 10) + 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.rectangle_track = Polygon([self.rectangle_pos, self.rectangle_pos_2, - self.rectangle_pos_3, self.rectangle_pos_4]) + self.inside_track = Polygon(self.inside_markers.values()) - self.rectangle_track_2 = Polygon([self.rectangle_pos_5, self.rectangle_pos_6, - self.rectangle_pos_7, self.rectangle_pos_8]) + self.outside_track = Polygon(self.outside_markers.values()) - self.start_strength_line = Polygon([(self.rectangle_pos_7[0]-0.08, self.rectangle_pos_7[1]), - (self.start_line[0]-0.08, self.start_line[1]), - (self.start_line[0]+0.08, self.start_line[1]), - (self.rectangle_pos_7[0]+0.08, self.rectangle_pos_7[1])]) + first_in = self.inside_markers.values()[0] + first_out = self.outside_markers.values()[0] - self.checkpoint_strength_line = Polygon([self.checkpoint_line, self.rectangle_pos_5, - (self.rectangle_pos_5[0]+0.1, self.rectangle_pos_5[1]), - (self.checkpoint_line[0] + 0.1, self.checkpoint_line[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 + 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.robot_pos_1_point = Point(self.robot_pos_1) self.robot_pos_2_point = Point(self.robot_pos_2) @@ -176,16 +191,16 @@ class race_track: - if self.rectangle_track.contains(self.robot_pos_1_point) and not \ - self.rectangle_track_2.contains(self.robot_pos_1_point): + 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") self.robot_1.controlling_allowed = False - if self.rectangle_track.contains(self.robot_pos_2_point) and not \ - self.rectangle_track_2.contains(self.robot_pos_2_point): + if self.outside_track.contains(self.robot_pos_2_point) and not \ + self.inside_track.contains(self.robot_pos_2_point): self.robot_2.controlling_allowed = True else: @@ -377,12 +392,13 @@ class race_track: class robot: # we have 4 arg for this class, because joysticks get the same (value, axis) events - def __init__(self, joy, ip, port): + def __init__(self, joy, ip, port, id): self.joy = joy self.ip = ip self.port = port self.robot_stopped = True self.rc_socket = socket.socket() + self.id = id self.update_control = False @@ -394,8 +410,10 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va self.u2 = 0 try: self.rc_socket.connect((self.ip, self.port)) - except socket.error(): - print("couldn't connect to socket") + print("connection established to robot with ip {}".format(ip)) + except socket.error: + print("error: could not connect to robot with ip {}".format(ip)) + sys.exit(1) self.check_joystick = False def joystick_init(self): # Joystick's initialisation @@ -548,15 +566,18 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va def main(): + + marker_config = 'track.cfg' + global screen pygame.init() - screen = pygame.display.set_mode((720, 576)) + screen = pygame.display.set_mode((720, 480)) rospy.init_node('game_node', anonymous=True) - robot_1 = robot(0, '192.168.1.102', 1234) + robot_1 = robot(0, '192.168.1.103', 1234, id=14) robot_1.joystick_init() - robot_2 = robot(1, '192.168.1.101', 1234) + robot_2 = robot(1, '192.168.1.101', 1234, id=11) robot_2.joystick_init() - robot_track = race_track(robot_1, robot_2) + robot_track = race_track(robot_1, robot_2, marker_config) time.sleep(1.0) @@ -564,17 +585,24 @@ def main(): while True: + screen.fill((0, 0, 0)) + robot_track.check_number_marker() + + robot_track.debug_output(screen) + robot_track.polygon_rectangle() robot_track.checkpoint() robot_track.finish_line() + + robot_1.check_control() robot_2.check_control() robot_track.check_countdown() - screen.fill((0, 0, 0)) + events = pygame.event.get() global pressed diff --git a/remote_control/track.cfg b/remote_control/track.cfg new file mode 100644 index 0000000..4adffa8 --- /dev/null +++ b/remote_control/track.cfg @@ -0,0 +1,5 @@ +# inside markers | outside markers +1 5 +2 6 +3 7 +4 8 \ No newline at end of file