def monitor(self, world_name, header, invalidations):
"""
"""
changes = Changes()
for node_id in invalidations.node_ids_updated:
self.updateBulletNodes(world_name, node_id)
evaluated = []
not_evaluated = []
for node in self.ctx.worlds()[world_name].scene().nodes():
#if node_id in invalidations.node_ids_updated:
if node.type == CAMERA:
if node.id in invalidations.node_ids_updated:
visibilities = self.computeVisibilities(
world_name, node.id)
situations = self.updateSituations(world_name, header,
node.id, visibilities)
for situation in situations:
changes.situations_to_update.append(situation)
evaluated.append(node.id)
for node in self.ctx.worlds()[world_name].scene().nodes():
if node.type == CAMERA:
if (rospy.Time() - node.last_observation.data).to_sec() > 1.0:
situations = self.updateSituations(world_name, header,
node.id, {})
for situation in situations:
changes.situations_to_update.append(situation)
return changes
def update(self, ros_msg):
current_nodes = self.ros_to_uwds_id.keys()
print(current_nodes)
nodes_to_update_ids = self.get_nodes_to_update(ros_msg, current_nodes)
nodes_to_delete_ids = []
if not nodes_to_update_ids:
if self.uwds_nodes:
nodes_to_delete_ids = self.ros_to_uwds_id.keys()
else:
return
changes = Changes()
header = Header(stamp=ros_msg.header.stamp,
frame_id=ros_msg.header.frame_id)
# delete old nodes
for i in list(set(current_nodes) - set(nodes_to_update_ids)):
nodes_to_delete_ids.append(i)
for node_id in nodes_to_update_ids:
changes.nodes_to_update.append(self.get_node(node_id))
for node_id in set(nodes_to_delete_ids):
changes.nodes_to_delete.append(self.ros_to_uwds_id[node_id])
self.remove_node(node_id)
print('to delete: ' + str(changes.nodes_to_delete))
self.submit_changes(changes, header)
def filter(self, world_name, header, invalidations):
"""
"""
changes = {}
beliefs_map = {}
for situation in self.worlds[world_name].timeline.situations.values():
if situation.end.data == rospy.Time(0):
if self.ctx.worlds()[world_name].timeline().by_property(situation.id, "predicate") == "isVisible":
subject_id = self.ctx.worlds()[world_name].timeline().by_property(situation.id, "subject")
object_id = self.ctx.worlds()[world_name].timeline().by_property(situation.id, "object")
if subject_id not in beliefs_map:
beliefs_map[subject_id] = {}
beliefs_map[subject_id][object_id] = self.ctx.worlds()[world_name].timeline().situations()[situation.id].confidence
for subject_id in beliefs_map.keys():
if subject_id not in changes:
changes[subject_id] = Changes()
changes[subject_id].nodes_to_update.append(self.worlds[world_name].scene.nodes[subject_id])
if len(beliefs_map[subject_id]) > 0:
print "camera <%s> see :" % self.worlds[world_name].scene.nodes[subject_id].name
for object_id in beliefs_map[subject_id].keys():
print " - see object <%s> with %5f confidence" % (self.worlds[world_name].scene.nodes[object_id].name, beliefs_map[subject_id][object_id])
if object_id in invalidations.node_ids_updated:
changes[subject_id].nodes_to_update.append(self.worlds[world_name].scene.nodes[object_id])
return changes
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 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