def __init__(self, dontInclude=[], wait_services=True):
self.tf_listener = tf_server.TFClient()
if 'head' not in dontInclude:
self.head = head.Head()
if 'base' not in dontInclude:
self.base = ros_navstack_base.Base(self.tf_listener,
wait_service=wait_services)
if 'spindle' not in dontInclude:
self.spindle = spindle.Spindle(wait_service=wait_services)
if 'arms' not in dontInclude:
self.arms = arms.Arms(
self.tf_listener) #TODO: use self.tf_listener here
if 'leftArm' not in dontInclude:
self.leftArm = arms.Arm(arms.Side.LEFT, self.tf_listener)
if 'rightArm' not in dontInclude:
self.rightArm = arms.Arm(arms.Side.RIGHT, self.tf_listener)
if 'ebutton' not in dontInclude:
self.ebutton = ebutton.EButton()
self.leftSide = arms.Side.LEFT
self.rightSide = arms.Side.RIGHT
self.pub_target = rospy.Publisher("/target_location",
geometry_msgs.msg.Pose2D,
queue_size=10)
def load_game():
# LISTS
# global GUI_LIST_BACKGROUND
# global GUI_LIST_FOREGROUND
# global FOOD_LIST
# global BODY_PARTS_LIST
GUI_LIST_FOREGROUND.empty()
GUI_LIST_BACKGROUND.empty()
FOOD_LIST.empty()
BODY_PARTS_LIST.empty()
gui.init()
global SCROLL_AVANCEMENT
SCROLL_AVANCEMENT = 0
global SCORE
SCORE[0] = 0
head = he.Head((488, 320), HEAD_SIZE, HEAD_RADIUS)
head.add(BODY_PARTS_LIST)
ha.Hand((SCREEN_WIDTH // 4, 3 * SCREEN_HEIGHT // 4), 0, HAND_SPEED,
HAND_SIZE, head, True).add(BODY_PARTS_LIST)
ha.Hand((400, 200), 0, HAND_SPEED, HAND_SIZE, head,
False).add(BODY_PARTS_LIST)
pg.time.set_timer(pg.USEREVENT, 1000)
pg.time.set_timer(MEM_MOUSE_EVENT, MEM_MOUSE_EVENT_TIME)
def __init__(self, robot_name="", wait_services=False):
self.robot_name = robot_name
self.tf_listener = tf_server.TFClient()
# Body parts
self.parts = dict()
self.parts['base'] = base.Base(self.robot_name, self.tf_listener)
self.parts['torso'] = torso.Torso(self.robot_name, self.tf_listener)
self.parts['leftArm'] = arms.Arm(self.robot_name,
self.tf_listener,
side="left")
self.parts['rightArm'] = arms.Arm(self.robot_name,
self.tf_listener,
side="right")
self.parts['head'] = head.Head(self.robot_name, self.tf_listener)
# Human Robot Interaction
self.parts['lights'] = lights.Lights(self.robot_name, self.tf_listener)
self.parts['speech'] = speech.Speech(
self.robot_name, self.tf_listener,
lambda: self.lights.set_color(1, 0, 0),
lambda: self.lights.set_color(0, 0, 1))
self.parts['hmi'] = api.Api(self.robot_name, self.tf_listener)
self.parts['ears'] = ears.Ears(self.robot_name,
lambda: self.lights.set_color(0, 1, 0),
lambda: self.lights.set_color(0, 0, 1))
self.parts['ebutton'] = ebutton.EButton(self.robot_name,
self.tf_listener)
# Reasoning/world modeling
self.parts['ed'] = world_model_ed.ED(self.robot_name, self.tf_listener)
# Miscellaneous
self.pub_target = rospy.Publisher("/target_location",
geometry_msgs.msg.Pose2D,
queue_size=10)
self.base_link_frame = "/" + self.robot_name + "/base_link"
# Grasp offsets
#TODO: Don't hardcode, load from parameter server to make robot independent.
self.grasp_offset = geometry_msgs.msg.Point(0.5, 0.2, 0.0)
# Create attributes from dict
for k, v in self.parts.iteritems():
setattr(self, k, v)
self.spindle = self.torso # (ToDo: kind of ugly, why do we still have spindle???)
self.arms = OrderedDict(
left=self.leftArm, right=self.rightArm
) # (ToDo: kind of ugly, why do we need this???)
self.ears._hmi = self.hmi # ToDo: when ears is gone, remove this line
# Wait for connections
s = rospy.Time.now()
for k, v in self.parts.iteritems():
v.wait_for_connections(1.0)
e = rospy.Time.now()
rospy.logdebug("Connecting took {} seconds".format((e - s).to_sec()))
def test_train(self):
options = ranking_head.Options()
head = ranking_head.Head(loss_fn=_make_loss_fn())
loss = head.run(mode=ranking_head.ModeKeys.TRAIN,
labels=self._default_labels,
logits=self._default_logits,
features={})
self.assertAlmostEqual(loss.item(), self._default_loss)
def __init__(self, length=None, **kwargs):
if length is None:
length = 100.0
self.length = length # length to constrain worm
self.head_ = head.Head(**kwargs)
#
self._bodypoints = [
self.head_.position.move(self.head_.angle, -self.length)
]
def test_eval(self):
metric_fns = {
'metric/precision@1':
metrics_lib.make_ranking_metric_fn(
metrics_lib.RankingMetricKey.PRECISION, topn=1),
}
head = ranking_head.Head(loss_fn=_make_loss_fn(),
eval_metric_fns=metric_fns)
loss, metrics_values = head.run(ranking_head.ModeKeys.EVAL,
self._default_labels,
self._default_logits,
features={})
self.assertAlmostEqual(loss.item(), self._default_loss, 5)
def __init__(self):
# Initialise pygame
os.environ["SDL_VIDEO_CENTERED"] = "1"
pygame.init()
s_width = 1600
s_height = 800
# Set up the display
self.screen = pygame.display.set_mode((s_width, s_height))
self.clock = pygame.time.Clock()
# define cornerpoints
wall_thickness = 20.0
c_depth = 250.0
p0 = (wall_thickness, wall_thickness)
p1 = (s_width - wall_thickness, wall_thickness)
p2 = (s_width - wall_thickness, s_height - wall_thickness)
p3 = (wall_thickness, s_height - wall_thickness)
self.walls = [
RectWall((p0, p1), (0.0, 0.0), (wall_thickness, s_height)),
RectWall((p1, p2), (s_width - wall_thickness, 0.0),
(wall_thickness, s_height)),
RectWall((p2, p3), (0.0, s_height - wall_thickness),
(s_width, wall_thickness)),
RectWall((p3, p0), (0.0, 0.0), (s_width, wall_thickness)),
PolyWall(((0, c_depth), (c_depth, 0.0), (0.0, 0.0))),
PolyWall(((s_width - c_depth, 0.0), (s_width, c_depth), (s_width,
0.0))),
PolyWall(((s_width, s_height - c_depth),
(s_width - c_depth, s_height), (s_width, s_height))),
PolyWall(((0, s_height - c_depth), (c_depth, s_height),
(0.0, s_height)))
]
self.orbits = [
orbit.Orbit((500, 600), 100, [40 for i in range(3)]),
orbit.Orbit((900, 500), 100, [40 for i in range(3)]),
orbit.Orbit((200, 400), 100, [40 for i in range(3)])
]
self.orbs = []
self.players = [
head.Head(self, (400.0, 400.0), utils.unit_vector((-1, -1.01)))
]
# self.players = [head.Head(self, (random.randrange(200.0, 1400.0), random.randrange(200.0, 600.0)), utils.unit_vector((random.random()*2 - 1, random.random() * 2 - 1))) for i in range(10)]
# self.players = [orb.Orb(self, (701.0, 200.0), utils.unit_vector((-1, 0))), orb.Orb(self, (100.0, 259.0), utils.unit_vector((1, 0)))]
self.tail = False
def __init__(self, robot_name="", wait_services=False):
self.robot_name = robot_name
self.tf_listener = tf_server.TFClient()
# Body parts
self.base = base.Base(self.robot_name,
self.tf_listener,
wait_service=wait_services)
self.torso = torso.Torso(self.robot_name, wait_service=wait_services)
self.spindle = self.torso
self.leftArm = arms.Arm(self.robot_name, "left", self.tf_listener)
self.rightArm = arms.Arm(self.robot_name, "right", self.tf_listener)
self.arms = OrderedDict(left=self.leftArm, right=self.rightArm)
self.head = head.Head(self.robot_name)
# Human Robot Interaction
self.speech = speech.Speech(self.robot_name,
wait_service=wait_services)
self.ears = ears.Ears(self.robot_name)
self.ebutton = ebutton.EButton()
self.lights = lights.Lights()
# Perception: can we get rid of this???
self.perception = perception_ed.PerceptionED(
wait_service=wait_services)
# Reasoning/world modeling
self.ed = world_model_ed.ED(wait_service=wait_services)
self.reasoner = reasoner.Reasoner()
# Miscellaneous
self.pub_target = rospy.Publisher("/target_location",
geometry_msgs.msg.Pose2D,
queue_size=10)
self.base_link_frame = "/" + self.robot_name + "/base_link"
#Grasp offsets
#TODO: Don't hardcode, load from parameter server to make robot independent.
self.grasp_offset = geometry_msgs.msg.Point(0.5, 0.2, 0.0)
def __init__(self, canvas, elements, changes):
self.canvas = canvas
self.head_commands = ["<titel>"]
self.head_tags = [[]]
# changes = all elements and their attributes seperated
if not isinstance(changes, str):
self.changes = changes
self.chan_ex = True
else:
self.changes = 0
self.chan_ex = False
#strings of tags+values
self.elements = elements
#list of objects
self.objects = []
# seperate head tags
index = 0
for x in range(len(self.elements) - 1):
for y in range(len(self.head_commands)):
if self.elements[x][0] == self.head_commands[y]:
self.head_tags.insert(
index, [self.head_commands[y], self.elements[x][1]])
index += 1
#number of objects
self.count = len(self.elements) - 1
for x in range(self.count):
if self.elements[x][0] == "<ue1>":
self.objects.append(
H1.H1(self.canvas, self.elements[x][1], 0, 0))
elif self.elements[x][0] == "<ue2>":
self.objects.append(
H2.H2(self.canvas, self.elements[x][1], 0, 0))
elif self.elements[x][0] == "<a>":
self.objects.append(P.P(self.canvas, self.elements[x][1], 0,
0))
elif self.elements[x][0] == "<ul>":
self.objects.append(
UL.UL(self.canvas, self.elements[x][1], 0, 0))
elif self.elements[x][0] == "<gl>":
self.objects.append(
OL.OL(self.canvas, self.elements[x][1], 0, 0))
self.obj_num = len(self.objects)
#show all the stuff and run ksb
if self.chan_ex:
for x in range(len(self.changes)):
if self.changes[x].element == "koerper":
for i in range(len(self.changes[x].functions) -
1): #go though all attributes
if self.changes[x].functions[i][
0] == "hintergrundfarbe":
self.change_background_body(
self.changes[x].functions[i][1])
# get <a>s done
if self.changes[x].element == "a":
self.scan_element("<a>", x)
# get ue1 done
if self.changes[x].element == "ue1":
self.scan_element("<ue1>", x)
# get ue2 done
if self.changes[x].element == "ue2":
self.scan_element("<ue2>", x)
# get uls done
if self.changes[x].element == "ul":
self.scan_element("<ul>", x)
# get ols done
if self.changes[x].element == "gl":
self.scan_element("<gl>", x)
# execute head
if len(self.head_tags) != 0:
head.Head(self.canvas, self.head_tags, self.objects)
overallSize = 0
for x in range(self.obj_num):
if x != 0:
overallSize += self.objects[x - 1].size
self.objects[x].y += 15 * x + overallSize
self.objects[x].display()
def __init__(self, robot_name="", wait_services=False):
self.robot_name = robot_name
self.tf_listener = tf.TransformListener()
# Body parts
self.parts = dict()
self.parts['base'] = base.Base(self.robot_name, self.tf_listener)
self.parts['torso'] = torso.Torso(self.robot_name, self.tf_listener)
self.parts['leftArm'] = arms.Arm(self.robot_name,
self.tf_listener,
side="left")
self.parts['rightArm'] = arms.Arm(self.robot_name,
self.tf_listener,
side="right")
self.parts['head'] = head.Head(self.robot_name, self.tf_listener)
self.parts['perception'] = perception.Perception(
self.robot_name, self.tf_listener)
self.parts['ssl'] = ssl.SSL(self.robot_name, self.tf_listener)
# Human Robot Interaction
self.parts['lights'] = lights.Lights(self.robot_name, self.tf_listener)
self.parts['speech'] = speech.Speech(
self.robot_name, self.tf_listener,
lambda: self.lights.set_color_colorRGBA(lights.SPEAKING),
lambda: self.lights.set_color_colorRGBA(lights.RESET))
self.parts['hmi'] = api.Api(
self.robot_name, self.tf_listener,
lambda: self.lights.set_color_colorRGBA(lights.LISTENING),
lambda: self.lights.set_color_colorRGBA(lights.RESET))
self.parts['ears'] = ears.Ears(
self.robot_name, self.tf_listener,
lambda: self.lights.set_color_colorRGBA(lights.LISTENING),
lambda: self.lights.set_color_colorRGBA(lights.RESET))
self.parts['ebutton'] = ebutton.EButton(self.robot_name,
self.tf_listener)
# Reasoning/world modeling
self.parts['ed'] = world_model_ed.ED(self.robot_name, self.tf_listener)
# Miscellaneous
self.pub_target = rospy.Publisher("/target_location",
geometry_msgs.msg.Pose2D,
queue_size=10)
self.base_link_frame = "/" + self.robot_name + "/base_link"
# Check hardware status
self._hardware_status_sub = rospy.Subscriber(
"/" + self.robot_name + "/hardware_status", DiagnosticArray,
self.handle_hardware_status)
# Grasp offsets
#TODO: Don't hardcode, load from parameter server to make robot independent.
self.grasp_offset = geometry_msgs.msg.Point(0.5, 0.2, 0.0)
# Create attributes from dict
for partname, bodypart in self.parts.iteritems():
setattr(self, partname, bodypart)
self.arms = OrderedDict(
left=self.leftArm, right=self.rightArm
) # (ToDo: kind of ugly, why do we need this???)
self.ears._hmi = self.hmi # ToDo: when ears is gone, remove this line
# Wait for connections
s = rospy.Time.now()
for partname, bodypart in self.parts.iteritems():
bodypart.wait_for_connections(1.0)
e = rospy.Time.now()
rospy.logdebug("Connecting took {} seconds".format((e - s).to_sec()))
if not self.operational:
not_operational_parts = [
name for name, part in self.parts.iteritems()
if not part.operational
]
rospy.logwarn("Not all hardware operational: {parts}".format(
parts=not_operational_parts))
"""
Author: Jan Bernhard
Last updated: 04/22/18
Purpose: Testing head.py with defined control sequence.
"""
import head
obj = head.Head()
obj.connect()
print(">>> TEST SEQUENCE STARTED <<<")
print(">>> Testing Turn Table: Standard use")
obj.test_look(turn=-45)
obj.test_look(turn=0)
obj.test_look(turn=45)
obj.test_look(turn=-90)
obj.test_look(turn=0)
obj.test_look(turn=90)
obj.test_look(turn=0)
print(">>> Testing Turn Table: Invalid inputs")
obj.test_look(turn=-100)
obj.test_look(turn=100)
print(">>> Testing Turn Table: Complete")
print(">>> Testing Tilt Axis: Standard use")
obj.test_look(tilt=-15)
obj.test_look(tilt=0)
obj.test_look(tilt=15)
obj.test_look(tilt=-30)
obj.test_look(tilt=0)
obj.test_look(tilt=30)
class avoidanceofObjects:
BP.set_sensor_type(BP.PORT_4, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
switcher = 0
direction = 0
ScanValues = [0, 0, 0]
leftScanArray = []
rightScanArray = []
headAngle = 0 # 0 is for centre, 1 is for left, and 2 is for right
i = 0
closeToObject = False
positionSet = False
h = head.Head(120, 107)
def main(self):
if (self.switcher == 0):
self.avoidance()
elif (self.switcher == 1):
self.checkObject()
elif (self.switcher == 2):
self.aroundObject()
else:
print("Error")
def getAverage(self, someArray):
print("Old Array:" + str(someArray))
for x in someArray:
if (x == 255.0):
someArray.remove(x)
print("New Array:" + str(someArray))
return statistics.median(someArray)
def getLowest(self, someArray):
print("Old Array:" + str(someArray))
return min(someArray)
def avoidance(self):
uValue = 70
uValue = self.getUltrasonic()
print("Avoidance:" + str(uValue))
if (uValue <= 10):
self.closeToObject = True
if (uValue == 0):
drive.stop()
else:
if (self.closeToObject == True):
if (self.ScanValues[2] == 0):
self.h.returnCentre()
drive.stop()
time.sleep(1)
self.ScanValues[2] = self.getUltrasonic()
print("Centre Scan Value:" + str(uValue))
if (self.i <= 9 and self.ScanValues[1] == 0):
if (self.i == 0):
scanV = self.h.scan(0)
else:
scanV = 3
if (scanV == 2):
self.leftScanArray.append(self.getUltrasonic())
elif (scanV == 3):
if (self.ScanValues[0] == 0):
self.ScanValues[0] = self.getLowest(
self.leftScanArray)
print("Left Scan Value:" + str(self.ScanValues[0]))
self.rightScanArray.insert(self.i,
self.getUltrasonic())
self.i += 1
print("i = " + str(self.i))
# print("Right Scan Value:" + str(self.getUltrasonic()))
elif (self.ScanValues[1] == 0):
self.ScanValues[1] = self.getLowest(self.rightScanArray)
print("Right Scan Value:" + str(self.ScanValues[1]))
self.i = 0
elif (self.positionSet == False):
if (self.ScanValues[1] > 60):
self.h.turnLeft(0.7)
self.direction = 1
self.headAngle = 1
elif (self.ScanValues[0] > 60):
self.h.turnRight(0.7)
self.direction = 0
self.headAngle = 2
else:
self.h.returnCentre()
self.direction = random.randint(0, 1)
self.headAngle = 0
self.positionSet = True
else:
if (uValue < 5):
if (self.headAngle == 0):
if (self.direction == 0):
drive.turnLeft90()
self.h.turnRight(1)
else:
drive.turnRight90()
self.h.turnLeft(1)
elif (self.headAngle == 1):
drive.turnRight45()
self.h.returnCentre()
self.h.turnLeft(1)
else:
drive.turnLeft45()
self.h.returnCentre()
self.h.turnRight(1)
self.switcher = 1
else:
drive.moveForward()
else:
drive.moveForward()
self.h.scan(1)
def checkObject(self):
self.closeToObject = False
self.positionSet = False
self.ScanValues[1] = 0
self.ScanValues[0] = 0
self.ScanValues[2] = 0
self.rightScanArray = []
self.leftScanArray = []
uValue = self.getUltrasonic()
p = -1
error = (uValue - 15) * p
if (uValue > 200):
BP.set_motor_power(BP.PORT_A, -20)
BP.set_motor_power(BP.PORT_D, -20)
elif (self.direction == 0):
BP.set_motor_power(BP.PORT_A, -speed - (error * 0.8))
BP.set_motor_power(BP.PORT_D, -speed + (error * 0.8))
else:
BP.set_motor_power(BP.PORT_A, -speed + (error * 0.8))
BP.set_motor_power(BP.PORT_D, -speed - (error * 0.8))
print(uValue)
if (uValue > 70 and uValue < 200):
self.switcher = 2
self.h.returnCentre()
drive.moveForward()
def aroundObject(self):
if (self.direction == 0):
time.sleep(0.5)
drive.pivotTurn90(25, 40, -2400)
else:
time.sleep(0.5)
drive.pivotTurn90(40, 25, -2400)
self.switcher = 0
def getUltrasonic(self):
try:
return BP.get_sensor(BP.PORT_4)
except brickpi3.SensorError as error:
print(error)
return 0
class avoidanceofObjects:
BP.set_sensor_type(BP.PORT_4, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
switcher = 0
direction = 0
h = head.Head(120, 107)
def main(self):
if (self.switcher == 0):
self.avoidance()
elif (self.switcher == 1):
self.checkObject()
elif (self.switcher == 2):
self.aroundObject()
else:
print("Error")
def avoidance(self):
uValue = 70
uValue = self.getUltrasonic()
if (uValue == 0):
drive.stop()
else:
if (uValue <= 3):
self.direction = random.randint(0, 1)
if (self.direction == 0):
drive.turnLeft45()
drive.pivotTurn45(30, 10)
self.h.turnRight()
time.sleep(1)
else:
drive.turnRight45()
drive.pivotTurn45(10, 30)
self.h.turnLeft()
time.sleep(1)
self.switcher = 1
else:
drive.moveForward()
def checkObject(self):
uValue = self.getUltrasonic()
p = -1
error = (uValue - 10) * p
if (self.direction == 0):
BP.set_motor_power(BP.PORT_A, -speed - (error * 0.8))
BP.set_motor_power(BP.PORT_D, -speed + (error * 0.8))
else:
BP.set_motor_power(BP.PORT_A, -speed + (error * 0.8))
BP.set_motor_power(BP.PORT_D, -speed - (error * 0.8))
print(uValue)
if (uValue > 40):
self.switcher = 2
self.h.returnCenter()
drive.moveForward()
def aroundObject(self):
if (self.direction == 0):
time.sleep(0.5)
# drive.pivotTurn90(20,40)
drive.pivotTurn90(15, 40)
else:
time.sleep(0.5)
drive.pivotTurn90(40, 20)
self.switcher = 0
def getUltrasonic(self):
try:
return BP.get_sensor(BP.PORT_4)
except brickpi3.SensorError as error:
print(error)
return 0
maxY = 400
minY = 50
minX = 0
maxX = 640
minPan = 175
maxPan = 150
minTilt = 100
maxTilt = 150
shutdownTime = 2
switchOlder = False
confidence = float(0.5)
nms_thresh = float(0.3)
#########################
heads = [head.Head() for count in range(4)]
dmx_data = list()
lightSettingsOn = list([255, 0, 0, 0, 200])
lightSettingsOff = list([255, 0, 0, 0, 0])
sender = sacn.sACNsender(bind_port=6553, bind_address="10.8.220.23")
def findList(list, obj):
counter = 0
for i in list:
if i == obj:
return counter
counter += 1
return -1
pygame.mixer.music.play()
#
# INITIALIZATION
#
# Joystick
j = xbox.Joystick()
# Maestro Controllers
servo = maestro.Controller()
# DriveTrain
drivetrain = drive.DriveTrain(servo, CH_RIGHT_MOTOR, CH_LEFT_MOTOR)
speedtoggle = True # false = slow, true = normal
# Head servo
head = head.Head(servo)
# Emotions
emotion = emotions.Emotions(head)
# Sound
pygame.mixer.init(22050, -16, 1, 1024)
channel = pygame.mixer.Channel(1)
sounds = [
"Chatter 2.mp3", "Motor whir 3.mp3", "Jitters.mp3", "Motor whir 4.mp3",
"Ohhhh.mp3", "Oooh.mp3", "Shakey shakey.mp3", "Whir Click.mp3",
"Whoa 2.mp3"
]
#
# MAIN LOOP
#
print("Robot Online")