RoboRally/remote_control/class_control_joystick.py

import sys
import socket
import pygame
import pygame.joystick
import rospy
import pygame.freetype
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

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, config_file):

        self.read_marker_config(config_file)

        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 = False

        marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)

        self.debug_plot = False

        self.checkpoint_count = 0
        self.finish_line_count = 0

        self.checkpoint_count_2 = 0
        self.finish_line_count_2 = 0
        self.countdown = False
        self.countdown_2 = False
        self.time_start = False
        self.countdown_check = False
        self.robot_1_time = False

        self.print_robot_1_time = False
        self.print_robot_2_time = False

        self.count_down_start = 0

        self.time_circle_robot_1 = 0
        self.time_circle_robot_2 = 0


        self.print_GO = False

        self.press_b_phrase = True
        self.robot_1_second_circle = None
        self.robot_1_first_circle = None
        self.robot_2_second_circle = None
        self.robot_2_first_circle = None
        self.final_time = None
        self.final_time_2 = None

    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 == self.robot_2.id:
            self.robot_pos_2 = (data.x, data.y)
            # 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])

    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.inside_track = Polygon(self.inside_markers.values())

            self.outside_track = Polygon(self.outside_markers.values())

            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])])

            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)

            if self.debug_plot:
                x, y = (self.checkpoint_strength_line.exterior.xy)
                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")
                self.robot_1.controlling_allowed = False

            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:
                #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

        button_robot_1 = robot_1.joystick.get_button(1)
        button_robot_2 = robot_2.joystick.get_button(1)
        if button_robot_1:
            if self.start_strength_line.contains(self.robot_pos_1_point):
                print("MLS is on the start line")
                self.robot_1.controlling_allowed_2 = True
                self.countdown = True
            else:
                print("put the MLS 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("BMW X6 is on the start line")
                self.robot_2.controlling_allowed_2 = True
                self.countdown_2 = True
            else:
                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()
            self.time_start = True
            self.countdown_check = True
            self.press_b_phrase = False
            self.countdown = False

    def check_countdown(self):
        # if countdown reaches 0 -> allow control
        if self.countdown_check:
            if time.time() - self.count_down_start < 5:
                self.robot_1.controlling_allowed_2 = False
                self.robot_2.controlling_allowed_2 = False

            else:
                self.print_GO = True

    def print_countdown(self):
        # prints countdown to screen
        if self.time_start:
            countdown_font = pygame.font.Font(None, 80)
            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))
            screen.blit(text, [260, 270])

    def time_robot_1_2(self):
        if self.robot_1_time and time.time() - self.count_down_start < 7.1:
            self.time_circle_robot_1 = time.time()
            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)
            time_robot_2_font_1.render_to(screen, (30, 20), "TIME MLS", (255, 0, 0))
            time_robot_1_font = pygame.font.Font(None, 52)
            output_time_robot_1 = "{: 6.2f}".format(time.time() - self.time_circle_robot_1)
            text = time_robot_1_font.render(output_time_robot_1, True, (255, 0, 0))
            screen.blit(text, [120, 90])
        if not self.final_time is None:
            self.print_robot_1_time = False

        if self.print_robot_2_time:
            time_robot_2_font_2 = pygame.freetype.SysFont(None, 42)
            time_robot_2_font_2.render_to(screen, (380, 20), "TIME BMW X6", (150, 50, 255))
            time_robot_2_font = pygame.font.Font(None, 52)
            output_time_robot_2 = "{: 6.2f}".format(time.time() - self.time_circle_robot_2)
            text = time_robot_2_font.render(output_time_robot_2, True, (150, 50, 255))
            screen.blit(text, [470, 90])
        if not self.final_time_2 is None:
            self.print_robot_2_time = False

    def print_result(self):
        if not self.final_time is None:
            final_time_robot_1_font = pygame.font.Font(None, 36)
            final_time_robot_1 = "FINAL TIME MLS  {: 6.2f}".format(self.final_time)
            text = final_time_robot_1_font.render(final_time_robot_1, True, (0, 255, 0))
            screen.blit(text, [30, 300])

        if not self.robot_1_second_circle is None:
            second_circle_robot_1_font = pygame.font.Font(None, 32)
            second_circle_robot_1 = "SECOND CIRCLE  {: 6.2f}".format(self.robot_1_second_circle)
            text = second_circle_robot_1_font.render(second_circle_robot_1, True, (255, 0, 0))
            screen.blit(text, [30, 200])

        if not self.robot_1_first_circle is None:
            first_circle_robot_1_font = pygame.font.Font(None, 32)
            first_circle_robot_1 = "FIRST CIRCLE  {: 6.2f}".format(self.robot_1_first_circle)
            text = first_circle_robot_1_font.render(first_circle_robot_1, True, (255, 0, 0))
            screen.blit(text, [30, 150])

        if not self.final_time_2 is None:
            final_time_robot_2_font = pygame.font.Font(None, 36)
            final_time_robot_2 = "FINAL TIME BMW X6  {: 6.2f}".format(self.final_time_2)
            text = final_time_robot_2_font.render(final_time_robot_2, True, (0, 255, 0))
            screen.blit(text, [380, 300])

        if not self.robot_2_second_circle is None:
            second_circle_robot_2_font = pygame.font.Font(None, 32)
            second_circle_robot_2 = "SECOND CIRCLE  {: 6.2f}".format(self.robot_2_second_circle)
            text = second_circle_robot_2_font.render(second_circle_robot_2, True, (150, 50, 255))
            screen.blit(text, [380, 200])

        if not self.robot_2_first_circle is None:
            first_circle_robot_2_font = pygame.font.Font(None, 32)
            first_circle_robot_2 = "FIRST CIRCLE  {: 6.2f}".format(self.robot_2_first_circle)
            text = first_circle_robot_2_font.render(first_circle_robot_2, True, (150, 50, 255))
            screen.blit(text, [380, 150])

    def first_phrase(self):

        if self.press_b_phrase:
            robot_font_2 = pygame.freetype.SysFont(None, 30)
            robot_font_2.render_to(screen, (240, 240), "BOTH PLAYERS!", (255, 160, 30))
            robot_font_2.render_to(screen, (30, 300), "PUT DOWN TWO ROBOTS ON THE START LINE", (255, 160, 30))
            robot_font_2.render_to(screen, (110, 360), "AND PRESS 'B' FOR THE BEGINNING", (255, 160, 30))
        if self.print_GO:
            if time.time() - self.count_down_start > 5 and time.time() - self.count_down_start < 7:
                print_go_font = pygame.freetype.SysFont(None, 152)
                print_go_font.render_to(screen, (230, 200), "GO!", (255, 160, 30))
                self.time_start = False
            else:
                self.robot_1.controlling_allowed_2 = True
                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):
                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.check_number_markers:
            if self.start_strength_line.contains(self.robot_pos_1_point):
                if self.finish_line_count < self.checkpoint_count:
                    self.finish_line_count += 1

                elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 3:
                    if self.final_time is None:
                        self.final_time = time.time() - self.time_circle_robot_1
                    self.robot_1.controlling_allowed = False

                elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 2:
                    if self.robot_1_second_circle is None:
                        self.robot_1_second_circle = time.time() - self.time_circle_robot_1

                elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 1:
                    if self.robot_1_first_circle is None:
                        self.robot_1_first_circle = time.time() - self.time_circle_robot_1

            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:
                    if self.final_time_2 is None:
                        self.final_time_2 = time.time() - self.time_circle_robot_2
                    self.robot_2.controlling_allowed = False
                    self.player_2 = False

                elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 2:
                    if self.robot_2_second_circle is None:
                        self.robot_2_second_circle = time.time() - self.time_circle_robot_2

                elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 1:
                    if self.robot_2_first_circle is None:
                        self.robot_2_first_circle = time.time() - self.time_circle_robot_2


