def callback_speech(self, msg): changes = Changes() if self.last_speaking_id != "": if self.ctx.worlds()[self.world].timeline().situations().has( self.last_speaking_id): fact = self.ctx.worlds()[self.world].timeline().situations()[ self.last_speaking_id] updated = False for property in fact.properties: if property.name == "speech": property.data = msg.data updated = True if updated is False: speech_property = Property(name="speech", data=msg.data) fact.properties.append(speech_property) changes.situations_to_update.append(fact) self.ctx.worlds()[self.world].update(changes)
def updateSituations(self, world_name, header, camera_id, visibilities): """ """ situations = [] situations_to_update = [] situations_ids_ended = [] for situation in self.ctx.worlds()[world_name].timeline().situations(): if situation.end.data == rospy.Time(0): if self.ctx.worlds()[world_name].timeline().situations( ).get_situation_property(situation.id, "predicate") == "isVisible": subject_id = self.ctx.worlds()[world_name].timeline( ).situations().get_situation_property( situation.id, "subject") object_id = self.ctx.worlds()[world_name].timeline( ).situations().get_situation_property( situation.id, "object") if camera_id == subject_id: if object_id in visibilities: situation.confidence = visibilities[object_id] print "update : %s with %f confidence" % ( situation.description, visibilities[object_id]) del visibilities[object_id] situations_to_update.append(situation) else: situation.end.data = header.stamp print "end : %s" % situation.description situations_ids_ended.append(situation.id) situations.append(situation) for id_seen, visibility_score in visibilities.items(): situation = Situation() situation.id = str(uuid.uuid4()) situation.type = FACT situation.description = self.worlds[world_name].scene.nodes[ id_seen].name + " is visible by " + self.worlds[ world_name].scene.nodes[camera_id].name predicate = Property() predicate.name = "predicate" predicate.data = "isVisibleBy" situation.properties.append(predicate) subject = Property() subject.name = "subject" subject.data = camera_id situation.properties.append(subject) object = Property() object.name = "object" object.data = id_seen situation.properties.append(object) situation.confidence = visibility_score situation.start.data = header.stamp situation.end.data = rospy.Time(0) if self.verbose: print "start : %s" % situation.description situations_to_update.append(situation) situations.append(situation) #self.ctx.worlds()[world_name].timeline().update(situations_to_update) #self.ctx.worlds()[world_name].timeline().remove(situations_ids_ended) return situations
def callback(self, gaze_msg, voice_msg, person_msg): # self.look_at_threshold = rospy.get_param("look_at_threshold", LOOK_AT_THRESHOLD) # self.engaging_distance = rospy.get_param("engaging_distance", ENGAGING_DISTANCE) # self.engaging_min_time = rospy.get_param("engaging_min_time", ENGAGING_MIN_TIME) # self.nb_min_detection = rospy.get_param("nb_min_detection", NB_MIN_DETECTION) # self.max_dist = rospy.get_param("max_dist", MAX_DIST) # self.close_max_distance = rospy.get_param("close_max_distance", CLOSE_MAX_DISTANCE) # self.near_max_distance = rospy.get_param("near_max_distance", NEAR_MAX_DISTANCE) # self.min_engagement_duration = rospy.get_param("min_engagement_duration", MIN_ENGAGEMENT_DURATION) # self.person_persistence = rospy.get_param("person_persistence", PERSON_PERSISTENCE) #print "perception callback called" self.changes = Changes() now = rospy.Time.now() gaze_attention_point = None voice_attention_point = None min_dist = 10000 self.currently_perceived_ids = [] self.currently_near_ids = [] self.currently_close_ids = [] self.currently_speaking_ids = [] self.currently_looking_at = {} self.currently_speaking_to = {} self.start_engaging_time = {} self.currently_engaging_with = {} self.currently_engaged_with = {} min_dist = float("inf") min_id_0 = None i = 0 min_it = None for person in person_msg.data: if person.head_distance < self.engaging_distance: if person.head_distance < min_dist: min_dist = person.head_distance min_id_0 = person.person_id min_it = i i += 1 if min_id_0 is not None: person_msg.data[min_it].person_id = 0 for gaze in gaze_msg.data: if gaze.person_id == min_id_0: gaze.person_id = 0 if len(person_msg.data) > 0: for person in person_msg.data: if str(person.person_id) not in self.nb_detection: self.nb_detection[str(person.person_id)] = 0 else: self.nb_detection[str(person.person_id)] += 1 min_id = person.person_id for id in person.alternate_ids: if id < min_id: min_id = id self.alternate_id_map[str(person.person_id)] = str(min_id) if self.nb_detection[str( person.person_id)] > self.nb_min_detection: if person.head_distance < self.max_dist: if str(person.person_id) not in self.persons: new_node = Node() new_node.name = "human-" + str(person.person_id) new_node.id = str(person.person_id) new_node.type = CAMERA clipnear_property = Property( name="clipnear", data=str(DEFAULT_CLIP_PLANE_NEAR)) clipfar_property = Property( name="clipfar", data=str(DEFAULT_CLIP_PLANE_FAR)) hfov_property = Property( name="hfov", data=str(DEFAULT_HORIZONTAL_FOV)) aspect_property = Property( name="aspect", data=str(DEFAULT_ASPECT)) class_property = Property(name="class", data="Human") new_node.properties.append(clipnear_property) new_node.properties.append(clipfar_property) new_node.properties.append(hfov_property) new_node.properties.append(aspect_property) new_node.properties.append(class_property) self.persons[str(person.person_id)] = new_node self.changes.nodes_to_update.append( self.persons[str(person.person_id)]) #self.predicates_map[str(person.person_id)] = [] self.currently_perceived_ids.append( str(person.person_id)) self.human_distances[str( person.person_id)] = person.head_distance t = [ person.head_pose.position.x, person.head_pose.position.y, person.head_pose.position.z ] q = [ person.head_pose.orientation.x, person.head_pose.orientation.y, person.head_pose.orientation.z, person.head_pose.orientation.w ] offset = euler_matrix(0, math.radians(90), math.radians(90), "rxyz") transform = numpy.dot(transformation_matrix(t, q), offset) position = translation_from_matrix(transform) quaternion = quaternion_from_matrix(transform) if self.ctx.worlds()[self.world].scene().nodes().has( str(person.person_id)): try: delta_x = self.persons[str( person.person_id )].position.pose.position.x - position[0] delta_y = self.persons[str( person.person_id )].position.pose.position.y - position[1] delta_z = self.persons[str( person.person_id )].position.pose.position.z - position[2] delta_t = person_msg.header.stamp - self.persons[ str(person.person_id )].last_observation.data self.persons[str( person.person_id )].velocity.position.x = delta_x / delta_t self.persons[str( person.person_id )].velocity.position.y = delta_y / delta_t self.persons[str( person.person_id )].velocity.position.z = delta_z / delta_t except Exception: pass self.persons[str( person.person_id )].position.pose.position.x = position[0] self.persons[str( person.person_id )].position.pose.position.y = position[1] self.persons[str( person.person_id )].position.pose.position.z = position[2] self.persons[str( person.person_id )].position.pose.orientation.x = quaternion[0] self.persons[str( person.person_id )].position.pose.orientation.y = quaternion[1] self.persons[str( person.person_id )].position.pose.orientation.z = quaternion[2] self.persons[str( person.person_id )].position.pose.orientation.w = quaternion[3] self.persons[str( person.person_id )].last_observation.data = person_msg.header.stamp self.changes.nodes_to_update.append(self.persons[str( person.person_id)]) transform = TransformStamped() transform.header = person_msg.header transform.transform.translation.x = position[0] transform.transform.translation.y = position[1] transform.transform.translation.z = position[2] transform.transform.rotation.x = quaternion[0] transform.transform.rotation.y = quaternion[1] transform.transform.rotation.z = quaternion[2] transform.transform.rotation.w = quaternion[3] transform.child_frame_id = "gaze_human_" + str( person.person_id) self.tfBroadcaster.sendTransform(transform) # min_dist = 10000 # min_id = None # for voice in voice_msg.data: # if str(voice.person_id) in self.persons: # if voice.is_speaking: # self.currently_speaking_ids.append(str(voice.person_id)) # if str(voice.person_id) in self.distances: # if self.human_distances[str(voice.person_id)] < min_dist: # min_dist = self.human_distances[str(voice.person_id)] # min_id = voice.person_id # if min_id is not None: # voice_attention_point = PointStamped() # voice_attention_point.header.frame_id = str(min_id) # voice_attention_point.point = Point(0, 0, 0) for gaze in gaze_msg.data: if str(gaze.person_id) in self.persons: if self.human_distances[str( gaze.person_id)] < self.engaging_distance: if str(gaze.person_id ) not in self.last_engagement_time: self.last_engagement_time[str(gaze.person_id)] = {} if gaze.probability_looking_at_robot > self.look_at_threshold: self.currently_looking_at[str( gaze.person_id)] = "robot" self.last_engagement_time[str( gaze.person_id)]["robot"] = now self.currently_engaging_with[str( gaze.person_id)] = "robot" else: if gaze.probability_looking_at_screen > self.look_at_threshold: self.currently_looking_at[str( gaze.person_id)] = "robot" self.last_engagement_time[str( gaze.person_id)]["robot"] = now self.currently_engaging_with[str( gaze.person_id)] = "robot" else: for attention in gaze.attentions: if attention.target_id in self.persons: if attention.probability_looking_at_target > self.look_at_threshold: if str(attention.target_id ) in self.alternate_id_map: self.currently_looking_at[str( gaze.person_id )] = self.alternate_id_map[str( attention.target_id)] self.currently_engaging_with[ str( gaze.person_id )] = self.alternate_id_map[ str(attention.target_id )] self.last_engagement_time[str( gaze.person_id )][self.alternate_id_map[str( attention.target_id )]] = now for person_id in self.last_engagement_time: for person2_id in self.last_engagement_time[person_id]: if person_id + "isEngagingWith" + person2_id in self.predicates_map: fact = self.ctx.worlds()[self.world].timeline( ).situations()[self.predicates_map[person_id + "isEngagingWith" + person2_id]] if fact is not None: if (now - fact.start.data ).to_sec() > self.engaging_min_time: self.currently_engaged_with[ person_id] = person2_id else: if person_id + "isEngagedWith" + person2_id in self.predicates_map: if (now - self.last_engagement_time[person_id] [person2_id] ).to_sec() < self.min_engagement_duration: self.currently_engaged_with[ person_id] = person2_id # min_dist = 10000 # min_id = None # for id, dist in self.human_distances.items(): # if id in self.currently_perceived_ids: # if dist < NEAR_MAX_DISTANCE: # if min_dist > dist: # min_dist = dist # min_id = id # if dist < CLOSE_MAX_DISTANCE: # self.currently_close_ids.append(id) # else: # self.currently_near_ids.append(id) # if min_id is not None: # voice_attention_point = PointStamped() # voice_attention_point.header.frame_id = str(min_id) # voice_attention_point.point = Point(0, 0, 0) for id in self.currently_perceived_ids: if id not in self.previously_perceived_ids: # start fact # print("start: robot is perceiving human-"+id) fact = Situation(description="robot is perceiving human-" + id, type=FACT, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data="robot") object_property = Property(name="object", data=id) predicate_property = Property(name="predicate", data="isPerceiving") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[id + "isPerceiving"] = fact.id self.changes.situations_to_update.append(fact) for id in self.previously_perceived_ids: if id not in self.currently_perceived_ids: # stop fact if id + "isPerceiving" in self.predicates_map: if self.ctx.worlds()[ self.world].timeline().situations().has( self.predicates_map[id + "isPerceiving"]): # print("stop: "+"human-"+id+" is perceived") fact = self.ctx.worlds()[self.world].timeline( ).situations()[self.predicates_map[id + "isPerceiving"]] fact.end.data = now fact.description = fact.description.replace( "is", "was") self.changes.situations_to_update.append(fact) del self.predicates_map[id + "isPerceiving"] for id in self.currently_near_ids: if id not in self.previously_near_ids: # start fact # print("start: "+"human-"+id+" is near") fact = Situation(description="human-" + str(id) + " is near", type=FACT, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=id) predicate_property = Property(name="predicate", data="isNear") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(predicate_property) self.predicates_map[id + "isNear"] = fact.id self.changes.situations_to_update.append(fact) for id in self.previously_near_ids: if id not in self.currently_perceived_ids or id not in self.currently_near_ids: # stop fact if id + "isNear" in self.predicates_map: print("stop: " + "human-" + id + " is near") if self.ctx.worlds()[ self.world].timeline().situations().has( self.predicates_map[id + "isNear"]): fact = self.ctx.worlds()[self.world].timeline( ).situations()[self.predicates_map[id + "isNear"]] fact.end.data = now fact.description = fact.description.replace( "is", "was") self.changes.situations_to_update.append(fact) del self.predicates_map[id + "isNear"] for id in self.currently_close_ids: if id not in self.previously_close_ids: # start fact # print "start: "+"human-"+id+" is close" fact = Situation(description="human-" + id + " is close", type=FACT, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=id) predicate_property = Property(name="predicate", data="isClose") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(predicate_property) self.predicates_map[id + "isClose"] = fact.id self.changes.situations_to_update.append(fact) for id in self.previously_close_ids: if id not in self.currently_perceived_ids or id not in self.currently_close_ids: # stop fact if id + "isClose" in self.predicates_map: if self.ctx.worlds()[ self.world].timeline().situations().has( self.predicates_map[id + "isClose"]): print("end: " + "human-" + id + " is close") fact = self.ctx.worlds()[self.world].timeline( ).situations()[self.predicates_map[id + "isClose"]] fact.end.data = now fact.description = fact.description.replace( "is", "was") self.changes.situations_to_update.append(fact) del self.predicates_map[id + "isClose"] # for id in self.currently_speaking_ids: # if id not in self.previously_speaking_ids: # # start fact # print("start: "+"human-"+id+" is speaking") # fact = Situation(description="human-"+id+" is speaking", type=FACT, id=str(uuid.uuid4().hex)) # subject_property = Property(name="subject", data=id) # predicate_property = Property(name="predicate", data="isSpeaking") # fact.start.data = now # fact.end.data = rospy.Time(0) # fact.properties.append(subject_property) # fact.properties.append(predicate_property) # self.predicates_map[id+"isSpeaking"] = fact.id # self.changes.situations_to_update.append(fact) # for id in self.previously_speaking_ids: # if id not in self.currently_perceived_ids or id not in self.currently_speaking_ids: # # stop fact # if id+"isSpeaking" in self.predicates_map: # if self.ctx.worlds()[self.world].timeline().situations().has(self.predicates_map[id+"isSpeaking"]): # print("stop: "+"human-"+id+" is speaking") # fact = self.ctx.worlds()[self.world].timeline().situations()[self.predicates_map[id+"isSpeaking"]] # fact.end.data = now # fact.description = fact.description.replace("is","was") # self.changes.situations_to_update.append(fact) # self.last_speaking_id = fact.id # del self.predicates_map[id+"isSpeaking"] for subject_id in self.currently_looking_at.keys(): object_id = self.currently_looking_at[subject_id] start_fact = False if subject_id not in self.previously_looking_at: start_fact = True else: if subject_id not in self.previously_looking_at: start_fact = True else: if object_id != self.previously_looking_at[subject_id]: start_fact = True if start_fact is True: if object_id != "robot": if self.ctx.worlds()[self.world].scene().nodes().has( object_id): object = self.ctx.worlds()[ self.world].scene().nodes()[object_id] # print("start: "+"human-"+subject_id+" is looking at "+object.name) fact = Situation(description="human-" + subject_id + " is looking at " + object.name, type=ACTION, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=subject_id) object_property = Property(name="object", data=object_id) predicate_property = Property(name="predicate", data="isLookingAt") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[subject_id + "isLookingAt" + object_id] = fact.id self.changes.situations_to_update.append(fact) else: # print("start: "+"human-"+subject_id+" is looking at "+object_id) fact = Situation(description="human-" + subject_id + " is looking at " + object_id, type=ACTION, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=subject_id) object_property = Property(name="object", data=object_id) predicate_property = Property(name="predicate", data="isLookingAt") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[subject_id + "isLookingAt" + object_id] = fact.id self.changes.situations_to_update.append(fact) for subject_id in self.previously_looking_at.keys(): object_id = self.previously_looking_at[subject_id] stop_fact = False if subject_id not in self.currently_looking_at: stop_fact = True else: if object_id != self.previously_looking_at[subject_id]: stop_fact = True if stop_fact is True: #if self.ctx.worlds()[self.world].scene().nodes().has(object_id): #object = self.ctx.worlds()[self.world].scene().nodes()[object_id] # print("stop: "+"human-"+subject_id+" is looking at "+object_id) fact = self.ctx.worlds()[self.world].timeline().situations()[ self.predicates_map[subject_id + "isLookingAt" + object_id]] fact.end.data = now fact.description = fact.description.replace("is", "was") self.changes.situations_to_delete.append(fact.id) del self.predicates_map[subject_id + "isLookingAt" + object_id] for subject_id in self.currently_engaging_with.keys(): object_id = self.currently_engaging_with[subject_id] start_fact = False if subject_id not in self.previously_engaging_with: start_fact = True else: if object_id != self.previously_engaging_with[subject_id]: start_fact = True if start_fact is True: if object_id != "robot": if self.ctx.worlds()[self.world].scene().nodes().has( object_id): # print("start: "+"human-"+subject_id+" is engaging with "+object.name) fact = Situation(description="human-" + subject_id + " is engaging with " + object.name, type=ACTION, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=subject_id) object_property = Property(name="object", data=object_id) predicate_property = Property(name="predicate", data="isEngagingWith") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[subject_id + "isEngagingWith" + object_id] = fact.id self.changes.situations_to_update.append(fact) else: # print("start: "+"human-"+subject_id+" is engaging with "+object_id) fact = Situation(description="human-" + subject_id + " is engaging with " + object_id, type=ACTION, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=subject_id) object_property = Property(name="object", data=object_id) predicate_property = Property(name="predicate", data="isEngagingWith") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[subject_id + "isEngagingWith" + object_id] = fact.id self.changes.situations_to_update.append(fact) for subject_id in self.previously_engaging_with.keys(): object_id = self.previously_engaging_with[subject_id] stop_fact = False if subject_id not in self.currently_engaging_with: stop_fact = True else: if object_id != self.currently_engaging_with[subject_id]: stop_fact = True if stop_fact is True: #if self.ctx.worlds()[self.world].scene().nodes().has(object_id): #object = self.ctx.worlds()[self.world].scene().nodes()[object_id] # print("stop: human-"+subject_id+" is engaging with "+object_id) fact = self.ctx.worlds()[self.world].timeline().situations()[ self.predicates_map[subject_id + "isEngagingWith" + object_id]] fact.end.data = now fact.description = fact.description.replace("is", "was") self.changes.situations_to_delete.append(fact.id) del self.predicates_map[subject_id + "isEngagingWith" + object_id] for subject_id in self.currently_engaged_with.keys(): object_id = self.currently_engaged_with[subject_id] start_fact = False if subject_id not in self.previously_engaged_with: start_fact = True else: if object_id not in self.previously_engaged_with[subject_id]: start_fact = True if start_fact is True: if object_id != "robot": if self.ctx.worlds()[self.world].scene().nodes().has( object_id): print("start: " + "human-" + subject_id + " is engaged with " + object.name) fact = Situation(description="human-" + subject_id + " is engaged with " + object.name, type=ACTION, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=subject_id) object_property = Property(name="object", data=object_id) predicate_property = Property(name="predicate", data="isEngagedWith") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[subject_id + "isEngagedWith" + object_id] = fact.id self.changes.situations_to_update.append(fact) else: print("start: " + "human-" + subject_id + " is engaged with " + object_id) fact = Situation(description="human-" + subject_id + " is engaged with " + object_id, type=ACTION, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=subject_id) object_property = Property(name="object", data=object_id) predicate_property = Property(name="predicate", data="isEngagedWith") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[subject_id + "isEngagedWith" + object_id] = fact.id self.changes.situations_to_update.append(fact) for subject_id in self.previously_engaged_with.keys(): object_id = self.previously_engaged_with[subject_id] stop_fact = False if subject_id not in self.currently_engaged_with: stop_fact = True else: if object_id != self.currently_engaged_with[subject_id]: #if self.ctx.worlds()[self.world].scene().nodes().has(object_id): stop_fact = True if stop_fact is True: #if self.ctx.worlds()[self.world].scene().nodes().has(object_id): #object = self.ctx.worlds()[self.world].scene().nodes()[object_id] print("stop: " + "human-" + subject_id + " is engaged with " + object_id) fact = self.ctx.worlds()[self.world].timeline().situations()[ self.predicates_map[subject_id + "isEngagedWith" + object_id]] fact.end.data = now fact.description = fact.description.replace("is", "was") self.changes.situations_to_delete.append(fact.id) del self.predicates_map[subject_id + "isEngagedWith" + object_id] del self.last_engagement_time[subject_id][object_id] # for subject_id in self.currently_speaking_to.keys(): # for object_id in self.currently_speaking_to[subject_id]: # start_fact = False # if subject_id not in self.previously_speaking_to: # start_fact = True # else: # if object_id not in self.previously_speaking_to[subject_id]: # if self.ctx.worlds()[self.world].scene().nodes().has(object_id): # start_fact = True # if start_fact is True: # object = self.ctx.worlds()[self.world].scene().nodes()[object_id] # fact = Situation(description="human is speaking to "+object.name, type=ACTION, id=str(uuid.uuid4().hex)) # subject_property = Property(name="subject", data=subject_id) # object_property = Property(name="object", data=object_id) # predicate_property = Property(name="action", data="isSpeakingTo") # fact.start.data = now # fact.description = fact.description.replace("is","was") # fact.end.data = rospy.Time(0) # fact.properties.append(subject_property) # fact.properties.append(object_property) # fact.properties.append(predicate_property) # self.predicates_map[subject_id+"isSpeakingTo"+object_id] = fact.id # self.changes.situations_to_update.append(fact) # # for subject_id in self.previously_speaking_to.keys(): # for object_id in self.previously_speaking_to[subject_id]: # stop_fact = False # if subject_id not in currently_speaking_at: # stop_fact = True # else: # if object_id not in self.previously_speaking_to[subject_id]: # if self.ctx.worlds()[self.world].scene().nodes().has(object_id): # stop_fact = True # if stop_fact is True: # fact = self.ctx.worlds()[self.world].timeline().situations()[self.predicates_map[subject_id+"isSpeakingTo"+object_id]] # fact.end.data = now # fact.description = fact.description.replace("is","was") # self.changes.situations_to_update(fact) # del self.predicates_map[subject_id+"isSpeakingTo"+object_id] ids_overrided = [] #print("local timeline size : "+str(len(self.ctx.worlds()[self.world].timeline().situations()))) to_repair = [] # if self.person_of_interest is not None: # if rospy.Time().now() - self.last_update_person > rospy.Duration(2.0): # self.person_of_interest = None for situation in self.ctx.worlds()[self.world].timeline().situations(): updated = False deleted = False if situation.end.data != rospy.Time(0): if rospy.Time().now().to_sec() - situation.end.data.to_sec( ) > 4.0: deleted = True subject = self.ctx.worlds()[ self.world].timeline().situations().get_situation_property( situation.id, "subject") object = self.ctx.worlds()[ self.world].timeline().situations().get_situation_property( situation.id, "object") if subject in to_repair and subject in self.alternate_id_map: subject = self.alternate_id_map[subject] if object in to_repair and object in self.alternate_id_map: object = self.alternate_id_map[object] for property in situation.properties: if property.data in self.alternate_id_map and property.data in to_repair: if property.name == "object": property.data = object #print "repair timeline for node :"+self.alternate_id_map[object] updated = True if property.name == "subject": property.data = subject #print "repair timeline for node :"+self.alternate_id_map[subject] updated = True if updated and not deleted: print "repair (update) : " + situation.description self.changes.situations_to_update.append(situation) if deleted: print "repair (delete) : " + situation.description self.changes.situations_to_delete.append(situation.id) for node in self.ctx.worlds()[self.world].scene().nodes(): if node.id in self.alternate_id_map: if self.alternate_id_map[ node. id] != node.id or node.last_observation.data + rospy.Duration( self.person_persistence) < rospy.Time.now(): print "remove node : human-" + node.id #self.clear_local_data(node.id) if node.id in self.nb_detection: self.nb_detection[node.id] = 0 self.changes.nodes_to_delete.append(node.id) self.previously_near_ids = self.currently_near_ids self.previously_close_ids = self.currently_close_ids self.previously_looking_at = self.currently_looking_at self.previously_speaking_ids = self.currently_speaking_ids self.previously_speaking_to = self.currently_speaking_to self.previously_perceived_ids = self.currently_perceived_ids self.previously_engaging_with = self.currently_engaging_with self.previously_engaged_with = self.currently_engaged_with #print ("nb nodes updated : "+str(len(changes.nodes_to_update))) #print ("nb situations updated : "+str(len(changes.situations_to_update))) self.ctx.worlds()[self.world].update(self.changes, header=person_msg.header)
def filter(self, world_name, header, invalidations): """ """ #print "start reasoning" start_reasoning_time = rospy.Time.now() changes = Changes() for mesh_id in invalidations.mesh_ids_updated: changes.meshes_to_update.append(self.meshes()[mesh_id]) for situation_id in invalidations.situation_ids_updated: changes.situations_to_update.append(self.meshes()[mesh_id]) for node in self.ctx.worlds()[world_name].scene().nodes(): if node.type == MESH: if node.id in self.invalidation_time: self.isPerceived[node.id] = ( header.stamp - self.invalidation_time[node.id]) < rospy.Duration( self.perception_duration) else: self.isPerceived[node.id] = True start_fall_reasoning_time = rospy.Time.now() for node_id in self.simulated_node_ids: self.isUnstable[node_id] = False for i in range(0, self.nb_step): p.stepSimulation() for node_id in self.simulated_node_ids: if self.isPerceived[node_id]: node = self.ctx.worlds()[world_name].scene().nodes( )[node_id] infered_position, infered_orientation = p.getBasePositionAndOrientation( self.bullet_node_id_map[node_id]) infered_linear_velocity, infered_angular_velocity = p.getBaseVelocity( self.bullet_node_id_map[node_id]) perceived_position = self.perceived_position[node_id] stability_distance = math.sqrt( pow(perceived_position[0] - infered_position[0], 2) + pow(perceived_position[1] - infered_position[1], 2) + pow(perceived_position[2] - infered_position[2], 2)) is_unstable = stability_distance > self.simulation_tolerance if self.isUnstable[node_id] is False and is_unstable: self.isUnstable[node_id] = True #print node.name + " is unstable after "+str(i)+"/"+str(self.nb_step)+" steps" for object_id in self.isContaining[node_id]: if object_id in self.perceived_position: t_perceived = tf.translation_matrix( self.perceived_position[node_id]) t_infered = tf.translation_matrix( infered_position) offset = tf.translation_from_matrix( np.dot(np.linalg.inv(t_infered), t_perceived)) #if not np.allclose(offset, [0, 0, 0], atol=0.1): object_position, object_orientation = p.getBasePositionAndOrientation( self.bullet_node_id_map[object_id]) object_position = [ object_position[0] + offset[0], object_position[1] + offset[1], object_position[2] + offset[2] ] self.updateBulletNode( world_name, object_id, object_position, object_orientation, self.perceived_linear_velocity[node_id], self.perceived_angular_velocity[node_id]) if self.isUnstable[node_id]: self.updateBulletNode( world_name, node_id, self.perceived_position[node_id], self.perceived_orientation[node_id], self.perceived_linear_velocity[node_id], self.perceived_angular_velocity[node_id]) end_fall_reasoning_time = rospy.Time.now() for node in self.ctx.worlds()[world_name].scene().nodes(): # print len(self.simulated_node_ids) if node.id in self.simulated_node_ids: if self.isUnstable[node.id] is True and self.isPerceived[ node.id] is True: if (self.node_action_state[node.id] == PLACED or self.node_action_state[node.id] == RELEASED ) and self.infer_actions and self.pick_confidence[ node_id] > PICK_CONFIDENCE: print node.name + " picked up" situation = Situation() situation.id = str(uuid.uuid4().hex) situation.type = ACTION situation.description = node.name + " picked up" situation.confidence = PICK_CONFIDENCE situation.start.data = header.stamp situation.end.data = header.stamp situation.properties.append( Property("subject", node.id)) situation.properties.append(Property( "action", "Place")) changes.situations_to_update.append(situation) self.node_action_state[node.id] = HELD self.pick_confidence[node.id] = self.pick_confidence[ node.id] * (1 + PICK_CONFIDENCE) #print self.pick_confidence[node_id] if self.pick_confidence[node.id] > 1.0: self.pick_confidence[node.id] = 1.0 self.place_confidence[node.id] = self.place_confidence[ node.id] * (1 - PICK_CONFIDENCE) if self.place_confidence[node.id] < .1: self.place_confidence[node.id] = 0.1 node.position.pose.position.x = self.perceived_position[ node.id][0] node.position.pose.position.y = self.perceived_position[ node.id][1] node.position.pose.position.z = self.perceived_position[ node.id][2] node.position.pose.orientation.x = self.perceived_orientation[ node.id][0] node.position.pose.orientation.y = self.perceived_orientation[ node.id][1] node.position.pose.orientation.z = self.perceived_orientation[ node.id][2] node.position.pose.orientation.w = self.perceived_orientation[ node.id][3] node.velocity.twist.linear.x = self.perceived_linear_velocity[ node.id][0] node.velocity.twist.linear.y = self.perceived_linear_velocity[ node.id][1] node.velocity.twist.linear.z = self.perceived_linear_velocity[ node.id][2] node.velocity.twist.angular.x = self.perceived_angular_velocity[ node.id][0] node.velocity.twist.angular.y = self.perceived_angular_velocity[ node.id][1] node.velocity.twist.angular.z = self.perceived_angular_velocity[ node.id][2] self.previous_position[node.id] = self.perceived_position[ node.id] self.previous_orientation[ node.id] = self.perceived_orientation[node.id] self.ctx.worlds()[world_name].scene().nodes()[ node.id] = node changes.nodes_to_update.append(node) else: if node.id in self.node_action_state: if self.node_action_state[ node.id] == HELD and self.infer_actions: if self.isPerceived[node.id]: self.place_confidence[ node.id] = self.place_confidence[ node.id] * (1 + PLACE_CONFIDENCE) if self.place_confidence[node.id] > 1.0: self.place_confidence[node.id] = 1.0 self.pick_confidence[ node.id] = self.pick_confidence[ node.id] * (1 - PLACE_CONFIDENCE) if self.pick_confidence[node.id] < .1: self.pick_confidence[node.id] = 0.1 self.release_confidence[ node.id] = self.release_confidence[ node.id] * (1 - RELEASE_CONFIDENCE) if self.release_confidence[node.id] < .1: self.release_confidence[node.id] = 0.1 if self.place_confidence[ node.id] > PLACE_CONFIDENCE: print node.name + " placed" situation = Situation() situation.id = str(uuid.uuid4().hex) situation.type = ACTION situation.description = node.name + " placed" situation.confidence = PLACE_CONFIDENCE situation.start.data = header.stamp situation.end.data = header.stamp situation.properties.append( Property("subject", node.id)) situation.properties.append( Property("action", "Pick")) changes.situations_to_update.append( situation) self.node_action_state[node.id] = PLACED else: self.release_confidence[ node.id] = self.release_confidence[ node.id] * (1 + RELEASE_CONFIDENCE) if self.release_confidence[node.id] > 1.0: self.release_confidence[node.id] = 1.0 self.pick_confidence[ node.id] = self.pick_confidence[ node.id] * (1 - PLACE_CONFIDENCE) if self.pick_confidence[node.id] < .1: self.pick_confidence[node.id] = 0.1 self.place_confidence[ node.id] = self.place_confidence[ node.id] * (1 - PICK_CONFIDENCE) if self.place_confidence[node.id] < .1: self.place_confidence[node.id] = 0.1 if self.release_confidence[ node.id] > RELEASE_CONFIDENCE: print node.name + " released" situation = Situation() situation.id = str(uuid.uuid4().hex) situation.type = ACTION situation.description = node.name + " released" situation.confidence = RELEASE_CONFIDENCE situation.start.data = header.stamp situation.end.data = header.stamp situation.properties.append( Property("subject", node.id)) situation.properties.append( Property("action", "Release")) changes.situations_to_update.append( situation) self.node_action_state[node.id] = RELEASED infered_position, infered_orientation = p.getBasePositionAndOrientation( self.bullet_node_id_map[node.id]) infered_linear_velocity, infered_angular_velocity = p.getBaseVelocity( self.bullet_node_id_map[node.id]) x, y, z = infered_position node.position.pose.position.x = x node.position.pose.position.y = y node.position.pose.position.z = z x, y, z, w = infered_orientation node.position.pose.orientation.x = x node.position.pose.orientation.y = y node.position.pose.orientation.z = z node.position.pose.orientation.w = w x, y, z = infered_linear_velocity node.velocity.twist.linear.x = x node.velocity.twist.linear.y = y node.velocity.twist.linear.z = z x, y, z = infered_angular_velocity node.velocity.twist.angular.x = x node.velocity.twist.angular.y = y node.velocity.twist.angular.z = z self.previous_position[node.id] = infered_position self.previous_orientation[node.id] = infered_orientation self.ctx.worlds()[world_name].scene().nodes()[ node.id] = node changes.nodes_to_update.append(node) else: changes.nodes_to_update.append(node) # for contact in p.getContactPoints(): # pass now = rospy.Time.now() for node1_id in self.simulated_node_ids: node1 = self.ctx.worlds()[world_name].scene().nodes()[node1_id] if node1.type != MESH: continue for node2_id in self.simulated_node_ids: node2 = self.ctx.worlds()[world_name].scene().nodes()[node2_id] if node1.id == node2.id: continue if node2.type != MESH: continue bb1 = self.aabb(node1) bb2 = self.aabb(node2) if node1.id not in self.isIn: self.isIn[node1.id] = {} if node1.id not in self.isOnTop: self.isOnTop[node1.id] = {} if node2.id not in self.isContaining: self.isContaining[node2.id] = {} if self.isin(bb1, bb2, node2.id in self.isIn[node1.id]): if node2.id not in self.isIn[node1.id]: sit = Situation() sit.id = str(uuid.uuid4()) sit.type = FACT sit.description = node1.name + " is in " + node2.name sit.properties.append(Property("subject", node1.id)) sit.properties.append(Property("object", node2.id)) sit.properties.append(Property("predicate", "isIn")) sit.confidence = IN_CONFIDENCE sit.start.data = now sit.end.data = rospy.Time(0) self.isIn[node1.id][node2.id] = sit self.isContaining[node2.id][node1.id] = sit changes.situations_to_update.append(sit) else: if node2.id in self.isIn[node1.id]: self.isIn[node1.id][node2.id].end.data = now self.isIn[node1.id][ node2. id].description = node1.name + " was in " + node2.name sit = self.isIn[node1.id][node2.id] changes.situations_to_update.append(sit) del self.isIn[node1.id][node2.id] del self.isContaining[node2.id][node1.id] if self.isontop(bb1, bb2, node2.id in self.isOnTop[node1.id]): if node2.id not in self.isOnTop[node1.id]: sit = Situation() sit.id = str(uuid.uuid4()) sit.type = FACT sit.description = node1.name + " is on " + node2.name sit.properties.append(Property("subject", node1.id)) sit.properties.append(Property("object", node2.id)) sit.properties.append(Property("predicate", "isOn")) sit.confidence = ONTOP_CONFIDENCE sit.start.data = now sit.end.data = rospy.Time(0) self.isOnTop[node1.id][node2.id] = sit changes.situations_to_update.append(sit) else: if node2.id in self.isOnTop[node1.id]: self.isOnTop[node1.id][ node2. id].description = node1.name + " was on " + node2.name self.isOnTop[node1.id][node2.id].end.data = now sit = self.isOnTop[node1.id][node2.id] changes.situations_to_update.append(sit) del self.isOnTop[node1.id][node2.id] end_reasoning_time = rospy.Time.now() if (1.0 / (end_reasoning_time - start_reasoning_time).to_sec() < self.reasoning_frequency * 0.5): rospy.logwarn( "[%s::filter] reasoning too slow ! %f", self.ctx.name(), 1.0 / (end_reasoning_time - start_reasoning_time).to_sec()) return changes
def onChanges(self, world_name, header, invalidations): """ """ changes = Changes() now = rospy.Time.now() self.currently_inside_area = {} for sit_id in invalidations.situation_ids_updated: changes.situations_to_update.append( self.ctx.worlds()[world_name].timeline().situations()[sit_id]) for sit_id in invalidations.situation_ids_deleted: changes.situations_to_delete.append(sit_id) for node_id in invalidations.node_ids_updated: changes.nodes_to_update.append( self.ctx.worlds()[world_name].scene().nodes()[node_id]) for mesh_id in invalidations.mesh_ids_updated: changes.meshes_to_update.append( self.ctx.worlds()[world_name].meshes()[mesh_id]) for mesh_id in invalidations.mesh_ids_deleted: changes.meshes_to_delete.append(mesh_id) for node_id in invalidations.node_ids_deleted: changes.nodes_to_delete.append(node_id) if node_id in self.is_inside_area_prob: del self.is_inside_area_prob[node_id] if node_id in self.previously_inside_area: del self.previously_inside_area[node_id] if node_id in self.currently_inside_area: if node_id + "isInsideArea" + self.previously_inside_area[ node_id] in self.predicates_map: fact = self.predicates_map[ node_id + "isInsideArea" + self.previously_inside_area[node_id]] print("stop : human-" + node_id + " is inside area " + self.previously_inside_area[node_id]) fact.end.data = now changes.situations_to_update.append(fact) #changes.situations_to_delete.append(fact_id) del self.predicates_map[ node_id + "isInsideArea" + self.previously_inside_area[node_id]] for node in self.ctx.worlds()[world_name].scene().nodes(): if self.ctx.worlds()[world_name].scene().nodes().get_node_property( node.id, "class") == "Human": if self.is_inside_area( world_name, self.ctx.worlds()[world_name].scene().nodes()[ node.id]) is True: self.currently_inside_area[node.id] = "robot_infodesk" if node.id in self.currently_inside_area and node.id not in self.previously_inside_area: area = self.currently_inside_area[node.id] print("start : human-" + node.id + " is inside area " + area) fact = Situation(description="human-" + node.id + " is inside area " + area, type=FACT, id=str(uuid.uuid4().hex)) subject_property = Property(name="subject", data=node.id) object_property = Property(name="object", data=area) predicate_property = Property(name="predicate", data="isInsideArea") fact.start.data = now fact.end.data = rospy.Time(0) fact.properties.append(subject_property) fact.properties.append(object_property) fact.properties.append(predicate_property) self.predicates_map[node.id + "isInsideArea" + area] = fact changes.situations_to_update.append(fact) if node.id in self.previously_inside_area and node.id not in self.currently_inside_area: if node.id + "isInsideArea" + self.previously_inside_area[ node.id] in self.predicates_map: fact = self.predicates_map[ node.id + "isInsideArea" + self.previously_inside_area[node.id]] #fact = self.ctx.worlds()[self.output_world].timeline().situations()[fact_id] #if fact is not None: print("stop : human-" + node.id + " is inside area " + self.previously_inside_area[node.id]) fact.end.data = now changes.situations_to_update.append(fact) del self.predicates_map[ node.id + "isInsideArea" + self.previously_inside_area[node.id]] self.previously_inside_area = self.currently_inside_area self.ctx.worlds()[self.output_world].update(changes)
def monitor(self, world_name, header, invalidations): """ """ changes = Changes() for node_id in invalidations.node_ids_updated: self.updateBulletNodes(world_name, node_id) changes.nodes_to_update.append(self.ctx.worlds()[world_name].scene().nodes()[node_id]) for node_id in invalidations.node_ids_deleted: for situation in self.ctx.worlds()[world_name].timeline().situations(): #if self.ctx.worlds()[world_name].timeline().situations().get_situation_property(situation.id, "predicate") == "isVisibleBy": object_id = self.ctx.worlds()[world_name].timeline().situations().get_situation_property(situation.id, "object") subject_id = self.ctx.worlds()[world_name].timeline().situations().get_situation_property(situation.id, "subject") if node_id == object_id or node_id == subject_id: changes.situations_to_delete.append(situation.id) print "delete : %s" % situation.description changes.nodes_to_delete.append(node_id) for sit_id in invalidations.situation_ids_updated: changes.situations_to_update.append(self.ctx.worlds()[world_name].timeline().situations()[sit_id]) for sit_id in invalidations.situation_ids_deleted: changes.situations_to_delete.append(sit_id) for mesh_id in invalidations.mesh_ids_updated: changes.meshes_to_update.append(self.ctx.worlds()[world_name].meshes()[mesh_id]) for mesh_id in invalidations.mesh_ids_deleted: changes.meshes_to_delete.append(mesh_id) #self.ctx.worlds()[world_name].timeline().update(changes.situations_to_update) #self.ctx.worlds()[world_name].timeline().remove(changes.situations_to_delete) for node in self.ctx.worlds()[world_name].scene().nodes(): if node.type == CAMERA and node.id == "0": if (rospy.Time.now() - node.last_update.data).to_sec() > 1.0: for situation in self.ctx.worlds()[world_name].timeline().situations(): if self.ctx.worlds()[world_name].timeline().situations().get_situation_property(situation.id, "predicate") == "isVisibleBy": camera_id = self.ctx.worlds()[world_name].timeline().situations().get_situation_property(situation.id, "object") id_seen = self.ctx.worlds()[world_name].timeline().situations().get_situation_property(situation.id, "subject") if node.id == camera_id or node.id == id_seen: changes.situations_to_delete.append(situation.id) print "delete : %s" % situation.description if id_seen+camera_id in self.visibilities_situations: del self.visibilities_situations[id_seen+camera_id] else: visibilities_ = self.computeVisibilities(world_name, node.id) for object_seen in self.ctx.worlds()[world_name].scene().nodes(): if object_seen.id in visibilities_: if object_seen.id+node.id not in self.visibilities_situations: situation = Situation() situation.id = str(uuid.uuid4()) situation.type = FACT situation.description = object_seen.name + " is visible by " + node.name predicate = Property() predicate.name = "predicate" predicate.data = "isVisibleBy" situation.properties.append(predicate) subject = Property() subject.name = "subject" subject.data = object_seen.id situation.properties.append(subject) object = Property() object.name = "object" object.data = node.id situation.properties.append(object) situation.confidence = visibilities_[object_seen.id] situation.start.data = header.stamp situation.end.data = rospy.Time(0) print "start : %s" % situation.description changes.situations_to_update.append(situation) self.visibilities_situations[object_seen.id+node.id] = situation else: if object_seen.id+node.id in self.visibilities_situations: changes.situations_to_delete.append(self.visibilities_situations[object_seen.id+node.id].id) del self.visibilities_situations[object_seen.id+node.id] self.ctx.worlds()[world_name].timeline().update(changes.situations_to_update) self.ctx.worlds()[world_name].timeline().remove(changes.situations_to_delete) return changes
from pyuwds.uwds import PROVIDER from uwds_msgs.msg import Node, Changes, Property from pyuwds.types.nodes import CAMERA, ENTITY from std_msgs.msg import Header from geometry_msgs.msg import PoseWithCovariance, Point, Pose, Quaternion from tf.transformations import * def transformation_matrix(t, q): translation_mat = translation_matrix(t) rotation_mat = quaternion_matrix(q) return numpy.dot(translation_mat, rotation_mat) CAMERA_PROPERTIES = [] CAMERA_PROPERTIES.append(Property(name="hfov", data="60.0")) CAMERA_PROPERTIES.append(Property(name="aspect", data="1.3333")) CAMERA_PROPERTIES.append(Property(name="clipnear", data="0.3")) CAMERA_PROPERTIES.append(Property(name="clipfar", data="100.0")) CAMERA_PROPERTIES.append(Property(name="class", data="Eyes")) class UwdsNodeBridge(UwdsClient): def __init__(self, ros_topic, uwds_world, uwds_client_name, ros_msg_type): super(UwdsNodeBridge, self).__init__(uwds_client_name, PROVIDER) self.uwds_nodes = {} self.ros_to_uwds_id = {} self.output_world = uwds_world # override 'output_world' attribute self.ros_topic_sub = rospy.Subscriber(ros_topic, ros_msg_type, self.update,