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