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