def SetHeadFocus(self,pos,speed):
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = speed
self.head_focus_pub.publish(msg)
def SetGazeFocus(self,pos,speed):
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = speed
self.gaze_focus_pub.publish(msg)
def look_at_point(self, x, y, z):
print "look at point: ", x, y, z
trg = Target()
trg.x = x
trg.y = y
trg.z = z
self.turn_pub.publish(trg)
def gaze_at_point(self, x, y, z):
print "gaze at point: ", x, y, z
trg = Target()
trg.x = x
trg.y = y
trg.z = z
self.gaze_pub.publish(trg)
def look_at_point(self, x, y, z): print "look at point: ", x, y, z trg = Target() trg.x = x trg.y = y trg.z = z self.turn_pub.publish(trg)
def gaze_at_point(self, x, y, z): print "gaze at point: ", x, y, z trg = Target() trg.x = x trg.y = y trg.z = z self.gaze_pub.publish(trg)
def SetHeadFocus(self, pos):
# publish head focus message
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = 5.0
self.head_focus_pub.publish(msg)
def face_target(self, faceid): (trans, rot) = self.tf_listener.lookupTransform( \ self.LOCATION_FRAME, 'Face' + str(faceid), rospy.Time(0)) t = Target() t.x = trans[0] t.y = trans[1] t.z = trans[2] + self.blackboard["z_pitch_eyes"] return t
def face_target(self, faceid):
(trans, rot) = self.tf_listener.lookupTransform( \
self.LOCATION_FRAME, 'Face' + str(faceid), rospy.Time(0))
t = Target()
t.x = trans[0]
t.y = trans[1]
t.z = trans[2]
return t
def look_at_point(self, x, y, z): xyz1=numpy.array([x,y,z,1.0]) xyz=numpy.dot(self.conv_mat,xyz1) trg = Target() trg.x = xyz[0] trg.y = xyz[1] trg.z = xyz[2] print "look at point: ", trg.x, trg.y, trg.z self.turn_pub.publish(trg)
def gaze_at_point(self, x, y, z):
xyz1 = numpy.array([x, y, z, 1.0])
xyz = numpy.dot(self.conv_mat, xyz1)
trg = Target()
trg.x = xyz[0]
trg.y = xyz[1]
trg.z = xyz[2]
# print "gaze at point: ", trg.x, trg.y, trg.z
self.gaze_pub.publish(trg)
def gaze_at_point(self, x, y, z): xyz1 = numpy.array([x,y,z,1.0]) xyz = numpy.dot(self.conv_mat, xyz1) trg = Target() trg.x = xyz[0] trg.y = xyz[1] trg.z = xyz[2] # print "gaze at point: ", trg.x, trg.y, trg.z self.gaze_pub.publish(trg)
def look_at_point(self, x, y, z):
xyz1 = numpy.array([x, y, z, 1.0])
xyz = numpy.dot(self.conv_mat, xyz1)
trg = Target()
trg.x = xyz[0]
trg.y = xyz[1]
trg.z = xyz[2]
print "look at point: ", trg.x, trg.y, trg.z
self.turn_pub.publish(trg)
def look_at_face(self, faceid):
logger.info("look at: " + str(faceid))
# Look at neutral position, 1 meter in front
if 0 == faceid :
trg = Target()
trg.x = 1.0
trg.y = 0.0
trg.z = 0.0
self.look_pub.publish(trg)
self.last_lookat = 0
if faceid not in self.visible_faces :
self.look_at = 0
return
self.look_at = faceid
def gaze_at_face(self, faceid):
logger.info("gaze at: " + str(faceid))
# Look at neutral position, 1 meter in front
if 0 == faceid :
trg = Target()
trg.x = 1.0
trg.y = 0.0
trg.z = 0.0
self.gaze_pub.publish(trg)
self.last_lookat = 0
if faceid not in self.visible_faces_blobs :
self.gaze_at = 0
return
self.gaze_at = faceid
def look_at_face(self, faceid):
print ("look at: " + str(faceid))
# Look at neutral position, 1 meter in front
if 0 == faceid :
trg = Target()
trg.x = 1.0
trg.y = 0.0
trg.z = 0.0
self.look_pub.publish(trg)
self.last_lookat = 0
if faceid not in self.visible_faces :
self.look_at = 0
return
self.look_at = faceid
def look_at_face(self, faceid):
logger.info("look at: " + str(faceid))
# Look at neutral position, 1 meter in front
if 0 == faceid :
trg = Target()
trg.x = 1.0
trg.y = 0.0
trg.z = 0.0
if self.control_mode & self.C_FACE:
self.look_pub.publish(trg)
self.last_lookat = 0
if faceid not in self.visible_faces :
self.look_at = 0
return
self.look_at = faceid
def gaze_at_face(self, faceid):
logger.info("gaze at: " + str(faceid))
# Look at neutral position, 1 meter in front
if 0 == faceid:
trg = Target()
trg.x = 1.0
trg.y = 0.0
trg.z = 0.0
if self.control_mode & self.C_EYES:
self.gaze_pub.publish(trg)
self.last_lookat = 0
if faceid not in self.visible_faces:
self.gaze_at = 0
return
self.gaze_at = faceid
def face_toward_point(self, x, y, z, speed):
""" Turn the robot's face towards the given target point.
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.face_target_pub.publish(msg)
rospy.logdebug("published face_(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
def unpack_config_look_around(self, config):
def get_values(from_config, num_values):
rtn_values = [float(z.strip()) for z in from_config.split(",")]
if len(rtn_values) != num_values:
raise Exception("List lengths don't match!")
return rtn_values
x_coordinates = [float(x.strip()) for x in config.get("boredom", "search_for_attention_x").split(",")]
numb = len(x_coordinates)
y_coordinates = get_values(config.get("boredom", "search_for_attention_y"), numb)
z_coordinates = get_values(config.get("boredom", "search_for_attention_z"), numb)
for (x, y, z) in zip (x_coordinates, y_coordinates, z_coordinates):
trg = Target()
trg.x = x
trg.y = y
trg.z = z
self.blackboard["search_for_attention_targets"].append(trg)
def lookat_salientP_cb(self, data):
loc = []
loc = data.positions[0]
degree = data.degree
t = Target()
t.x = 1.0
if loc.x <= 0.5:
loc.x = (0.5 - loc.x)
else:
loc.x = (0.5 - loc.x)
t.y = loc.x
if loc.y <= 0.5:
loc.y = (0.5 - loc.y)
else:
loc.y = (0.5 - loc.y)
t.z = loc.y
if self.GLOBAL_FLAG and degree >= 7: # Look to Certainly Determined Salient Point
self.look_pub.publish(t)
self.look_gaze.publish(t)