docu/notes_intel_realsense.txt

@@ -1,44 +0,0 @@
-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,379 +1,6 @@
 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
@@ -381,15 +8,11 @@ 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.robot_stopped = True
+        self.robot0_stopped_1 = True
+        self.robot0_stopped_2 = True
+        self.robot0_stopped_3 = True
+        self.robot0_stopped_4 = 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:
@@ -419,199 +42,127 @@ class robot:  # we have 4 arg for this class, because joysticks get the same (va
 
 
 
-    def check_control(self):
+    def control(self, event):  # the control of two robots with joysticks
-        if not self.controlling_allowed:
+        joy = event.joy
-            if self.robot_stopped:
+        value = event.value
-                pass
+        axis = event.axis
-                #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")
+        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
 
+                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
 
-    def control_alt(self):
+                elif not self.robot0_stopped_2:
-        if self.controlling_allowed and self.controlling_allowed_2:
+                    u1 = u2 = 0
-            ax1 = self.joystick.get_axis(1) * 0.75
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
-            ax2 = self.joystick.get_axis(2) * 0.75
+                    self.robot0_stopped_2 = True
-            ax3 = self.joystick.get_axis(3) * 0.75
+            elif axis == 2:
-            ax5 = self.joystick.get_axis(5) * 0.75
+                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
 
-            u1_old = deepcopy(self.u1)
+                elif not self.robot0_stopped_3:
-            u2_old = deepcopy(self.u2)
+                    u1 = u2 = 0
-
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
-            threshold = 0.2
+                    self.robot0_stopped_3 = True
-            if abs(ax1) < threshold and ax2 < threshold and abs(ax3) < threshold and ax5 < threshold and not self.robot_stopped:
+            elif axis == 5:
-                self.robot_stopped = True
+                if value > 0.2:
-                #print("all axis in neutral -> stopping")
+                    u1 = value/1.2
-                self.u1 = self.u2 = 0
+                    u2 = value/1.9
-                #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
-            else:
+                    self.robot0_stopped_4 = False
-                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]:
-            self.u1 = -1.0
+            u1 = -1.0
-            self.u2 = 1.0
+            u2 = 1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif pressed[pygame.K_RIGHT]:
-            self.u1 = 1.0
+            u1 = 1.0
-            self.u2 = -1.0
+            u2 = -1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif pressed[pygame.K_UP]:
-            self.u1 = 1.0
+            u1 = -1.0
-            self.u2 = 1.0
+            u2 = -1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif pressed[pygame.K_DOWN]:
-            self.u1 = -1.0
+            u1 = 1.0
-            self.u2 = -1.0
+            u2 = 1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif event.type == pygame.KEYUP:
-            self.u1 = 0.0
+            self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
-            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:
+    def control_keyboard_2(self, event):  # keyboard control for robot1
-            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
+            u1 = -1.0
-            self.u2 = 1.0
+            u2 = 1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif pressed[pygame.K_d]:
-            self.u1 = 1.0
+            u1 = 1.0
-            self.u2 = -1.0
+            u2 = -1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif pressed[pygame.K_w]:
-            self.u1 = 1.0
+            u1 = -1.0
-            self.u2 = 1.0
+            u2 = -1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif pressed[pygame.K_s]:
-            self.u1 = -1.0
+            u1 = 1.0
-            self.u2 = -1.0
+            u2 = 1.0
-            #self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
         elif event.type == pygame.KEYUP:
-            self.u1 = 0.0
+            self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
-            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()
-    screen = pygame.display.set_mode((720, 576))
+    pygame.display.set_mode((640, 480))
-    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.101', 1234)
+    robot_2 = robot(1, '192.168.1.103', 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:
-                if event.joy == robot_2.joy:
+                robot_1.control(event)
-                    robot_2.control_alt()
+                robot_2.control(event)
-                    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(11, '192.168.1.103')]
+        self.robots = [Robot(15, '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(14, 0.275), Obstacle(15, 0.275)]
+        obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(13, 0.275), Obstacle(14, 0.275)]
 
         self.obstacles = {}
         for r in obst:
@@ -369,18 +369,10 @@ class RemoteController:
         markers_out = self.track.outer.values()
 
         # find targets:
-        lamb = 0.2
+        lamb = 0.3
-        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[j] = (p[0],p[1], 0.0)
+            targets[i] = (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
@@ -413,12 +405,6 @@ 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]
@@ -430,7 +416,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) % 7
+                        current_target = (current_target + 1) % 4
                         self.target = targets[current_target]
                         print("new target = {}".format(current_target))
 

roboflut/keyboard_controller.py

@@ -1,43 +0,0 @@
-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

@@ -1,180 +0,0 @@
-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)