class FaceTrack:
# Control flags. Ideally FaceTrack should publish targets using ros_commo EvaControl class.
C_EYES = 16
C_FACE = 32
# Facetracking will be disabled if neither of those flags are set
C_FACE_TRACKING = C_FACE | C_EYES
def __init__(self):
rospy.init_node("OpenCog_Facetracker")
logger.info("Starting OpenCog Face Tracker ROS Node")
# The OpenCog API. This is used to send face data to OpenCog.
self.atomo = FaceAtomic()
# List of currently visible faces
self.visible_faces = []
# List of locations of currently visible faces
self.face_locations = {}
# List of no longer visible faces, but seen recently.
self.recent_locations = {}
# How long (in seconds) to keep around a recently seen, but now
# lost face. tf does the tracking for us.
self.RECENT_INTERVAL = 20
# Current look-at-target
self.look_at = 0
self.gaze_at = 0
self.glance_at = 0
self.first_glance = -1
self.glance_howlong = -1
# How often we update the look-at target.
self.LOOKAT_INTERVAL = 0.1
self.last_lookat = 0
# Last time that the list of active faces was vacuumed out.
self.last_vacuum = 0
self.VACUUM_INTERVAL = 1
# Subscribed pi_vision topics and events
self.TOPIC_FACE_EVENT = "/camera/face_event"
self.EVENT_NEW_FACE = "new_face"
self.EVENT_LOST_FACE = "lost_face"
self.EVENT_RECOGNIZED_FACE = "recognized_face"
# Overrides current face being tracked by WebUI
self.EVENT_TRACK_FACE = "track_face"
self.TOPIC_FACE_LOCATIONS = "/camera/face_locations"
self.control_mode = 255
# Subscribed OpenCog commands
self.TOPIC_GLANCE_FACE = "/opencog/glance_at"
self.TOPIC_LOOKAT_FACE = "/opencog/look_at"
self.TOPIC_GAZEAT_FACE = "/opencog/gaze_at"
rospy.Subscriber(self.TOPIC_GLANCE_FACE, Int32, self.glance_at_cb)
rospy.Subscriber(self.TOPIC_LOOKAT_FACE, Int32, self.look_at_cb)
rospy.Subscriber(self.TOPIC_GAZEAT_FACE, Int32, self.gaze_at_cb)
rospy.Subscriber("/manyears/source_pose",PoseStamped, \
self.snd1_cb)
# Published blender_api topics
self.TOPIC_FACE_TARGET = "/blender_api/set_face_target"
self.TOPIC_GAZE_TARGET = "/blender_api/set_gaze_target"
# Face appearance/disappearance from pi_vision
rospy.Subscriber(self.TOPIC_FACE_EVENT, FaceEvent, self.face_event_cb)
# Face location information from pi_vision
rospy.Subscriber(self.TOPIC_FACE_LOCATIONS, Faces, self.face_loc_cb)
# Which face to look at
# rospy.Subscriber(self.TOPIC_FACE_TARGET, xxxFaceEvent, xxxself.face_event_cb)
rospy.Subscriber("chatbot_speech", ChatMessage,
self.stt_cb)
# Where to look
self.look_pub = rospy.Publisher(self.TOPIC_FACE_TARGET,
Target, queue_size=10)
self.gaze_pub = rospy.Publisher(self.TOPIC_GAZE_TARGET,
Target, queue_size=10)
# Frame in which coordinates will be returned from transformation
self.LOCATION_FRAME = "blender"
# Transform Listener. Tracks history for RECENT_INTERVAL.
self.tf_listener = tf.TransformListener(False, \
rospy.Duration(self.RECENT_INTERVAL))
rospy.Subscriber("/behavior_control", Int32, \
self.behavior_control_callback)
# Control Eeys and face by default
#self.control_mode = 255
# ---------------------------------------------------------------
# Public API. Use these to get things done.
# Turn only the eyes towards the given target face; track that face.
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
# Turn entire head to look at the given target face. The head-turn is
# performed only once per call; after that, the eyes will then
# automatically track that face, but the head will not. Call again,
# to make the head move again.
#
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 glance_at_face(self, faceid, howlong):
logger.info("glance at: " + str(faceid) + " for " + str(howlong) + " seconds")
self.glance_at = faceid
self.glance_howlong = howlong
self.first_glance = -1
def study_face(self, faceid, howlong):
logger.info("study: " + str(faceid) + " for " + str(howlong) + " seconds")
self.glance_at = faceid
self.glance_howlong = howlong
self.first_glance = -1
# ---------------------------------------------------------------
# Private functions, not for use outside of this class.
def look_at_cb(self, msg):
self.look_at_face(msg.data)
def gaze_at_cb(self, msg):
self.gaze_at_face(msg.data)
def glance_at_cb(self, msg):
self.glance_at_face(msg.data, 0.5)
def snd1_cb(self,msg):
self.save_snd1(msg.pose.position.x,msg.pose.position.y,msg.pose.position.z)
# ---------------------------------------------------------------
# Private functions, not for use outside of this class.
#
# Start tracking a face
def save_snd1(self,x,y,z):
#correct by translation and rotation for camera space
r = [[0],[0],[0],[0]]
vs = [[x],[y],[z],[1]]
n=0
#fill cmat correctly from icp results
'''
0.943789 0.129327 0.304204 0.00736024
-0.131484 0.991228 -0.0134787 0.00895614
-0.303278 -0.0272767 0.952513 0.0272001
0 0 0 1
'''
cmat = [[0.943789,0.129327,0.304204,0.00736024],
[-0.131484,0.991228,-0.0134787,0.00895614],
[-0.303278,-0.0272767,0.952513,0.0272001],
[0, 0, 0, 1]]
for i in range(0,3):
for j in range(0,3):
r[i][0]+=cmat[i][j]*vs[j][0]
xc=r[0][0]
yc=r[1][0]
zc=r[2][0]
self.atomo.save_snd1(xc,yc,zc)
def add_face(self, faceid):
if faceid in self.visible_faces:
return
self.visible_faces.append(faceid)
logger.info("New face added to visibile faces: " +
str(self.visible_faces))
self.atomo.add_face_to_atomspace(faceid)
# Stop tracking a face
def remove_face(self, faceid):
self.atomo.remove_face_from_atomspace(faceid)
if faceid in self.visible_faces:
self.visible_faces.remove(faceid)
logger.info("Lost face; visibile faces now: " + str(self.visible_faces))
# Update face location in octomap if it is in visible faces list
def update_face_loc(self,face):
if face.id in self.visible_faces:
self.atomo.update_face_octomap(face.id,face.point.x,face.point.y,face.point.z)
# ----------------------------------------------------------
# Main look-at action driver. Should be called at least a few times
# per second. This publishes all of the eye-related actions that the
# blender api robot head should be performing.
#
# This performs multiple actions:
# 1) updates the list of currently visible faces
# 2) updates the list of recently seen (but now lost) faces
# 3) If we should be looking at one of these faces, then look
# at it, now.
#
# Note that step 3 is a kind-of imprecise visual-servoing. That is,
# if the look-at face is moving around, the step 3 will result in
# her automatically tracking that face, as it moves.
def do_look_at_actions(self) :
now = time.time()
# Should we be glancing elsewhere? If so, then do it, and
# do it actively, i.e. track that face intently.
if 0 < self.glance_at:
if self.first_glance < 0:
self.first_glance = now
if (now - self.first_glance < self.glance_howlong):
# Find latest position known
try:
trg = self.face_target(self.glance_at)
if self.control_mode & self.C_EYES:
self.gaze_pub.publish(trg)
except:
logger.info("Error: no face to glance at!")
self.glance_at = 0
self.first_glance = -1
else :
# We are done with the glance. Resume normal operations.
self.glance_at = 0
self.first_glance = -1
# Publish a new lookat target to the blender API
elif (now - self.last_lookat > self.LOOKAT_INTERVAL):
self.last_lookat = now
# Update the eye position, if need be. Skip, if there
# is also a pending look-at to perform.
if 0 < self.gaze_at and self.look_at <= 0:
# logger.info("Gaze at id " + str(self.gaze_at))
try:
if not self.gaze_at in self.visible_faces:
raise Exception("Face not visible")
trg = self.face_target(self.gaze_at)
if self.control_mode & self.C_EYES:
self.gaze_pub.publish(trg)
except tf.LookupException as lex:
logger.info("Warning: TF has forgotten about face id:" +
str(self.look_at))
self.remove_face(self.look_at)
self.look_at_face(0)
return
except Exception as ex:
logger.info("Error: no gaze-at target: ")
self.gaze_at_face(0)
return
if 0 < self.look_at:
logger.info("Look at id: " + str(self.look_at))
try:
if not self.look_at in self.visible_faces:
raise Exception("Face not visible")
trg = self.face_target(self.look_at)
if self.control_mode & self.C_FACE:
self.look_pub.publish(trg)
self.gaze_pub.publish(trg)
except tf.LookupException as lex:
logger.info("Warning: TF has forgotten about face id: " +
str(self.look_at))
self.remove_face(self.look_at)
self.look_at_face(0)
return
except Exception as ex:
logger.info("Error: no look-at target: " + str(ex))
self.look_at_face(0)
return
# Now that we've turned to face the target, don't do it
# again; instead, just track with the eyes.
self.gaze_at = self.look_at
self.look_at = -1
# Adds given face to atomspace as requested face
def track_face(self, faceid):
if faceid in self.visible_faces:
logger.info("Face requested interaction: " + str(faceid))
self.atomo.add_tracked_face_to_atomspace(faceid)
return
# ----------------------------------------------------------
def stt_cb(self,msg):
if msg.confidence >= 50:
self.atomo.who_said(msg.utterance)
# pi_vision ROS callbacks
# pi_vision ROS callback, called when a new face is detected,
# or a face is lost. Note: I don't think this is really needed,
# the face_loc_cb accomplishes the same thing. So maybe should
# remove this someday.
def face_event_cb(self, data):
if not self.control_mode & self.C_FACE_TRACKING:
return
if data.face_event == self.EVENT_NEW_FACE:
self.add_face(data.face_id)
elif data.face_event == self.EVENT_LOST_FACE:
self.remove_face(data.face_id)
elif data.face_event == self.EVENT_TRACK_FACE:
self.track_face(data.face_id)
elif data.face_event == self.EVENT_RECOGNIZED_FACE:
self.atomo.face_recognition(data.face_id,data.recognized_id)
# pi_vision ROS callback, called when pi_vision has new face
# location data for us. Because this happens frequently (10x/second)
# we also use this as the main update loop, and drive all look-at
# actions from here.
def face_loc_cb(self, data):
if not self.control_mode & self.C_FACE_TRACKING:
return
for face in data.faces:
self.update_face_loc(face)
# Now perform all the various looking-at actions
self.do_look_at_actions()
# Queries tf_listener to get latest available position
# Throws TF exceptions if transform cannot be returned
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 behavior_control_callback(self, data):
# Were there facetracking enabled
facetracking = self.control_mode & self.C_FACE_TRACKING
self.control_mode = data.data
print("New Control mode %i" % self.control_mode )
if facetracking > 0 and self.control_mode & self.C_FACE_TRACKING == 0:
self.atomo.update_ft_state_to_atomspace(False)
# Need to clear faces:
for face in self.visible_faces[:]:
self.remove_face(face)
elif self.control_mode & self.C_FACE_TRACKING > 0:
self.atomo.update_ft_state_to_atomspace(True)