class robot:  # we have 4 arg for this class, because joysticks get the same (value, axis) events
    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

        self.controlling_allowed = True
        self.controlling_allowed_2 = False


        self.u1 = 0
        self.u2 = 0
        try:
            self.rc_socket.connect((self.ip, self.port))
            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
        joystick_count = pygame.joystick.get_count()
        for count in range(joystick_count):
            if joystick_count == 2:
                self.joystick = pygame.joystick.Joystick(self.joy)
                print("{}-->joystick count".format(joystick_count))
                self.joystick.init()
                self.check_joystick = True
            elif joystick_count == 1:
                joystick = pygame.joystick.Joystick(0)
                joystick.init()
                print("connected only 1 joystick - {}".format(joystick))
                self.check_joystick = False
            elif not joystick_count:
                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")
            else:
                self.u1 = self.u2 = 0
                self.robot_stopped = True
                self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())

                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
            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 ax2 < threshold and abs(ax3) < threshold and 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
                elif ax2 > threshold:
                    self.u1 = ax2
                    self.u2 = ax2*0.75
                    #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
                elif ax5 > threshold:
                    self.u1 = ax5*0.75
                    self.u2 = ax5
            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]:
            self.u1 = -1.0
            self.u2 = 1.0
            #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())

        elif pressed[pygame.K_UP]:
            self.u1 = 1.0
            self.u2 = 1.0
            #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())

        elif event.type == pygame.KEYUP:
            self.u1 = 0.0
            self.u2 = 0.0
            #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")
            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]:
            self.u1 = -1.0
            self.u2 = 1.0
            #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())

        elif pressed[pygame.K_w]:
            self.u1 = 1.0
            self.u2 = 1.0
            #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())

        elif event.type == pygame.KEYUP:
            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():

    marker_config = 'track.cfg'

    global screen
    pygame.init()
    screen = pygame.display.set_mode((720, 480))
    rospy.init_node('game_node', anonymous=True)
    robot_1 = robot(0, '192.168.1.103', 1234, id=14)
    robot_1.joystick_init()
    robot_2 = robot(1, '192.168.1.101', 1234, id=11)
    robot_2.joystick_init()
    robot_track = race_track(robot_1, robot_2, marker_config)
    time.sleep(1.0)





    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()



        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_1, robot_2)
                robot_track.start_countdown()
            if event.type == pygame.JOYAXISMOTION:
                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()
        robot_track.time_robot_1_2()
        robot_track.print_countdown()
        robot_track.print_time_robot_1_2()
        robot_track.first_phrase()
        robot_track.print_result()
        pygame.display.flip()


if __name__ == '__main__':
    main()