def __init__(self):
self.bkb = SharedMemory() # Instance of a blackboard
config = ConfigParser() # Configuration file
try:
config.read('../../Control/Data/config.ini') # Reads the config archive
mem_key = int(config.get('Communication', 'no_player_robofei'))*100 # Get the memory key
except:
print "#----------------------------------#"
print "# Error loading config parser. #"
print "#----------------------------------#"
sys.exit()
self.Mem = self.bkb.shd_constructor(mem_key) # Create the link to the blackboard
self.args = parser.parse_args()
# Timestamp to use on the time step used for motion
self.timestamp = time.time()
# Clears the variables in the blackboard
self.bkb.write_float(self.Mem, 'VISION_BLUE_LANDMARK_DEG', -999)
self.bkb.write_float(self.Mem, 'VISION_RED_LANDMARK_DEG', -999)
self.bkb.write_float(self.Mem, 'VISION_YELLOW_LANDMARK_DEG', -999)
self.bkb.write_float(self.Mem, 'VISION_PURPLE_LANDMARK_DEG', -999)
def __init__(self):
self.bkb = SharedMemory()
config = ConfigParser()
self.args = parser.parse_args()
try:
config.read('../../Control/Data/config.ini')
self.robot_id = int(
config.get('Communication', 'no_player_robofei'))
mem_key = self.robot_id * 100
except: # noqa E722
if self.args.robot_id:
self.robot_id = self.args.robot_id
mem_key = self.robot_id * 100
print "Error loading ConfigParser."
print "Resuming with robot_id", self.robot_id
else:
print "Neither ConfigParser worked nor robot_id was given."
print "Finishing execution of Localization."
sys.exit()
self.Mem = self.bkb.shd_constructor(mem_key)
# Timestamp to use on the time step used for motion
self.timestamp = time.time()
class Slave:
def __init__(self, ID=1, name=None):
self.ID = ID
self.keywords = ['setPyPy', 'pycross()']
self.Pycross = []
self.MainFile = None
if len(sys.argv) > 1:
from SharedMemory import SharedMemory
self.ID = int(sys.argv[3])
self.SharedMemory = SharedMemory(0,
size=int(sys.argv[4]),
name=sys.argv[1])
self.FileSetUp()
self.mainLoop()
self.mainNet()
def FileSetUp(self):
with open(sys.argv[2], "r") as file:
preprocessing.filesetup(self.keywords, file.read())
self.MainFile = import_module('MainFile')
def mainNet(self):
host = socket.gethostbyname(socket.gethostname())
self.net = net(0, host, 1705)
stdout = output(self.net)
stderr = error(self.net)
sys.stdout = stdout
sys.stderr = stderr
while 1:
msg, data = self.net.recv()
msg = msg.replace(' ', '') #remove white spaces from the message
if len(msg):
end = self.net.msgToFunc[msg](data)
if end:
break
self.net.socket.close()
self.net.socket.close()
def mainLoop(self):
while 1:
data = self.SharedMemory.wait()
Hlen, ID, SRC, DST, MSG = data
data = self.SharedMemory.recv()
print(data)
funcName = data[0]
args = data[1]
kwargs = data[2]
Func = getattr(self.MainFile, funcName)
result = Func(*args, **kwargs)
self.SharedMemory.send(result, ID, self.ID, SRC)
def make_sharedmemory(self, size=2000):
from SharedMemory import SharedMemory
self.SharedMemory = SharedMemory(1, size=size)
self.slavePath = __file__[::-1].replace('pycross.py'[::-1],
'slave.py'[::-1], 1)[::-1]
if self.slavePath[-1] == "c":
self.slavePath = self.slavePath[:-1]
def __init__(self, ID=1, name=None):
self.ID = ID
self.keywords = ['setPyPy', 'pycross()']
self.Pycross = []
self.MainFile = None
if len(sys.argv) > 1:
from SharedMemory import SharedMemory
self.ID = int(sys.argv[3])
self.SharedMemory = SharedMemory(0,
size=int(sys.argv[4]),
name=sys.argv[1])
self.FileSetUp()
self.mainLoop()
self.mainNet()
def __init__(self):
# instantiate:
self.config = ConfigParser()
# looking for the file config.ini:
self.config.read('../../Control/Data/config.ini')
self.mem_key = int(
self.config.get('Communication', 'no_player_robofei')) * 100
#Instantiate the BlackBoard's class:
self.bkb = SharedMemory()
self.mem = self.bkb.shd_constructor(self.mem_key)
print
print 'Raw data - read (get) and write (set) methods'
print
def __init__(self):
self.bkb = SharedMemory() # Instance of a blackboard
config = ConfigParser() # Configuration file
try:
config.read(
'../../Control/Data/config.ini') # Reads the config archive
mem_key = int(
config.get('Communication',
'no_player_robofei')) * 100 # Get the memory key
except:
print "#----------------------------------#"
print "# Error loading config parser. #"
print "#----------------------------------#"
sys.exit()
self.Mem = self.bkb.shd_constructor(
mem_key) # Create the link to the blackboard
def __init__(self):
self.__bkb = SharedMemory(
) # Instantiating blackboard functions in C ++.
self.__mem = configparser.RawConfigParser(
) # Instantiating library to find robot number.
# Looking for config.ini file
if self.__mem.read(
'../Control/Data/config.ini'
) is not [] and "Communication" in self.__mem.sections(
) and "no_player_robofei" in self.__mem["Communication"].keys():
self.__mem = int(
self.__mem["Communication"]["no_player_robofei"]) * 100
else:
# Error while not finding file, section or number of robot.
raise VisionException(0, 'Could not read config, no robot number')
# Creating blackboard with config.ini read value.
self.__mem = self.__bkb.shd_constructor(self.__mem)
class MINDREADER():
def __init__(self):
self.bkb = SharedMemory() # Instance of a blackboard
config = ConfigParser() # Configuration file
try:
config.read(
'../../Control/Data/config.ini') # Reads the config archive
mem_key = int(
config.get('Communication',
'no_player_robofei')) * 100 # Get the memory key
except:
print "#----------------------------------#"
print "# Error loading config parser. #"
print "#----------------------------------#"
sys.exit()
self.Mem = self.bkb.shd_constructor(
mem_key) # Create the link to the blackboard
def main(self):
while True:
# self.bkb.write_int(self.Mem, 'DECISION_LOCALIZATION', -999)
# self.bkb.write_int(self.Mem, 'DECISION_LOCALIZATION', -999)
# print "CONTROL_ACTION", self.bkb.read_int(self.Mem, 'CONTROL_ACTION')
# print "VISION_FIRST_GOALPOST", self.bkb.read_float(self.Mem, 'VISION_FIRST_GOALPOST')
# print "VISION_SECOND_GOALPOST", self.bkb.read_float(self.Mem, 'VISION_SECOND_GOALPOST')
# print "VISION_THIRD_GOALPOST", self.bkb.read_float(self.Mem, 'VISION_THIRD_GOALPOST')
# print "VISION_FOURTH_GOALPOST", self.bkb.read_float(self.Mem, 'VISION_FOURTH_GOALPOST')
print "iVISION_FIELD", self.bkb.read_int(self.Mem, 'iVISION_FIELD')
print "fVISION_FIELD", self.bkb.read_float(self.Mem,
'fVISION_FIELD')
# print "iVISION_FIELD", self.bkb.read_int(self.Mem, 'iVISION_FIELD')
# print "fVISION_FIELD", self.bkb.read_float(self.Mem, 'fVISION_FIELD')
print "IMU_EULER_Z", self.bkb.read_float(self.Mem, 'IMU_EULER_Z')
print 'VISION_PAN_DEG', self.bkb.read_float(
self.Mem, 'VISION_PAN_DEG')
# x = self.bkb.read_int(self.Mem, 'VISION_FIELD')
# v = read(x)
print
time.sleep(0.5)
def addListener(self, name: str, timeout=1):
"""Method to add a Shared Memory Server
Args:
name (str): Shared Memory Name
timeout (int, optional): mutex timeout. Defaults to 1.
"""
self._checkNameExistOrNot(name, False)
self.listener[name] = SharedMemory(name,
client=False) #(name, timeout)
def addSender(self,
name: str,
value=None,
path: str = None,
size=10,
timeout=1):
"""Method to add a Shared Memory Client
Args:
name (str): Shared Memory name
value ([type], optional): value to share with the other module. Defaults to None.
path (str, optional): path to load JSON file and share the data inside. Defaults to None.
size (int, optional): or str value. Defaults to 10.
timeout (int, optional): mutex timeout. Defaults to 1.
"""
self._checkNameExistOrNot(name, False)
self.sender[name] = SharedMemory(
name, value, path, size,
client=True) #(name, value, path, size, timeout)
TILT = 750 # Centralizes camera vertically for calibration.
import sys
sys.path.append('../../Blackboard/src/')
from SharedMemory import SharedMemory
import cv2
import os
import ctypes
import argparse
import time
from runningvision import *
from servo import Servo
# Creates a black board object.
bkb = SharedMemory()
mem_key = 100 # Sets the memory key.
Mem = bkb.shd_constructor(mem_key) # Creates a shared memory.
# Parsing of arguments on execution.
parser = argparse.ArgumentParser(
description='Run Vision',
epilog='Vision program used for the Humanoid Running Challenge.')
parser.add_argument('--step',
'--s',
action="store_true",
help='Used for step challenge.')
parser.add_argument('--swerve',
'--w',
action="store_true",
class TreatingRawData(object):
def __init__(self):
# instantiate:
self.config = ConfigParser()
# looking for the file config.ini:
self.config.read('../../Control/Data/config.ini')
self.mem_key = int(
self.config.get('Communication', 'no_player_robofei')) * 100
#Instantiate the BlackBoard's class:
self.bkb = SharedMemory()
self.mem = self.bkb.shd_constructor(self.mem_key)
self.flag_move_ac = False
print
print 'Raw data - read (get) and write (set) methods'
print
#self.bkb.write_int(self.Mem,'VISION_SEARCH_BALL',1)
def get_referee_usage(self):
return self.config.get('Decision', 'referee')
def get_orientation_usage(self):
return self.config.get('Decision', 'orientation')
def get_distance_to_kick(self):
return self.config.get('Decision', 'distance_to_kick')
def get_referee(self):
return self.bkb.read_int(self.mem, 'COM_REFEREE')
def get_motor_tilt_degrees(self):
return int(self.bkb.read_float(self.mem, 'VISION_TILT_DEG'))
def get_motor_pan_degrees(self):
return int(self.bkb.read_float(self.mem, 'VISION_PAN_DEG'))
def get_angle_ball(self):
return self.bkb.read_float(self.mem, 'VISION_BALL_ANGLE')
def get_dist_ball(self):
return self.bkb.read_float(self.mem, 'VISION_BALL_DIST')
''''def get_head_pan_initial(self):
return self.config.getint('Offset', 'ID_19')
def get_head_tilt_initial(self):
return self.config.getint('Offset', 'ID_20')'''
def get_search_status(self):
time.sleep(0.1)
return self.bkb.read_int(self.mem, 'VISION_LOST')
def set_vision_search(self):
return self.bkb.write_int(self.mem, 'DECISION_SEARCH_ON', 1)
def get_orientation(self):
#print degrees(self.bkb.read_float(self.mem, 'IMU_EULER_Z'))
#print self.bkb.read_float(self.mem, 'IMU_EULER_Z')
return degrees(self.bkb.read_float(self.mem, 'IMU_EULER_Z'))
def set_stand_still(self):
#print 'stand still'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 0)
#time.sleep(1)
def set_walk_forward(self):
print 'walk forward'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 1)
def set_walk_speed(self, vel):
self.bkb.write_int(self.mem, 'DECISION_ACTION_B', vel)
def set_turn_left(self):
print 'turn left'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 2)
def set_turn_right(self):
print 'turn right'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 3)
def set_kick_right(self):
print 'kick right'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 4)
self.flag_move_ac = True
def set_kick_left(self):
print 'kick left'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 5)
self.flag_move_ac = True
def set_sidle_left(self):
print 'sidle left'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 6)
def set_sidle_right(self):
print 'sidle right'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 7)
def set_walk_forward_slow(self, vel):
print 'walk forward slow'
self.set_walk_speed(vel)
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 8)
def set_revolve_around_ball_clockwise(self):
print 'revolve around ball - clockwise'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 9)
#time.sleep(1) #Tempo de Giro
def set_revolve_around_ball_anticlockwise(self):
print 'revolve around ball - anticlockwise'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 14)
#time.sleep(1) #Tempo de Giro
def set_walk_backward(self):
print 'walk backward'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 10)
def set_gait(self):
print 'gait'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 11)
def set_pass_left(self):
print 'pass left'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 12)
self.flag_move_ac = True
def set_pass_right(self):
print 'pass right'
self.bkb.write_int(self.mem, 'DECISION_ACTION_A', 13)
self.flag_move_ac = True
def set_vision_ball(self):
self.bkb.write_int(self.mem, 'DECISION_ACTION_VISION', 0)
#return time.sleep(2)
def set_vision_robot(self):
self.bkb.write_int(self.mem, 'DECISION_ACTION_VISION', 2)
sys.path.append('../../..')
from config import *
## instantiate config:
prob_starvars_config = Config()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
number_active_entities = prob_starvars_config.number_of_oriented_points
# Instantiate the BlackBoard's class:
bkb = SharedMemory()
mem = bkb.shd_constructor(mem_key)
m = prob_starvars_config.m
IP = '127.0.0.' # Endereco IP do Servidor
PORT = 5000 # Porta que o Servidor esta
print 'Client running...\n'
bkb.write_int(mem, 'CONTROL_MESSAGES', 0)
bkb.write_int(mem, 'LOOK_DRIVEN_CTRL', 0)
sync = 0
sent2 = time.time()
bkb.write_int(mem, 'GOAL_FOUND', 0)
### looking for the library SharedMemory
import sys
sys.path.append('../../Blackboard/src/')
sys.path.append('../../Decision/src/')
from SharedMemory import SharedMemory
# instantiate configparser:
config = ConfigParser()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
# Instantiate the BlackBoard's class:
bkb = SharedMemory()
mem = bkb.shd_constructor(mem_key)
sys.path.append('../../..')
from config import *
## instantiate config:
prob_starvars_config = Config()
IP = '127.0.0.' + config.get('Communication',
'no_player_robofei') # Endereco IP do Servidor
PORT = 5000 + int(config.get('Communication',
'no_player_robofei')) # Porta que o Servidor esta
tcp = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
orig = (IP, PORT)
tcp.bind(orig)
class TreatingRawData(object):
#Instantiate the BlackBoard's class:
bkb = SharedMemory()
# instantiate:
config = ConfigParser()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
def __init__(self):
print
print 'Raw data - read (get) and write (set) methods'
print
def get_referee_usage(self):
return self.config.get('Decision', 'referee')
def get_orientation_usage(self):
return self.config.get('Decision', 'orientation')
def get_referee(self):
return self.bkb.read_int('COM_REFEREE')
def get_motor_tilt(self):
return self.bkb.read_int('VISION_MOTOR1_ANGLE')
def get_motor_pan(self):
return self.bkb.read_int('VISION_MOTOR2_ANGLE')
def get_orientation(self):
'''1 for correct orientation'''
return self.bkb.read_int('LOCALIZATION_THETA')
def get_dist_ball(self):
return self.bkb.read_int('VISION_DIST_BALL')
def get_head_pan_initial(self):
return self.config.getint('Offset', 'ID_19')
def get_head_tilt_initial(self):
return self.config.getint('Offset', 'ID_20')
def get_search_ball_status(self):
return self.bkb.read_int('VISION_SEARCH_BALL')
def get_lost_ball_status(self):
return self.bkb.read_int('VISION_LOST_BALL')
def set_search_ball_status(self):
return self.bkb.write_int('VISION_SEARCH_BALL', 1)
def set_stand_still(self):
print 'stand still'
self.bkb.write_int('DECISION_ACTION_A', 0)
return time.sleep(2)
def set_walk_forward(self):
print 'walk forward'
return self.bkb.write_int('DECISION_ACTION_A', 1)
def set_walk_speed(self,vel):
return self.bkb.write_int('DECISION_ACTION_B', vel)
def set_turn_left(self):
print 'turn left'
return self.bkb.write_int('DECISION_ACTION_A', 2)
def set_turn_right(self):
print 'turn right'
return self.bkb.write_int('DECISION_ACTION_A', 3)
def set_kick_right(self):
print 'kick right'
self.bkb.write_int('DECISION_ACTION_A', 4)
time.sleep(1)
self.bkb.write_int('DECISION_ACTION_A', 0)
return self.set_search_ball_status()
def set_kick_left(self):
print 'kick left'
self.bkb.write_int('DECISION_ACTION_A', 5)
time.sleep(2)
self.bkb.write_int('DECISION_ACTION_A', 0)
return self.set_search_ball_status()
def set_sidle_left(self):
print 'sidle left'
return self.bkb.write_int('DECISION_ACTION_A', 6)
def set_sidle_right(self):
print 'sidle right'
return self.bkb.write_int('DECISION_ACTION_A', 7)
def set_walk_forward_slow(self):
print 'walk forward slow'
self.set_walk_speed(5)
return self.bkb.write_int('DECISION_ACTION_A', 8)
def set_revolve_around_ball(self):
print 'revolve around ball'
self.bkb.write_int('DECISION_ACTION_A', 9)
time.sleep(7) #Tempo de Giro
return self.set_stand_still()
def set_walk_backward(self):
print 'walk backward'
return self.bkb.write_int('DECISION_ACTION_A', 10)
def set_gait(self):
print 'gait'
return self.bkb.write_int('DECISION_ACTION_A', 11)
def set_pass_left(self):
print 'pass left'
return self.bkb.write_int('DECISION_ACTION_A', 12)
def set_pass_right(self):
print 'pass right'
return self.bkb.write_int('DECISION_ACTION_A', 13)
def set_jump_left(self):
return self.bkb.write_int('DECISION_ACTION_A', 14)
def set_jump_right(self):
return self.bkb.write_int('DECISION_ACTION_A', 15)
def set_vision_ball(self):
self.bkb.write_int('DECISION_ACTION_VISION', 0)
return time.sleep(2)
def set_vision_orientation(self):
print "orientating"
self.set_stand_still()
self.bkb.write_int('LOCALIZATION_THETA', 0)
self.bkb.write_int('DECISION_ACTION_VISION', 2)
while(self.get_orientation() == 0):
pass
return self.bkb.write_int('DECISION_ACTION_VISION', 0)
def delta_position_pan(self):
'''right > 0 / left < 0'''
return self.get_head_pan_initial() - self.get_motor_pan()
def delta_position_tilt(self):
'''up > 0 / down < 0 / middle is looking for the horizon'''
return self.get_head_tilt_initial() - self.get_motor_tilt()
def __init__(self, x, y, theta, KEY, color):
pygame.sprite.Sprite.__init__(self)
Vision.__init__(self)
self.x = x
self.y = y
self.rotate = theta
self.KEY = KEY
self.color = color
self.new_x = x
self.new_y = y
self.robot_width = 26
self.robot_height = 26
self.radius = 13
self.index = 0
self.front = 0
self.turn = 0
self.drift = 0
self.in_motion = False
self.collision = False
self.image = pygame.Surface([self.robot_width, self.robot_height])
self.rect = self.image.get_rect()
#self.saved_image = self.image
self.front = 0
self.rect.x = x
self.rect.y = y
self.sum_time = 0
self.old_x = x
self.old_y = y
self.view_rot = theta
self.bkb = SharedMemory()
self.Mem = self.bkb.shd_constructor(KEY)
print 'Shared Memory successfully created as ', KEY
#TODO remover a linha vision_search_ball.... como nao estou utilizando decisao ainda, estou forcando a busca..
self.bkb.write_int(self.Mem, 'DECISION_SEARCH_ON', 1)
#TODO instanciar a classe visao passando o blackboard
self.fast_walk_speed = 20
self.slow_walk_speed = 10
self.turn_angle = 20
self.drift_speed = 20
self.drift_turn_speed = 15
self.control = CONTROL(self)
self.vision = VISION(self)
self.ball = None
# Errors
# Mean = 0 for symmetrical errors
# Variances should not be 0
self.errors_on = False # Turn errors on and off!
self.walk_error_mean = 0
self.walk_error_variance = 0
self.drift_error_mean = 0
self.drift_error_variance = 0
self.turn_error_mean = 0
self.turn_error_variance = 0
self.kick_error_angle_mean = 0
self.kick_error_angle_variance = 0
self.kick_error_force_mean = 0
self.kick_error_force_variance = 0
# IMU
self.imu_error_mean = 0
self.imu_error_variance = 0
self.orientation_error = 0
self.imu_initial_value = 0
#EOPRA // StarVars
self.delta_eopra = 100 # 1 meter
self.m = 8 # 4 // 6 // 8
class LocalizationVision():
#----------------------------------------------------------------------------------------------
# Class constructor
#----------------------------------------------------------------------------------------------
def __init__(self):
self.args = parser.parse_args(
) # Holds arguments to be used throughout the program
self.bkb = SharedMemory() # Creates the blackboard object
# Reads config.ini to get needed information
config = ConfigParser() #
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
self.Mem = self.bkb.shd_constructor(mem_key)
self.Pan = int(config.get('Offset', 'ID_19'))
self.Tilt = int(config.get('Offset', 'ID_20'))
self.servo = Servo(self.Pan, self.Tilt) # Initializes servos
self.limit = (self.Pan - 307, self.Pan, self.Pan + 307
) # Limits the head turning to only 90 degrees
# If calibration is requested it executes calibration
if self.args.calibrate:
calib = Calibration()
calib.Main()
calib = None
while True:
try: # Try to open an existing file
file = open('.thresholds') # open file to read and write
data = file.read() # reads file into data
file.close() # closes file
data = data.split(' ') # split lines
data = [int(data[aux]) for aux in range(30)]
self.Blue = np.array(data[0:6]) # Thresholds for blue color
self.Red = np.array(data[6:12]) # Thresholds for red color
self.Yellow = np.array(
data[12:18]) # Thresholds for yellow color
self.Purple = np.array(
data[18:24]) # Thresholds for purple color
self.blur = data[24] # How much blur in the image
self.krnl = data[
25] # Chooses the size of the kernel for erosion/dilatation
self.ersn = data[
26] # Chooses the quantity of iterations for erosion
self.dltn = data[
27] # Chooses the quantity of iterations for dilatation
self.rads = data[
28] # Chooses the radius of the color picking tool
self.thrs = data[29] # Chooses the thresholds to adjust colors
print "\n>>> Thresholds loading successful! <<<\n"
break
except:
print ">>> ERROR LOADING THRESHOLDS! <<<"
# Forcely executes calibration
calib = Calibration()
calib.Main()
calib = None
self.img = None # holds the captured frame
self.hsv = None # holds the frame after changing the color segment
self.pos = self.Pan # holds the position which the frame was generated
self.pandirection = 0 # Changing step for pan.
self.Main()
#----------------------------------------------------------------------------------------------
# Method used to initialize frame capture
#----------------------------------------------------------------------------------------------
def InitCap(self):
self.cap = cv2.VideoCapture(
1) # Try opening any different device, other than the main
if not self.cap.isOpened(): # if there is only one device
self.cap = cv2.VideoCapture(0) # opens the main device
if not self.cap.isOpened():
print "ERROR LOADING CAMERA:\nDEVICE NOT FOUND!"
exit()
#----------------------------------------------------------------------------------------------
# Method which will capture a image from the camera
#----------------------------------------------------------------------------------------------
def Capture(self):
try:
_, self.img = self.cap.read() # Get image from camera
imgblur = cv2.medianBlur(self.img, self.blur) # Blurs image
self.hsv = cv2.cvtColor(imgblur,
cv2.COLOR_BGR2HSV) # Convert to HSV
self.pos = self.limit[1] # Saves pan position
except:
print "ERROR ON FRAME CAPTURE!"
#----------------------------------------------------------------------------------------------
# Method which changes head's position
#----------------------------------------------------------------------------------------------
def PanHead(self):
if self.Pan < self.limit[0] and self.pandirection < 0:
self.pandirection *= -1
elif self.Pan > self.limit[2] and self.pandirection > 0:
self.pandirection *= -1
self.Pan += self.pandirection
self.servo.writeWord(19, 30, int(self.Pan))
#----------------------------------------------------------------------------------------------
# Method which converts a relative position on pixels into degrees
#----------------------------------------------------------------------------------------------
def IsIn(self, value):
x = value - len(self.img[0]) / 2
y = (0.1 * x * 640) / (65 * len(self.img[0]))
return degrees(atan(y))
#----------------------------------------------------------------------------------------------
# Method which returns the head's angle
#----------------------------------------------------------------------------------------------
def GetAng(self):
return (self.limit[1] - self.Pan) * 300 / 1024.0
#----------------------------------------------------------------------------------------------
# Main method
#----------------------------------------------------------------------------------------------
def Main(self):
if self.args.show:
cv2.namedWindow('ROBOT VISION')
self.InitCap()
self.pandirection = 7
thrs = [self.Blue, self.Red, self.Yellow, self.Purple]
c = [(255, 255, 100), (100, 100, 255), (100, 255, 255),
(200, 100, 200)]
while True:
ret = [-999, -999, -999, -999]
self.Capture()
for i in range(4):
try:
mask = cv2.inRange(self.hsv, thrs[i][0:3], thrs[i][3:6])
kern = np.ones((self.krnl, self.krnl), np.uint8)
erod = cv2.erode(mask, kern, iterations=self.ersn)
dila = cv2.dilate(erod, kern, iterations=self.dltn)
cnt, _ = cv2.findContours(dila, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
areas = [cv2.contourArea(aux) for aux in cnt]
max_index = np.argmax(areas)
cnt = cnt[max_index]
x, y, w, h = cv2.boundingRect(cnt)
M = cv2.moments(cnt)
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
if cnt != None:
ret[i] = -self.IsIn(cx) - self.GetAng()
if self.args.show:
cv2.rectangle(self.img, (x, y), (x + w, y + h), c[i],
2)
cv2.circle(self.img, (cx, cy), 5, c[i], -1)
except:
pass
self.bkb.write_float(self.Mem, 'VISION_BLUE_LANDMARK_DEG', ret[0])
self.bkb.write_float(self.Mem, 'VISION_RED_LANDMARK_DEG', ret[1])
self.bkb.write_float(self.Mem, 'VISION_YELLOW_LANDMARK_DEG',
ret[2])
self.bkb.write_float(self.Mem, 'VISION_PURPLE_LANDMARK_DEG',
ret[3])
print ret
if self.args.show:
cv2.line(self.img, (len(self.img[0]) / 2, 0),
(len(self.img[0]) / 2, len(self.img)), (0, 255, 0))
cv2.imshow('ROBOT VISION', self.img)
self.PanHead()
k = cv2.waitKey(15) & 0xFF
if k == 27:
break
cv2.destroyAllWindows()
def __init__(self):
self.args = parser.parse_args(
) # Holds arguments to be used throughout the program
self.bkb = SharedMemory() # Creates the blackboard object
# Reads config.ini to get needed information
config = ConfigParser() #
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
self.Mem = self.bkb.shd_constructor(mem_key)
self.Pan = int(config.get('Offset', 'ID_19'))
self.Tilt = int(config.get('Offset', 'ID_20'))
self.servo = Servo(self.Pan, self.Tilt) # Initializes servos
self.limit = (self.Pan - 307, self.Pan, self.Pan + 307
) # Limits the head turning to only 90 degrees
# If calibration is requested it executes calibration
if self.args.calibrate:
calib = Calibration()
calib.Main()
calib = None
while True:
try: # Try to open an existing file
file = open('.thresholds') # open file to read and write
data = file.read() # reads file into data
file.close() # closes file
data = data.split(' ') # split lines
data = [int(data[aux]) for aux in range(30)]
self.Blue = np.array(data[0:6]) # Thresholds for blue color
self.Red = np.array(data[6:12]) # Thresholds for red color
self.Yellow = np.array(
data[12:18]) # Thresholds for yellow color
self.Purple = np.array(
data[18:24]) # Thresholds for purple color
self.blur = data[24] # How much blur in the image
self.krnl = data[
25] # Chooses the size of the kernel for erosion/dilatation
self.ersn = data[
26] # Chooses the quantity of iterations for erosion
self.dltn = data[
27] # Chooses the quantity of iterations for dilatation
self.rads = data[
28] # Chooses the radius of the color picking tool
self.thrs = data[29] # Chooses the thresholds to adjust colors
print "\n>>> Thresholds loading successful! <<<\n"
break
except:
print ">>> ERROR LOADING THRESHOLDS! <<<"
# Forcely executes calibration
calib = Calibration()
calib.Main()
calib = None
self.img = None # holds the captured frame
self.hsv = None # holds the frame after changing the color segment
self.pos = self.Pan # holds the position which the frame was generated
self.pandirection = 0 # Changing step for pan.
self.Main()
import time
from math import log, exp, tan
from BallVision import *
from PanTilt import *
import sys
sys.path.append('../../Blackboard/src/')
from SharedMemory import SharedMemory
try:
from configparser import ConfigParser
except ImportError:
from ConfigParser import ConfigParser # ver. < 3.0
bkb = SharedMemory()
config = ConfigParser()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
Mem = bkb.shd_constructor(200)
parser = argparse.ArgumentParser(
description='Robot Vision',
epilog=
'Responsavel pela deteccao dos objetos em campo / Responsible for detection of Field objects'
)
parser.add_argument('--visionball',
'--vb',
action="store_true",
help='Calibra valor para a visao da bola')
from SharedMemory import SharedMemory
sys.path.append('../../..')
from config import *
# instantiate configparser and config:
config = ConfigParser()
prob_starvars_config = Config()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
# Instantiate the BlackBoard's class:
bkb = SharedMemory()
mem = bkb.shd_constructor(mem_key)
bkb.write_int(mem, 'DECISION_ACTION_VISION', 0)
bkb.write_int(mem, 'LOCALIZATION_FIND_ROBOT', 1)
bkb.write_int(mem, 'GOAL_FOUND', 0)
###function to set the number of the robot that will be found for the vision system
def set_find_robot(other_robot_number, goal):
if other_robot_number != goal:
bkb.write_int(mem, 'DECISION_ACTION_VISION', 2)
bkb.write_int(mem, 'LOCALIZATION_FIND_ROBOT', other_robot_number)
else:
bkb.write_int(mem, 'DECISION_ACTION_VISION', 0)
bkb.write_int(mem, 'LOCALIZATION_FIND_ROBOT', other_robot_number)
import time
from servo import Servo
from PID import *
import matplotlib.pyplot as plt
import sys
sys.path.append('../../Blackboard/src/')
from SharedMemory import SharedMemory
#try:
# from configparser import ConfigParser
#except ImportError:
from ConfigParser import ConfigParser # ver. < 3.0
bkb = SharedMemory()
config = ConfigParser()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = 100 #int(config.get('Communication', 'no_player_robofei'))*100
#Mem = bkb.shd_constructor(mem_key)
#Mem = bkb.shd_constructor(200)
class Pantilt(object):
servo = None #Para controle dos servos
cen_posTILT = None #Centro do Tilt
cen_posPAN = None #Centro do Pan
import sys
""" Initiate the path to blackboard (Shared Memory)"""
sys.path.append('../../Blackboard/src/')
"""Import the library Shared memory """
from SharedMemory import SharedMemory
""" Treatment exception: Try to import configparser from python. Write and Read from config.ini file"""
try:
"""There are differences in versions of the config parser
For versions > 3.0 """
from ConfigParser import ConfigParser
except ImportError:
"""For versions < 3.0 """
from ConfigParser import ConfigParser
""" Instantiate bkb as a shared memory """
bkb = SharedMemory()
""" Config is a new configparser """
config = ConfigParser()
""" Path for the file config.ini:"""
config.read('../../Control/Data/config.ini')
""" Mem_key is for all processes to know where the blackboard is. It is robot number times 100"""
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
"""Memory constructor in mem_key"""
Mem = bkb.shd_constructor(mem_key)
parser = argparse.ArgumentParser(
description='Robot Vision',
epilog=
'Responsavel pela deteccao dos objetos em campo / Responsible for detection of Field objects'
)
parser.add_argument('--visionball',
#looking for the library SharedMemory
import sys
sys.path.append('../../Blackboard/src/')
from SharedMemory import SharedMemory
# instantiate:
config = ConfigParser()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
# Instantiate the BlackBoard's class:
bkb = SharedMemory()
mem = bkb.shd_constructor(mem_key)
rbt_number = int(config.get('Communication', 'no_player_robofei'))
bkb.write_int(mem, 'ROBOT_NUMBER', rbt_number)
# Used to centralize the robot somewhere, given it's number.
X_ROBOT = 0
Y_ROBOT = 0
if rbt_number % 2 == 1:
X_ROBOT = 295
else:
X_ROBOT = 745
if rbt_number > 2:
Y_ROBOT = 520
#looking for the library SharedMemory
import sys
sys.path.append('../../Blackboard/src/')
from SharedMemory import SharedMemory
# instantiate:
config = ConfigParser()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
# Instantiate the BlackBoard's class:
bkb = SharedMemory()
mem = bkb.shd_constructor(mem_key)
rbt_number = int(config.get('Communication', 'no_player_robofei'))
TEAM_ROBOFEI = int(config.get('Communication', 'no_team_robofei'))
TEAM_OPPONENT = int(config.get('Communication', 'team_opponent'))
bkb.write_int(mem, 'ROBOT_NUMBER', rbt_number)
"""
This module shows how the GameController Communication protocol can be used
in python and also allows to be changed such that every team using python to
interface with the GC can utilize the new protocol.
.. moduleauthor:: Nils Rokita <*****@*****.**>
.. moduleauthor:: Robert Kessler <*****@*****.**>
"""
class SpiralWidget(QGLWidget):
'''
Widget for drawing two spirals.
'''
#import os
#os.system("pwd")
# instantiate:
config = ConfigParser()
# looking for the file config.ini:
config.read('../Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei'))*100
#Instantiate the BlackBoard's class:
bkb = SharedMemory()
mem = bkb.shd_constructor(mem_key)
#bkb = SharedMemory() #Init class BlackBoard
ax = bkb.read_float(mem, "IMU_EULER_X")
ay = -bkb.read_float(mem, "IMU_EULER_Y")
az = bkb.read_float(mem, "IMU_EULER_Z")
def __init__(self, parent):
QGLWidget.__init__(self, parent)
# self.initializeGL()
def resizeGL(self, width=0, height=0):
'''
Resize the GL window
'''
if height==0:
height=1
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45, 1.0*width/height, 0.1, 100.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
# print "resize"
def initializeGL(self):
'''
Initialize GL
'''
glShadeModel(GL_SMOOTH)
glClearColor(0.0, 0.0, 0.0, 0.0)
glClearDepth(1.0)
glEnable(GL_DEPTH_TEST)
glDepthFunc(GL_LEQUAL)
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)
# print "init"
# def drawText(self, position, textString):
# font = pygame.font.SysFont ("Courier", 18, True)
# textSurface = font.render(textString, True, (255,255,255,255), (0,0,0,255))
# textData = pygame.image.tostring(textSurface, "RGBA", True)
# glRasterPos3d(*position)
# glDrawPixels(textSurface.get_width(), textSurface.get_height(), GL_RGBA, GL_UNSIGNED_BYTE, textData)
def paintGL(self):
'''
Drawing routine
'''
global rquad
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity()
glTranslatef(0,0.0,-7.0)
# osd_text = "pitch: " + str("{0:.2f}".format(ay)) + ", roll: " + str("{0:.2f}".format(ax))
# if yaw_mode:
# osd_line = osd_text + ", yaw: " + str("{0:.2f}".format(az))
# else:
# osd_line = osd_text
# self.drawText((-2,-2, 2), osd_line)
# the way I'm holding the IMU board, X and Y axis are switched
# with respect to the OpenGL coordinate system
if yaw_mode: # experimental
glRotatef(self.az, 0.0, 1.0, 0.0) # Yaw, rotate around y-axis
else:
glRotatef(0.0, 0.0, 1.0, 0.0)
glRotatef(self.ay ,1.0,0.0,0.0) # Pitch, rotate around x-axis
glRotatef(-1*self.ax ,0.0,0.0,1.0) # Roll, rotate around z-axis
glBegin(GL_QUADS)
glColor3f(0.0,1.0,0.0)
glVertex3f( 1.0, 0.2,-1.0)
glVertex3f(-1.0, 0.2,-1.0)
glVertex3f(-1.0, 0.2, 1.0)
glVertex3f( 1.0, 0.2, 1.0)
glColor3f(1.0,0.5,0.0)
glVertex3f( 1.0,-0.2, 1.0)
glVertex3f(-1.0,-0.2, 1.0)
glVertex3f(-1.0,-0.2,-1.0)
glVertex3f( 1.0,-0.2,-1.0)
glColor3f(1.0,0.0,0.0)
glVertex3f( 1.0, 0.2, 1.0)
glVertex3f(-1.0, 0.2, 1.0)
glVertex3f(-1.0,-0.2, 1.0)
glVertex3f( 1.0,-0.2, 1.0)
glColor3f(1.0,1.0,0.0)
glVertex3f( 1.0,-0.2,-1.0)
glVertex3f(-1.0,-0.2,-1.0)
glVertex3f(-1.0, 0.2,-1.0)
glVertex3f( 1.0, 0.2,-1.0)
glColor3f(0.0,0.0,1.0)
glVertex3f(-1.0, 0.2, 1.0)
glVertex3f(-1.0, 0.2,-1.0)
glVertex3f(-1.0,-0.2,-1.0)
glVertex3f(-1.0,-0.2, 1.0)
glColor3f(1.0,0.0,1.0)
glVertex3f( 1.0, 0.2,-1.0)
glVertex3f( 1.0, 0.2, 1.0)
glVertex3f( 1.0,-0.2, 1.0)
glVertex3f( 1.0,-0.2,-1.0)
glEnd()
# print self.ax
def read_data(self):
self.ax = self.bkb.read_float(self.mem, "IMU_EULER_X")
self.ay = -self.bkb.read_float(self.mem, "IMU_EULER_Y")
self.az = self.bkb.read_float(self.mem, "IMU_EULER_Z")
line_done = 0
# request data by sending a dot
# ser.write(".")
#while not line_done:
# line = ser.readline()
angles = [self.ax*180/3.1415,
self.ay*180/3.1415,
self.az*180/3.1415] #line.split(", ")
if len(angles) == 3:
self.ax = float(angles[0])
self.ay = float(angles[1])
self.az = float(angles[2])
line_done = 1
""" Initiate the path to blackboard (Shared Memory)"""
sys.path.append('../../Blackboard/src/')
"""Import the library Shared memory """
from SharedMemory import SharedMemory
""" Treatment exception: Try to import configparser from python. Write and Read from config.ini file"""
try:
"""There are differences in versions of the config parser
For versions > 3.0 """
from ConfigParser import ConfigParser
except ImportError:
"""For versions < 3.0 """
from ConfigParser import ConfigParser
""" Instantiate bkb as a shared memory """
bkb = SharedMemory()
""" Config is a new configparser """
config1 = ConfigParser()
""" Path for the file config.ini:"""
config1.read('../../Control/Data/config.ini')
""" Mem_key is for all processes to know where the blackboard is. It is robot number times 100"""
mem_key = int(config1.get('Communication', 'no_player_robofei'))*100
"""Memory constructor in mem_key"""
Mem = bkb.shd_constructor(mem_key)
parser = argparse.ArgumentParser(description='Robot Vision', epilog= 'Responsavel pela deteccao dos objetos em campo / Responsible for detection of Field objects')
parser.add_argument('--visionball', '--vb', action="store_true", help = 'Calibra valor para a visao da bola')
parser.add_argument('--visionMask', '--vm', action="store_true", help = 'Calibra valor para a visao da bola')
parser.add_argument('--visionMorph1', '--vm1', action="store_true", help = 'Mostra a imagem da morfologia perto')
parser.add_argument('--visionMorph2', '--vm2', action="store_true", help = 'Mostra a imagem da morfologia medio')
from SharedMemory import SharedMemory
# instantiate configparser:
config = ConfigParser()
# looking for the file config.ini:
config.read('../../Control/Data/config.ini')
mem_key = int(config.get('Communication', 'no_player_robofei')) * 100
UDP_IP = "255.255.255.255"
UDP_PORT = 3800 + int(config.get('Communication', 'no_player_robofei'))
# Instantiate the BlackBoard's class:
bkb = SharedMemory()
mem = bkb.shd_constructor(mem_key)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
bkb.write_int(mem, 'CONTROL_MESSAGES', 0)
with open("../../prob_starvars/src/relations.dat", "w") as f:
pass
while True:
data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
print "received message:", data
#time.sleep(0.1)
class Localization():
# Class constructor and pre-processing.
def __init__(self):
self.bkb = SharedMemory()
config = ConfigParser()
self.args = parser.parse_args()
try:
config.read('../../Control/Data/config.ini')
self.robot_id = int(
config.get('Communication', 'no_player_robofei'))
mem_key = self.robot_id * 100
except: # noqa E722
if self.args.robot_id:
self.robot_id = self.args.robot_id
mem_key = self.robot_id * 100
print "Error loading ConfigParser."
print "Resuming with robot_id", self.robot_id
else:
print "Neither ConfigParser worked nor robot_id was given."
print "Finishing execution of Localization."
sys.exit()
self.Mem = self.bkb.shd_constructor(mem_key)
# Timestamp to use on the time step used for motion
self.timestamp = time.time()
# Localization's main method.
def main(self):
screen = Screen(self.args.graphs)
if self.args.graphs:
simul = Simulation(screen)
field = SoccerField(screen)
simul.field = field
PF = MonteCarlo(robot_id=self.robot_id)
# Erasing Shared memories values.
obs = [
'LANDMARK_L_1', 'LANDMARK_L_2', 'LANDMARK_L_3', 'LANDMARK_L_4',
'LANDMARK_T_1', 'LANDMARK_T_2', 'LANDMARK_X_1', 'LANDMARK_X_2',
'LANDMARK_P'
]
for i in obs:
self.bkb.write_int(self.Mem, i, -999)
std = 100
hp = -999
weight = 1
# Main loop
while True:
landmarks = []
self.bkb.write_int(self.Mem, 'LOCALIZATION_WORKING', 1)
# Process interactions events
if self.args.graphs:
simul.perform_events()
# Gets the motion command from the blackboard.
u = self.GetU(self.bkb.read_int(self.Mem, 'CONTROL_ACTION'))
# Gets the measured variable from the blackboard,
z = []
auxz = []
for i in obs[:4]:
aux = self.bkb.read_int(self.Mem, i)
if aux == -999:
aux = None
auxz.append(aux)
z.append(auxz)
auxz = []
for i in obs[4:6]:
aux = self.bkb.read_int(self.Mem, i)
if aux == -999:
aux = None
auxz.append(aux)
z.append(auxz)
auxz = []
for i in obs[6:8]:
aux = self.bkb.read_int(self.Mem, i)
if aux == -999:
aux = None
auxz.append(aux)
z.append(auxz)
z.append([self.bkb.read_int(self.Mem, obs[-1])])
z.append(self.bkb.read_int(
self.Mem,
"IMU_EULER_Z",
))
# and free them.
for i in obs:
self.bkb.write_int(self.Mem, i, -999)
pos, std = PF.main(u, z)
if PF.meanweight < 1:
weight = np.log(0.05) / np.log(PF.meanweight)
if self.args.log:
print '\t\x1b[32mRobot at',
print 'ent\x1b[32m[x:\x1b[34m{} cm'.format(int(pos[0])),
print '\x1b[32m| y:\x1b[34m{} cm'.format(int(pos[1])),
print u'\x1b[32m| \u03B8:\x1b[34m{}\u00B0'.format(int(pos[2])),
print u'\x1b[32m| \u03C3:\x1b[34m{} cm\x1b[32m]'.format(
int(std)) # noqa E501
# Write the robot's position on Black Board to be read by telemetry
self.bkb.write_int(self.Mem, 'LOCALIZATION_X', int(pos[0]))
self.bkb.write_int(self.Mem, 'LOCALIZATION_Y', int(pos[1]))
self.bkb.write_int(self.Mem, 'LOCALIZATION_THETA', int(pos[2]))
self.bkb.write_float(self.Mem, 'LOCALIZATION_RBT01_X', std)
if self.args.graphs:
# Redraws the screen background
field.draw_soccer_field()
simul.DrawStd(pos, std, weight, hp)
# Draws all particles on screen
simul.display_update(PF.particles)
# Updates for the next clock
screen.clock.tick(5)
# This method returns a command instruction to the particles.
def GetU(self, Action):
if Action in [0, 4, 5, 12, 13, 19, 20, 21, 22]:
return (0, 0, 0, 0, self.dt()) # Stop or kick
elif Action == 11:
return (0, 0, 0, 1, self.dt()) # Gait
elif Action == 1:
return (20, 0, 0, 1, self.dt()) # Fast Walk Forward
elif Action == 8:
return (10, 0, 0, 1, self.dt()) # Slow Walk Forward
elif Action == 17:
return (-20, 0, 0, 1, self.dt()) # Fast Walk Backward
elif Action == 18:
return (-10, 0, 0, 1, self.dt()) # Slow Walk Backward
elif Action == 6:
return (0, -10, 0, 1, self.dt()) # Walk Left
elif Action == 7:
return (0, 10, 0, 1, self.dt()) # Walk Right
elif Action == 3:
return (0, 0, 18.7, 1, self.dt()) # Turn Right
elif Action == 2:
return (0, 0, -18.7, 1, self.dt()) # Turn Left
elif Action == 9:
return (0, -10, -20, 1, self.dt()) # Turn Left Around the Ball
elif Action == 14:
return (0, 10, 20, 1, self.dt()) # Turn Right Around the Ball
elif Action == 16:
return (0, 0, 0, 2, self.dt()) # Get up, back up
elif Action == 15:
return (0, 0, 0, 3, self.dt()) # Get up, front up
elif Action == 10:
print "ERROR - Please, edit Localization.GetU() for Goalkeeper before resuming!" # noqa E501
return (0, 0, 0, 0, self.dt())
# This method returns the time since the last update
def dt(self):
auxtime = time.time()
timer = auxtime - self.timestamp
self.timestamp = auxtime
return timer
class Robot(pygame.sprite.Sprite, Vision):
def __init__(self, x, y, theta, KEY, color):
pygame.sprite.Sprite.__init__(self)
Vision.__init__(self)
self.x = x
self.y = y
self.rotate = theta
self.KEY = KEY
self.color = color
self.new_x = x
self.new_y = y
self.robot_width = 26
self.robot_height = 26
self.radius = 13
self.index = 0
self.front = 0
self.turn = 0
self.drift = 0
self.in_motion = False
self.collision = False
self.image = pygame.Surface([self.robot_width, self.robot_height])
self.rect = self.image.get_rect()
#self.saved_image = self.image
self.front = 0
self.rect.x = x
self.rect.y = y
self.sum_time = 0
self.old_x = x
self.old_y = y
self.view_rot = theta
self.bkb = SharedMemory()
self.Mem = self.bkb.shd_constructor(KEY)
print 'Shared Memory successfully created as ', KEY
#TODO remover a linha vision_search_ball.... como nao estou utilizando decisao ainda, estou forcando a busca..
self.bkb.write_int(self.Mem, 'DECISION_SEARCH_ON', 1)
#TODO instanciar a classe visao passando o blackboard
self.fast_walk_speed = 20
self.slow_walk_speed = 10
self.turn_angle = 20
self.drift_speed = 20
self.drift_turn_speed = 15
self.control = CONTROL(self)
self.vision = VISION(self)
self.ball = None
# Errors
# Mean = 0 for symmetrical errors
# Variances should not be 0
self.errors_on = False # Turn errors on and off!
self.walk_error_mean = 0
self.walk_error_variance = 0
self.drift_error_mean = 0
self.drift_error_variance = 0
self.turn_error_mean = 0
self.turn_error_variance = 0
self.kick_error_angle_mean = 0
self.kick_error_angle_variance = 0
self.kick_error_force_mean = 0
self.kick_error_force_variance = 0
# IMU
self.imu_error_mean = 0
self.imu_error_variance = 0
self.orientation_error = 0
self.imu_initial_value = 0
#EOPRA // StarVars
self.delta_eopra = 100 # 1 meter
self.m = 8 # 4 // 6 // 8
def draw_robot(self, robot_index, screen):
self.image.fill(screen.GREEN)
self.image.set_colorkey(screen.GREEN)
self.rect.x = self.x - 13
self.rect.y = self.y - 13
#robot's body
pygame.draw.rect(self.image, self.color, (3, 0, 16, 26), 0)
#feet
pygame.draw.rect(self.image, screen.BLACK, (19, 2, 5, 10), 0)
pygame.draw.rect(self.image, screen.BLACK, (19, 14, 5, 10), 0)
#sum time between frames
self.sum_time = (screen.clock.get_time() + self.sum_time) % 500
#feet movement while walking
if self.control.action_flag != 0:
if self.sum_time < 250:
pygame.draw.rect(self.image, screen.BLACK, (19, 1, 6, 12), 0)
pygame.draw.rect(self.image, screen.BLACK, (19, 14, 5, 10), 0)
else:
pygame.draw.rect(self.image, screen.BLACK, (19, 2, 5, 10), 0)
pygame.draw.rect(self.image, screen.BLACK, (19, 13, 6, 12), 0)
image2 = pygame.transform.rotate(self.image, self.rotate)
#fix rotation to the center
rot_rect = image2.get_rect(center=self.rect.center)
#show
screen.background.blit(image2, (rot_rect))
#text
font = pygame.font.SysFont("Arial", 15)
self.index = robot_index + 1
robot_name = "B" + str(self.index)
text = font.render(robot_name, 1, (10, 10, 10))
textpos = (self.x - 5, self.y - 40)
screen.background.blit(text, textpos)
self.vision.DrawLM(screen.background)
'''Control'''
def motion_vars(self, front, rotate, drift):
self.front = front
self.turn = rotate
self.drift = drift
def set_errors(self,
walk_err_mean=0,
walk_err_var=0,
turn_err_mean=0,
turn_err_var=0,
drift_err_mean=0,
drift_err_var=0,
kick_ang_err_mean=0,
kick_ang_err_var=0,
kick_force_err_mean=0,
kick_force_err_var=0,
imu_err_mean=0,
imu_err_var=0):
self.errors_on = True
self.walk_error_mean = walk_err_mean
self.walk_error_variance = walk_err_var
self.turn_error_mean = turn_err_mean
self.turn_error_variance = turn_err_var
self.drift_error_mean = drift_err_mean
self.drift_error_variance = drift_err_var
self.kick_error_angle_mean = kick_ang_err_mean
self.kick_error_angle_variance = kick_ang_err_var
self.kick_error_force_mean = kick_force_err_mean
self.kick_error_force_variance = kick_force_err_var
self.imu_error_mean = imu_err_mean
self.imu_error_variance = imu_err_var
def motion_model(self, lines, goals, robots):
turn = self.turn
if self.errors_on and self.in_motion:
turn += gauss(self.turn_error_mean, self.turn_error_variance)
front = self.front
if self.errors_on and self.in_motion:
front += gauss(self.walk_error_mean, self.walk_error_variance)
drift = self.drift
if self.errors_on and self.in_motion:
drift += gauss(self.drift_error_mean, self.drift_error_variance)
self.rotate = (self.rotate + turn) % 360
self.view_rot = (self.view_rot + turn) % 360
self.x += cos(radians(self.rotate)) * front
self.y -= sin(radians(self.rotate)) * front
self.x -= sin(radians(self.rotate)) * drift
self.y -= cos(radians(self.rotate)) * drift
for line in lines:
x, y = collision_robot_vs_line(self, line)
self.x += x
self.y += y
for goal in goals:
x, y = collision_robot_vs_goal(self, goal)
self.x += x
self.y += y
for robot in robots:
if robot.KEY != self.KEY:
x, y = collision_robot_vs_robot(self, robot)
self.x += x
self.y += y
collision_robot_vs_ball(self, self.ball)
self.get_orientation() # cumulative error
def get_orientation(self):
if self.errors_on:
error = gauss(self.imu_error_mean, self.imu_error_variance)
self.orientation_error += error
if self.imu_initial_value == 0:
if self.rotate > 180:
return (self.rotate - 360) + self.orientation_error
else:
return self.rotate + self.orientation_error
elif self.imu_initial_value == 180:
return (self.rotate - 180) + self.orientation_error
def right_kick(self):
self.Kick(5, 30)
def left_kick(self):
self.Kick(30, 5)
def pass_left(self):
self.Kick(30, 30, 90,
8) #leftlimit, rightlimit, direction, force = 1 to 12
def pass_right(self):
self.Kick(30, 30, -90,
8) #leftlimit, rightlimit, direction, force 1 to 12
def Kick(self, LeftLimit, RightLimit, Direction=0, force_limit=12):
R = degrees(atan2((self.y - self.ball.y), (self.ball.x - self.x)))
d = sqrt((self.x - self.ball.x)**2 + (self.y - self.ball.y)**2)
force = min(
10, force_limit * exp(-((self.radius - d)**2) / (force_limit)**2))
r = R
if R < 0: r = R + 360
if self.rotate > 360 - LeftLimit and (
r > self.rotate - RightLimit
or r < self.rotate - 360 + LeftLimit
) or self.rotate < RightLimit and (
r > 360 - RightLimit + self.rotate
or r < self.rotate + LeftLimit
) or r > self.rotate - RightLimit and r < self.rotate + LeftLimit:
if self.errors_on:
self.ball.put_in_motion(
force + gauss(self.kick_error_force_mean,
self.kick_error_force_variance),
self.rotate + Direction +
gauss(self.kick_error_angle_mean,
self.kick_error_angle_variance))
else:
self.ball.put_in_motion(force, self.rotate + Direction)
self.control.action_select(0)
'''Vision'''
def toRectangular(self, point):
r = point[0]
a = point[1]
return (r * cos(a), r * sin(a))
def draw_vision(self, screen):
#print '********************************************* ',self.view_rot
vision_dist = self.vision_dist
field_of_view = self.field_of_view
startRad = radians(-35 + self.view_rot)
endRad = radians(35 + self.view_rot)
vision_surface = pygame.Surface((vision_dist * 2, vision_dist * 2))
vision_surface.fill(screen.GREEN)
vision_surface.set_colorkey(screen.GREEN)
vision_surface.set_alpha(200)
vision_surface_center = (vision_dist, vision_dist)
# if self.view_rot < 0:
# self.view_rot = self.view % 360
#if self.view_rot > (self.rotate + 90):
# self.view_rot = self.rotate + 90
#elif self.view_rot < (self.rotate - 90):
# self.view_rot = self.rotate - 90
theta_vision = radians(self.view_rot)
angle_1 = -theta_vision + field_of_view / 2
angle_2 = -theta_vision - field_of_view / 2
point_1 = (vision_dist, angle_1)
point_2 = (vision_dist, angle_2)
point_1 = self.toRectangular(point_1)
point_2 = self.toRectangular(point_2)
point_1 = (point_1[0] + vision_surface_center[0],
point_1[1] + vision_surface_center[1])
point_2 = (point_2[0] + vision_surface_center[0],
point_2[1] + vision_surface_center[1])
pygame.draw.arc(screen.background, screen.WHITE, [
self.x - vision_dist, self.y - vision_dist, vision_dist * 2,
vision_dist * 2
], startRad, endRad, 1)
pygame.draw.arc(screen.background, screen.WHITE, [
self.x - vision_dist / 2, self.y - vision_dist / 2, vision_dist,
vision_dist
], startRad, endRad, 1)
pygame.draw.line(vision_surface, screen.WHITE, vision_surface_center,
point_1, 1)
pygame.draw.line(vision_surface, screen.WHITE, vision_surface_center,
point_2, 1)
position = (self.x - vision_dist, self.y - vision_dist)
screen.background.blit(vision_surface, position)
def ball_search(self, x, y):
self.ball.view_obj(self.Mem, self.bkb, self.x, self.y, x, y,
self.rotate, self.vision_dist)
def perform_pan(self):
self.view_rot = self.pan(self.view_rot, self.rotate)
def vision_process(self, ballX, ballY, robots):
# ball detect
if self.bkb.read_int(
self.Mem, 'DECISION_ACTION_VISION'
) == 0: # decision saying to vision to focus on ball
if self.bkb.read_int(self.Mem,
'DECISION_SEARCH_ON') == 1: # searching ball
self.perform_pan()
view_rot_aux = self.ball_detect(self.Mem, self.bkb, self.view_rot,
self.rotate, self.x, self.y, ballX,
ballY)
if view_rot_aux != None:
self.view_rot = view_rot_aux
#DECISION_ACTION_VISION = 1 ---- search for the robots of the team
if (self.bkb.read_int(self.Mem, 'DECISION_ACTION_VISION') == 1
): #to test the code, please set 0.
#robot detect
if robots:
for j in range(0, len(robots)):
if j != self.index - 1 and self.color == robots[j].color:
#print 'Oponente: ', robots[j].color
self.robot_detect(self.Mem, self.bkb, self.view_rot,
self.rotate, self.x, self.y,
robots[j].x, robots[j].y, j)
# DECISION_ACTION_VISION = 2 ---- search for the opponent robots
if (self.bkb.read_int(self.Mem, 'DECISION_ACTION_VISION') == 2
): # to test the code, please set 0.
# robot detect
if robots:
for j in range(0, len(robots)):
if j != self.index - 1 and self.color != robots[j].color:
#print 'Robo ', self.index, 'found: '
self.robot_detect(self.Mem, self.bkb, self.view_rot,
self.rotate, self.x, self.y,
robots[j].x, robots[j].y, j,
self.index)
def draw_eopra(self, screen):
resolution = 180 / self.m
half_resolution = resolution / 2
farthest_boundary = self.delta_eopra * self.m / (2 * self.m -
(2 * self.m - 2))
if self.m == 6:
# qualitative distance
# e * delta / m => e = region / m = granularity
farthest_boundary = self.delta_eopra * self.m / (2 * self.m - 10)
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
2 * self.delta_eopra / self.m, 1)
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
4 * self.delta_eopra / self.m, 1)
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
6 * self.delta_eopra / self.m, 1)
# delta * m / (2m-e)
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
self.delta_eopra * self.m / (2 * self.m - 8), 1)
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
self.delta_eopra * self.m / (2 * self.m - 10),
1)
# qualitative direction
pygame.draw.line(
screen.background, self.color, (int(self.x), int(self.y)),
(cos(radians(self.rotate)) * farthest_boundary + int(self.x),
int(self.y) - sin(radians(self.rotate)) * farthest_boundary),
3)
for i in range(resolution, 360, resolution):
pygame.draw.line(
screen.background, self.color, (int(self.x), int(self.y)),
(cos(radians(self.rotate + i)) * farthest_boundary +
int(self.x), int(self.y) -
sin(radians(self.rotate + i)) * farthest_boundary), 1)
# text
if self.m == 4:
# e * delta / m => e = region / m = granularity
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
2 * self.delta_eopra / self.m, 1)
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
4 * self.delta_eopra / self.m, 1)
# delta * m / (2m-e)
pygame.draw.circle(screen.background, self.color,
(int(self.x), int(self.y)),
self.delta_eopra * self.m / (2 * self.m - 6), 1)
# qualitative direction
pygame.draw.line(
screen.background, self.color, (int(self.x), int(self.y)),
(cos(radians(self.rotate)) * farthest_boundary + int(self.x),
int(self.y) - sin(radians(self.rotate)) * farthest_boundary),
3)
for i in range(resolution, 360, resolution):
pygame.draw.line(
screen.background, self.color, (int(self.x), int(self.y)),
(cos(radians(self.rotate + i)) * farthest_boundary +
int(self.x), int(self.y) -
sin(radians(self.rotate + i)) * farthest_boundary), 1)
def draw_starvars(self, screen):
resolution = 180 / (self.m / 2)
half_resolution = resolution / 2
farthest_boundary = 1000
if self.m == 8:
# qualitative direction
pygame.draw.line(
screen.background, self.color, (int(self.x), int(self.y)),
(cos(radians(self.rotate)) * farthest_boundary + int(self.x),
int(self.y) - sin(radians(self.rotate)) * farthest_boundary),
3)
for i in range(resolution, 360, resolution):
pygame.draw.line(
screen.background, self.color, (int(self.x), int(self.y)),
(cos(radians(self.rotate + i)) * farthest_boundary +
int(self.x), int(self.y) -
sin(radians(self.rotate + i)) * farthest_boundary), 1)
