added some images for assembly instructions

docu/notes_intel_realsense.txt

@@ -0,0 +1,44 @@
+Installing Intel RealSense drivers:
+###################################
+echo 'deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main' || sudo tee /etc/apt/sources.list.d/realsense-public.list
+sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main"
+sudo apt-get update
+sudo apt-get install librealsense2-dev
+sudo apt-get install librealsense2-dkms # enter a key for installing driver when asked (in my case the pw was realsense2019bt)
+sudo apt-get install librealsense2-utils
+
+Check if drivers were installed sucessfully:
+modinfo uvcvideo | grep "version:" # -> this should report the realsense camera driver
+
+
+# Installing the ROS package (assuming ros base is installed):
+Install dependencies:
+sudo apt-get install ros-kinetic-cv-bridge -y
+sudo apt-get install ros-kinetic-image-transport
+sudo apt-get install ros-kinetic-tf -y
+sudo apt-get install ros-kinetic-diagnostic-updater -y
+sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+sudo apt-get update
+sudo apt-get install ros-kinetic-ddynamic-reconfigure -y
+
+# clone github repo:
+git clone https://github.com/IntelRealSense/realsense-ros.git
+cd realsense-ros/
+git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1`
+mkdir -p ~/realsense_catkin_ws/src/realsense
+mv * ~/realsense_catkin_ws/src/realsense/
+
+cd ~/realsense_catkin_ws/src/
+catkin_init_workspace
+cd .. 
+catkin_make clean
+catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
+catkin_make install
+echo "source ~/realsense_catkin_ws/devel/setup.bash" >> ~/.bashrc
+source ~/.bashrc
+
+Test with:
+roslaunch realsense2_camera rs_camera.launch
+
+Possible issue:
+The camera requires a USB 3 port to provide full resolution streams

remote_control/class_control_joystick.py

@@ -1,6 +1,379 @@
 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
+        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 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, 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
@@ -8,11 +381,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.u1 = 0
         self.u2 = 0
         try:
@@ -42,127 +419,199 @@ class robot:  # we have 4 arg for this class, because joysticks get the same (va
 
 
 
-    def control(self, event):  # the control of two robots with joysticks
-        joy = event.joy
-        value = event.value
-        axis = event.axis
+    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())
 
-        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
+                print("check control -> stop")
 
-                elif not self.robot0_stopped_1:
-                    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.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
+    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
 
-                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
+            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())
 
-                elif not self.robot0_stopped_4:
-                    u1 = u2 = 0
-                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
-                    self.robot0_stopped_4 = True
 
     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():
+    global screen
     pygame.init()
-    pygame.display.set_mode((640, 480))
-
+    screen = pygame.display.set_mode((720, 576))
+    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()
+
+        robot_track.check_countdown()
+
+        screen.fill((0, 0, 0))
+
         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:
-                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()
+        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__':

remote_control/position_controller.py

@@ -124,14 +124,14 @@ def f_ode(t, x, u):
 class RemoteController:
     def __init__(self):
 
-        self.robots = [Robot(15, '192.168.1.103')]
+        self.robots = [Robot(11, '192.168.1.103')]
         #self.robots = [Robot(14, '192.168.1.102')]
 
         self.robot_ids = {}
         for r in self.robots:
             self.robot_ids[r.id] = r
 
-        obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(13, 0.275), Obstacle(14, 0.275)]
+        obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(14, 0.275), Obstacle(15, 0.275)]
 
         self.obstacles = {}
         for r in obst:
@@ -369,10 +369,18 @@ class RemoteController:
         markers_out = self.track.outer.values()
 
         # find targets:
-        lamb = 0.3
+        lamb = 0.2
+        j = 0
         for i in range(0,4):
             p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i]))
-            targets[i] = (p[0],p[1], 0.0)
+            targets[j] = (p[0],p[1], 0.0)
+            j += 1
+            if i < 3:
+                mean_in = (np.array(markers_in[i]) + np.array(markers_in[i+1])) * 0.5
+                mean_out = (np.array(markers_out[i]) + np.array(markers_out[i+1])) * 0.5
+                mean = mean_in + (1.0 - lamb) * (mean_out - mean_in)
+                targets[j] = (mean[0], mean[1], 0.0)
+                j += 1
 
         auto_control = False
         current_target = 0
@@ -405,6 +413,12 @@ class RemoteController:
                         #self.target = (-0.5,0.5, 0.0)
                         self.target = targets[3]
                     elif event.key == pygame.K_5:
+                        self.target = targets[4]
+                    elif event.key == pygame.K_6:
+                        self.target = targets[5]
+                    elif event.key == pygame.K_7:
+                        self.target = targets[6]
+                    elif event.key == pygame.K_SPACE:
                         auto_control = not auto_control
                         if auto_control:
                             self.target = targets[current_target]
@@ -416,7 +430,7 @@ class RemoteController:
                 if auto_control:
                     if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3:
                         print("close to target!")
-                        current_target = (current_target + 1) % 4
+                        current_target = (current_target + 1) % 7
                         self.target = targets[current_target]
                         print("new target = {}".format(current_target))
 

roboflut/keyboard_controller.py

@@ -0,0 +1,43 @@
+import socket
+import pygame
+
+pygame.init()
+pygame.display.set_mode((640, 480))
+
+rc_socket = socket.socket()
+try:
+    rc_socket.connect(('127.0.0.1', 1337)) # connect to robot
+except socket.error:
+    print("could not connect to socket")
+
+
+while True:
+    u1 = 0
+    u2 = 0
+    vmax = 10.0
+    events = pygame.event.get()
+    for event in events:
+        if event.type == pygame.KEYDOWN:
+            if event.key == pygame.K_LEFT:
+                u1 = -vmax / 10.0
+                u2 = vmax/ 10.0
+
+                print("turn left: ({},{})".format(u1, u2))
+            elif event.key == pygame.K_RIGHT:
+                u1 = vmax/ 10.0
+
+                u2 = -vmax/ 10.0
+
+                print("turn right: ({},{})".format(u1, u2))
+            elif event.key == pygame.K_UP:
+                u1 = vmax
+                u2 = vmax
+                print("forward: ({},{})".format(u1, u2))
+            elif event.key == pygame.K_DOWN:
+                u1 = -vmax 
+                u2 = -vmax 
+                print("backward: ({},{})".format(u1, u2))
+            rc_socket.send('({},{})\n'.format(u1, u2))
+        elif event.type == pygame.KEYUP:
+            print("key released, resetting: ({},{})".format(u1, u2))
+            rc_socket.send('({},{})\n'.format(u1, u2))

roboflut/roboflut.py

@@ -0,0 +1,180 @@
+import sys
+import socket
+import time
+import numpy as np
+
+import pygame
+
+from threading import Thread, Lock
+
+import scipy.integrate
+
+single_robot = True
+
+class Server:
+    def __init__(self):
+        self.ip = '127.0.0.1'
+        self.recv_port = 1337  # port for receiving data
+        self.send_port = 1338  # port for sending data
+        self.recv_addr = socket.getaddrinfo(self.ip, self.recv_port)[0][-1]
+
+        if not single_robot:
+            self.robots = []
+        else:
+            self.rob = Robot()
+
+        self.receiver = Thread(target=self.remote_control)
+        self.receiver.start()
+
+    def remote_control(self):
+        print("setting up socket communication ...")
+        recv_socket = socket.socket()
+        recv_socket.bind(self.recv_addr)
+        while True:
+            print("waiting for connections on {} ...".format(self.recv_addr))
+            recv_socket.listen(2)
+            (comm_socket, address) = recv_socket.accept() # this blocks until someone connects to the socket
+
+            print("starting thread for handling communication with {}".format(address))
+            Thread(target=self.handle_communication, args=(comm_socket, address)).start()
+
+    def handle_communication(self, socket, address):
+            print("connected!")
+            listening = True
+
+            if not single_robot:
+                rob = Robot()
+                self.robots.append(rob)
+
+            try:
+                while listening:
+                     # expected data: '(u1, u2)'\n"
+                     # where ui = control for motor i
+                     # ui \in [-1.0, 1.0]
+                    data = socket.recv(4096)
+                    data_str = data.decode()
+                    #print("Data received: {}".format(data_str))
+                    #print("processing data = {}".format(data_str))
+                    l = data_str.strip('()\n').split(',')
+                    #print("l = {}".format(l))
+                    u1 = float(l[0])
+                    #print("u1 = {}".format(u1))
+                    u2 = float(l[1])
+                    #print("u2 = {}".format(u2))
+
+                    if single_robot:
+                        self.rob.set_control(u1, u2)
+                    else:
+                        rob.set_control(u1, u2)
+            except ValueError:
+                print("ValueError: Data has wrong format.")
+                print("Data received: {}".format(data_str))
+            except IndexError:
+                print("IndexError: Data has wrong format.")
+                print("Data received: {}".format(data_str))
+            except Exception as e:
+                print("Some other error occured")
+                print("Exception: {}".format(e))
+            finally:
+                socket.close()
+                if not single_robot:
+                    self.robots.remove(rob)
+
+class Robot:
+    def __init__(self):
+        self.x = np.array([0.0, 0.0, 0.0])
+
+        self.u1 = 0.0
+        self.u2 = 0.0
+
+        self.r = 0.3
+        self.R = 0.5
+        self.d = 0.2
+
+        self.mutex = Lock()
+
+        self.omega_max = 50.0
+
+        self.integrator = scipy.integrate.ode(self.f)
+        self.integrator.set_integrator('dopri5')
+
+        self.simulator = Thread(target=self.simulate)
+        self.simulator.start()
+
+    def set_control(self, u1, u2):
+        self.mutex.acquire()
+        try:
+            self.u1 = u1
+            self.u2 = u2
+        finally:
+            self.mutex.release()
+
+    def simulate(self):
+        t0 = time.time()
+        t = time.time()-t0
+
+        while True:
+            self.mutex.acquire()
+            try:
+                self.integrator.set_f_params(self.omega_max * np.array([self.u1, self.u2]))
+            finally:
+                self.mutex.release()
+            self.integrator.set_initial_value(self.x, t)
+
+            dt = time.time() - t0 - t
+
+            self.x = self.integrator.integrate(self.integrator.t + dt)
+            t = time.time() - t0
+
+    def f(self, t, x, u):
+        r = self.r
+        d = self.d
+        R = self.R
+
+        theta = x[2]
+        omegar = u[0]
+        omegal = u[1]
+
+        dx = np.zeros((3,1))
+        dx[0] = (r/2.0 * np.cos(theta) - r * d/(2.0 * R) * np.sin(theta)) * omegar + \
+                (r/2.0 * np.cos(theta) + r * d/(2.0 * R) * np.sin(theta)) * omegal
+        dx[1] = (r/2.0 * np.sin(theta) + r * d/(2.0 * R) * np.cos(theta)) * omegar + \
+                (r/2.0 * np.sin(theta) - r * d/(2.0 * R) * np.cos(theta)) * omegal
+        dx[2] = r/(2.0 * R) * (omegar - omegal)
+
+        return dx
+
+def main(args):
+    # wait for connections of socket
+    s = Server()
+
+    background_colour = (255, 255, 255)
+    (width, height) = (640, 480)
+    screen = pygame.display.set_mode((width, height))
+    pygame.display.set_caption('Roboflut')
+    screen.fill(background_colour)
+    pygame.display.flip()
+
+    running = True
+    while running:
+        r = s.rob
+        #for r in s.robots:
+        x = int(r.x[0] + width/2.0)
+        y = int(r.x[1] + height / 2.0)
+        phi = r.x[2]
+        phases = ( phi + np.pi) % (2 * np.pi ) - np.pi
+
+        if x < 0 or x > width or y < 0 or y > height:
+            r.x[0] = 0.0
+            r.x[1] = 0.0
+        c = int((phases + 2.0 * np.pi) / (4.0 * np.pi) * 255)
+        #color = (c, c, c)
+        color = (0,0,0)
+
+        screen.set_at((x, y), color)
+
+        pygame.display.flip()
+
+
+if __name__ == '__main__':
+    main(sys.argv)