def __input_recurse(self, input_dict, root_name, root_id: sml.Identifier):
"""
Recursively update WMEs that have a sub-tree structure in the input link.
We scan through the `input_dict`, which represents the input value getters (or further
sub-trees) of the sub-tree root, either adding terminal WMEs as usual or further recursing.
:param input_dict: A dict mapping attributes to getter functions
:param root_name: The attribute which is the root of this sub-tree
:param root_id: The sml identifier of the root of the sub-tree
:return: None
"""
assert isinstance(input_dict, dict), "Should only recurse on dicts!"
for input_name in input_dict.keys():
new_val = input_dict[input_name]
wme = self.WMEs.get(root_name + "." + input_name)
if not callable(new_val):
if wme is None:
wme = root_id.CreateIdWME(input_name)
self.WMEs[root_name + "." + input_name] = wme
self.__input_recurse(new_val, root_name + "." + input_name,
wme)
continue
new_val = new_val()
if wme is None:
new_wme = psl.SoarWME(att=input_name, val=new_val)
self.WMEs[root_name + "." + input_name] = new_wme
new_wme.add_to_wm(root_id)
else:
wme.set_value(new_val)
wme.update_wm()
def on_input_phase(self, input_link: sml.Identifier):
"""
Prior to each input phase, update the changed values of Soar's input link
Scan through the designated Cozmo inputs and update the corresponding WMEs in Soar via
instances of the `SoarWME` class. For each input, we first get the value, then check
whether there exists a WME with that attribute name. If not, we add one to the Soar agent
and the WME dict of the `CozmoSoar` object. Otherwise, we retrieve the `SoarWME` object
associated with the input and update its value, then call its `update_wm` method. For
terminal WMEs, this is simple. However, for sub-trees we need to recursively update
the WMEs.
We have to handle temporary inputs e.g., faces or objects, differently, because they
need to be removed when they are no longer detected.
:param input_link: The Soar WME corresponding to the input link of the agent.
:return: None
"""
# TODO (Tony): camera localizer (static inputs include xyzr)
# update transformation
cozmo_position = [
self.r.pose.position.x, self.r.pose.position.y,
self.r.pose.position.z, 0, 0, self.r.pose.rotation.angle_z.degrees
]
self.localizer.recalculate_transform(cozmo_position)
# First, we handle inputs which will always be present
for input_name in self.static_inputs.keys():
new_val = self.static_inputs[input_name]
wme = self.WMEs.get(input_name)
if not callable(new_val):
if wme is None:
wme = input_link.CreateIdWME(input_name)
self.WMEs[input_name] = wme
self.__input_recurse(new_val, input_name, wme)
continue
new_val = new_val()
if wme is None:
new_wme = psl.SoarWME(att=input_name, val=new_val)
self.WMEs[input_name] = new_wme
new_wme.add_to_wm(input_link)
else:
wme.set_value(new_val)
wme.update_wm()
# Then, check through the visible faces and objects to see if they need to be added,
# updated, or removed
#######################
# FACE INPUT HANDLING #
#######################
vis_faces = set(list(self.w.visible_faces))
for face in vis_faces:
face_designation = "face{}".format(face.face_id)
if face_designation in self.faces:
face_wme = self.WMEs[face_designation]
else:
self.faces[face_designation] = face
face_wme = input_link.CreateIdWME("face")
self.WMEs[face_designation] = face_wme
self.__build_face_wme_subtree(face, face_designation, face_wme)
faces_missing = set()
for face_dsg in self.faces.keys():
if self.faces[face_dsg] not in vis_faces:
faces_missing.add(face_dsg)
for face_dsg in faces_missing:
del self.faces[face_dsg]
remove_list = [(n, self.WMEs[n]) for n in self.WMEs.keys()
if n.startswith(face_dsg)]
remove_list = sorted(remove_list, key=lambda s: 1 / len(s[0]))
for wme_name, wme in remove_list:
del self.WMEs[wme_name]
if isinstance(wme, psl.SoarWME):
wme.remove_from_wm()
elif isinstance(wme, sml.Identifier):
wme.DestroyWME()
else:
raise Exception("WME wasn't of proper type")
#########################
# OBJECT INPUT HANDLING #
#########################
vis_objs = set(list(self.w.visible_objects))
for obj in vis_objs:
obj_designation = "obj{}".format(obj.object_id)
if obj_designation in self.objects:
obj_wme = self.WMEs[obj_designation]
else:
self.objects[obj_designation] = obj
obj_wme = input_link.CreateIdWME("object")
self.WMEs[obj_designation] = obj_wme
self.__build_obj_wme_subtree(obj, obj_designation, obj_wme)
objs_missing = set()
for obj_dsg in self.objects.keys():
if self.objects[obj_dsg] not in vis_objs:
objs_missing.add(obj_dsg)
for obj_dsg in objs_missing:
del self.objects[obj_dsg]
remove_list = [(n, self.WMEs[n]) for n in self.WMEs.keys()
if n.startswith(obj_dsg)]
remove_list = sorted(remove_list, key=lambda s: 1 / len(s[0]))
for wme_name, wme in remove_list:
del self.WMEs[wme_name]
if isinstance(wme, psl.SoarWME):
wme.remove_from_wm()
elif isinstance(wme, sml.Identifier):
wme.DestroyWME()
else:
raise Exception("WME wasn't of proper type")
# Finally, we want to check all our on-going actions and handle them appropriately:
# Actions are by default on the output link and have a `status` attribute already,
# we just need to update that status if needed
for action, status_wme, root_id in self.actions:
if action is None and status_wme is None:
self.actions.remove((action, status_wme, root_id))
continue
if action.is_completed:
state = "complete" if action.has_succeeded else "failed"
failure_reason = action.failure_reason
status_wme.set_value(state)
if failure_reason != (None, None):
code_wme = psl.SoarWME("failure-code", failure_reason[0])
reason_wme = psl.SoarWME("failure-reason",
failure_reason[1])
code_wme.add_to_wm(root_id)
code_wme.update_wm()
reason_wme.add_to_wm(root_id)
reason_wme.update_wm()
status_wme.update_wm()
self.actions.remove((action, status_wme, root_id))
