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"
# Overrides current face being tracked by WebUI
self.EVENT_TRACK_FACE = "track_face"
self.TOPIC_FACE_LOCATIONS = "/camera/face_locations"
# 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)
# 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)
# 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
