diff --git a/remote_control/class_control_joystick.py b/remote_control/class_control_joystick.py index 77d6a0f..268a403 100644 --- a/remote_control/class_control_joystick.py +++ b/remote_control/class_control_joystick.py @@ -1,6 +1,229 @@ import socket import pygame import pygame.joystick +import rospy +import pygame.freetype +from marker_pos_angle.msg import id_pos_angle +from shapely.geometry import Polygon +import matplotlib.pyplot as plt +from shapely.geometry import Point + +from copy import deepcopy + +import time + + + + +# for debugging: +# rosrun image_view image_view image:=/fiducial_images + +class race_track: + """ + This class existing for a checking position of robots on the track + We have four aruco markers for outside rectangle and four for inside small rectangle + If one of the robots stay inside of the big rectangle we can to control its, but in another case works + only keyboard control - we use it for came back of robots in there + """ + + def __init__(self, robot_1, robot_2): + + marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback) + + 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 + self.checkpoint_line = None + self.check_number_markers = True + + + self.debug_plot = False + + self.checkpoint_count = 0 + self.finish_line_count = 0 + + self.checkpoint_count_2 = 0 + self.finish_line_count_2 = 0 + + + 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: + self.robot_pos_1 = (data.x, data.y) + # print ("{}->ID MARKER, {}->POSITION".format(data.id, self.robot_pos_1)) + elif data.id == 15: + 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 + + 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.rectangle_track_2 = Polygon([self.rectangle_pos_5, self.rectangle_pos_6, + self.rectangle_pos_7, self.rectangle_pos_8]) + + 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])]) + + 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.robot_pos_1_point = Point(self.robot_pos_1) + self.robot_pos_2_point = Point(self.robot_pos_2) + + if self.debug_plot: + x, y = (self.checkpoint_strength_line.exterior.xy) + plt.plot(x, y) + plt.show() + + + + if self.rectangle_track.contains(self.robot_pos_1_point) and not \ + self.rectangle_track_2.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): + + self.robot_2.controlling_allowed = True + else: + #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): # when you pressing B robot must staying on the start line for beginning of race + button_robot_1 = self.robot_1.joystick.get_button(1) + button_robot_2 = self.robot_2.joystick.get_button(1) + if button_robot_1: + if self.start_strength_line.contains(self.robot_pos_1_point): + print("player 1 is on the start line") + self.robot_1.controlling_allowed_2 = True + else: + print("put the player 1 on the start line") + self.robot_1.controlling_allowed_2 = False + if button_robot_2: + if self.start_strength_line.contains(self.robot_pos_2_point): + print("player 2 is on the start line") + self.robot_2.controlling_allowed_2 = True + else: + print("put the player 2 on the start line") + self.robot_2.controlling_allowed_2 = False + else: + print("no button pressed") + + def checkpoint(self): + if self.checkpoint_strength_line.contains(self.robot_pos_1_point): + if self.finish_line_count == self.checkpoint_count: + self.checkpoint_count += 1 + print("Player 1 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count)) + if self.checkpoint_strength_line.contains(self.robot_pos_2_point): + if self.finish_line_count_2 == self.checkpoint_count_2: + self.checkpoint_count_2 += 1 + print("Player 2 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count_2)) + + def finish_line(self): + if self.start_strength_line.contains(self.robot_pos_1_point): + if self.finish_line_count < self.checkpoint_count: + self.finish_line_count += 1 + print("reached finish line, finish line count = {}".format(self.finish_line_count)) + + elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 3: + print("Player1 finished") + self.robot_1.controlling_allowed = False + + if self.start_strength_line.contains(self.robot_pos_2_point): + if self.finish_line_count_2 < self.checkpoint_count_2: + self.finish_line_count_2 += 1 + print("Player 2 reached finish line, finish line count = {}".format(self.finish_line_count_2)) + + elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 3: + self.robot_2.controlling_allowed = False + self.player_2 = False + if not self.player_2: + print("Player_2 win") + self.player_2 = True class robot: # we have 4 arg for this class, because joysticks get the same (value, axis) events @@ -8,11 +231,15 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va self.joy = joy self.ip = ip self.port = port - self.robot0_stopped_1 = True - self.robot0_stopped_2 = True - self.robot0_stopped_3 = True - self.robot0_stopped_4 = True + self.robot_stopped = True self.rc_socket = socket.socket() + + self.update_control = False + + self.controlling_allowed = True + self.controlling_allowed_2 = False + self.joystick = pygame.joystick.Joystick(self.joy) + self.u1 = 0 self.u2 = 0 try: @@ -42,128 +269,262 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va + def check_control(self): + if not self.controlling_allowed: + if self.robot_stopped:# and self.robot0_stopped_2 and self.robot0_stopped_3 and self.robot0_stopped_4: + pass + #print("check control -> already stopped") + else: + self.u1 = self.u2 = 0 + self.robot_stopped = True + # self.robot0_stopped_2 = True + # self.robot0_stopped_3 = True + # self.robot0_stopped_4 = True + self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) + + print("check control -> stop") + + + + def control(self, event): # the control of two robots with joysticks - joy = event.joy - value = event.value - axis = event.axis + if self.controlling_allowed_2: + if self.controlling_allowed: + joy = event.joy + value = event.value*0.75 + axis = event.axis - if joy == self.joy: - if axis == 1: - if abs(value) > 0.2: - u1 = u2 = -value - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_1 = False + if joy == self.joy: + if axis == 1: + if abs(value) > 0.2: + u1 = u2 = -value + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot_stopped = False - elif not self.robot0_stopped_1: + elif not self.robot_stopped: + u1 = u2 = 0 + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot_stopped = True + elif axis == 3: + if abs(value) > 0.2: + u1 = value + u2 = -value + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot0_stopped_2 = False + + elif not self.robot0_stopped_2: + u1 = u2 = 0 + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot0_stopped_2 = True + elif axis == 2: + if value > 0.2: + u1 = value/1.9 + u2 = value/1.2 + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot0_stopped_3 = False + + elif not self.robot0_stopped_3: + u1 = u2 = 0 + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot0_stopped_3 = True + elif axis == 5: + if value > 0.2: + u1 = value/1.2 + u2 = value/1.9 + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot0_stopped_4 = False + + elif not self.robot0_stopped_4: + u1 = u2 = 0 + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot0_stopped_4 = True + + else: # not self.robot_stopped and not self.robot0_stopped_2 + if self.robot_stopped and self.robot0_stopped_2 and self.robot0_stopped_3 and self.robot0_stopped_4: + pass + print("check control -> already stopped") + else: u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_1 = True - elif axis == 3: - if abs(value) > 0.2: - u1 = value - u2 = -value - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_2 = False - - elif not self.robot0_stopped_2: - u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.robot_stopped = True self.robot0_stopped_2 = True - elif axis == 2: - if value > 0.2: - u1 = value/1.9 - u2 = value/1.2 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_3 = False - - elif not self.robot0_stopped_3: - u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) self.robot0_stopped_3 = True - elif axis == 5: - if value > 0.2: - u1 = value/1.2 - u2 = value/1.9 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) - self.robot0_stopped_4 = False - - elif not self.robot0_stopped_4: - u1 = u2 = 0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) self.robot0_stopped_4 = True + self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + + def control_alt(self): + if self.controlling_allowed and self.controlling_allowed_2: + ax1 = self.joystick.get_axis(1) * 0.75 + ax2 = self.joystick.get_axis(2) * 0.75 + ax3 = self.joystick.get_axis(3) * 0.75 + ax5 = self.joystick.get_axis(5) * 0.75 + + u1_old = deepcopy(self.u1) + u2_old = deepcopy(self.u2) + + threshold = 0.2 + if abs(ax1) < threshold and abs(ax2) < threshold and abs(ax3) < threshold and abs(ax5) < threshold and not self.robot_stopped: + self.robot_stopped = True + #print("all axis in neutral -> stopping") + self.u1 = self.u2 = 0 + #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()) + elif abs(ax1) > threshold: + self.u1 = self.u2 = -ax1 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + elif abs(ax3) > threshold: + self.u1 = ax3 + self.u2 = -ax3 + + if u1_old != self.u1 or u2_old != self.u2: + #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) + u2_old = deepcopy(self.u2) + if pressed[pygame.K_LEFT]: - u1 = -1.0 - u2 = 1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = -1.0 + self.u2 = 1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_RIGHT]: - u1 = 1.0 - u2 = -1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = 1.0 + self.u2 = -1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_UP]: - u1 = -1.0 - u2 = -1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = -1.0 + self.u2 = -1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_DOWN]: - u1 = 1.0 - u2 = 1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = 1.0 + self.u2 = 1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif event.type == pygame.KEYUP: - self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) + self.u1 = 0.0 + self.u2 = 0.0 + #selfd.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) - def control_keyboard_2(self, event): # keyboard control for robot1 + if u1_old != self.u1 or u2_old != self.u2: + print("update_control = True") + self.update_control = True + + def control_keyboard_2(self, event): # keyboard control for robot2 + + u1_old = deepcopy(self.u1) + u2_old = deepcopy(self.u2) if pressed[pygame.K_a]: - u1 = -1.0 - u2 = 1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = -1.0 + self.u2 = 1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_d]: - u1 = 1.0 - u2 = -1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = 1.0 + self.u2 = -1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_w]: - u1 = -1.0 - u2 = -1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = -1.0 + self.u2 = -1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif pressed[pygame.K_s]: - u1 = 1.0 - u2 = 1.0 - self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) + self.u1 = 1.0 + self.u2 = 1.0 + #self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) elif event.type == pygame.KEYUP: - self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode()) + self.u1 = 0.0 + self.u2 = 0.0 + #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") + self.update_control = True def main(): pygame.init() - pygame.display.set_mode((640, 480)) - + screen= pygame.display.set_mode((640, 480)) + robot_font = pygame.freetype.SysFont("shut up and play", 32) + robot_font_2 = pygame.freetype.SysFont("shut up and play", 22) + clock = pygame.time.Clock() + rospy.init_node('game_node', anonymous=True) robot_1 = robot(0, '192.168.1.102', 1234) robot_1.joystick_init() - robot_2 = robot(1, '192.168.1.103', 1234) + robot_2 = robot(1, '192.168.1.101', 1234) robot_2.joystick_init() + + robot_track = race_track(robot_1, robot_2) + time.sleep(1.0) + while True: + robot_track.check_number_marker() + robot_track.polygon_rectangle() + robot_track.checkpoint() + robot_track.finish_line() + + robot_1.check_control() + robot_2.check_control() + + events = pygame.event.get() global pressed pressed = pygame.key.get_pressed() + + robot_1.update_control = False + robot_2.update_control = False + input_1_recorded = False + input_2_recorded = False for event in events: + if event.type == pygame.JOYBUTTONDOWN: + robot_track.check_starting_position() + #robot_track.check_starting_position() if event.type == pygame.JOYAXISMOTION: - robot_1.control(event) - robot_2.control(event) + if event.joy == robot_2.joy: + robot_2.control_alt() + input_2_recorded = True + + if event.joy == robot_1.joy: + robot_1.control_alt() + input_1_recorded = True + else: robot_1.control_keyboard(event) robot_2.control_keyboard_2(event) + input_1_recorded = True + input_2_recorded = True + if input_2_recorded: + robot_2.send_control() + if input_1_recorded: + robot_1.send_control() + screen.fill((0, 0, 0)) + # You can use 'render' and then blit the text surface ... + + # or just 'render_to' the target surface. + robot_font.render_to(screen, (80, 50), "Become your weapon and fight!", (255, 255, 255)) + robot_font_2.render_to(screen, (280, 300), "PRESS 'B' FOR THE BEGINNING", (255, 130, 70)) + pygame.display.flip() + if __name__ == '__main__': - main() \ No newline at end of file + main()