frame_registrar.py

Go to the documentation of this file.

#!/usr/bin/env python
# frame_registrar.py

import roslib; roslib.load_manifest('frame_registrar')
import rospy
import sys
import os
import yaml
import traceback
from roboframenet_msgs.msg import *
from roboframenet_msgs.srv import *



###############################################################################
### Globals

semantic_frames = {}   # Index: frame.name ; Element: _SemanticFrame




###############################################################################
### Internal Classes

class _SemanticFrame(object):
    def __init__(self, name):
        self.name = name # string
        self.description = "" # string
        self.frame_elements = [] # listof(_FrameElement)
        self.children = [] # listof(listof(_RelatedFrame))
        # ("list of frame sequences") ^^
        self.callbacks = [] # listof(string)


    @staticmethod
    def import_yaml(yaml):
        try:
            sf = _SemanticFrame(yaml['name'])
            sf.description = yaml['description']
            if 'frame_elements' in yaml:
                for fe in yaml['frame_elements']:
                    f = _FrameElement(fe['name'], fe['is_core'])
                    if not f.is_core and 'default' in fe:
                        f.default = str(fe['default'])
                    sf.frame_elements.append(f)
            if 'children' in yaml:
                for child in yaml['children']:
                    sf.children.append(map(lambda subchild:
                                               _RelatedFrame.import_yaml(subchild),
                                           listify(child)))
            if 'parents' in yaml:
                for parent in yaml['parents']:
                    parent_relframe = _RelatedFrame.import_yaml(parent)
                    parent_semframe = get_semantic_frame(parent_relframe.name)
                    # Since the relations are the same between parent and child,
                    #   just change the parent's name to the child's,
                    #   and you have the relation from the parent's perspective.
                    parent_relframe.name = sf.name
                    parent_semframe.children.append(parent_relframe)
            # sf.callbacks: not allowed.  (ActionServer must register
            # itself upon startup.)
        except:
            rospy.logerr("Error converting yaml '" +
                         (yaml['name'] if 'name' in yaml else "???") +
                         "' to _SemanticFrame: " + str((sys.exc_info())))
            traceback.print_tb(sys.exc_info()[2])
        else:
            return sf

    def get_frame_element(self, name):
        for fe in self.frame_elements:
            if fe.name == name:
                return fe
        rospy.logerr("Frame " + str(self) + " does not have frame element '" +
                     name + "'!  Returning False.")
        return False

    def describe_frame(self):
        return self.description

    def has_callbacks(self):
        return self.callbacks # [] is False; [anything but blank] is True

    # filled_sem_frame is _FilledSemanticFrame
    # returns a FilledSemanticFrame
    def to_ros_EmptySemanticFrame(self):
        esf = EmptySemanticFrame()
        esf.name = self.name
        for fe in self.frame_elements:
            # See if the filled_lexical_unit has an argument for each fe.  Otherwise,
            #   the argument will be "".
            efe = EmptyFrameElement()
            efe.name = fe.name
            efe.default = fe.default
            efe.is_core = fe.is_core
            esf.frame_elements.append(efe)
        return esf

    def __str__(self):
        return self.to_str(self.name)

    @staticmethod
    def to_str(name):
        return "<_SemanticFrame '" + name + "'>"



class _FilledSemanticFrame(_SemanticFrame):
    def __init__(self, semframe):
        self.name           = semframe.name
        self.description    = semframe.description
        self.frame_elements = map(lambda fe: _FilledFrameElement(fe.name, fe.is_core),
                                  semframe.frame_elements)
        self.children       = semframe.children
        #self.parents
        self.callbacks      = semframe.callbacks

    def fill_frame_element(self, name, arg):
        fe = self.get_frame_element(name)
        if fe:
            fe.argument = arg
            return True
        else:
            return False

    def describe_frame():
        return(self.description + '\n' +
               '\n'.join([(element.name + " == " + element.argument)
                          for element in frame_elements]) +
               '\n')

    # filled_sem_frame is _FilledSemanticFrame
    # returns a FilledSemanticFrame
    def to_ros_FilledSemanticFrame(self):
        fsf = FilledSemanticFrame()
        fsf.name = self.name
        fsf.action_server_topics = self.callbacks
        for fe in self.frame_elements:
            # See if the filled_lexical_unit has an argument for each fe.  Otherwise,
            #   the argument will be "".
            ffe = FilledFrameElement()
            ffe.name = fe.name
            ffe.argument = fe.argument
            fsf.frame_elements.append(ffe)
        return fsf


class _FrameElement:
    def __init__(self, name, is_core):
        self.name = name # string
        self.is_core = is_core # boolean
        self.default = "" # string


class _FilledFrameElement:
    def __init__(self, name, is_core):
        self.name = name # string
        self.is_core = is_core
        self.argument = "" # string


class _RelatedFrame(object): # enable class methods (see http://www.rexx.com/~dkuhlman/python_101/python_101.html 2.5.2.4)
    def __init__(self, name):
        self.name = name # string
        self.frame_element_relations = [] # listof(_FrameElementRelation)

    @staticmethod
    def import_yaml(yaml):
        rf = _RelatedFrame(yaml['name'])
        if 'frame_element_relations' in yaml:
            for relation in yaml['frame_element_relations']:
                # Get parent name/value
                if 'parent_name' in relation:
                    parent = relation['parent_name']
                    parent_is_value = False
                elif 'parent_value' in relation:
                    parent = relation['parent_value']
                    parent_is_value = True
                else:
                    rospy.logerr("_RelatedFrame.import_yaml: No parent name or value!")
                # Get child name/value
                if 'child_name' in relation:
                    child = relation['child_name']
                    child_is_value = False
                elif 'child_value' in relation:
                    child = relation['child_value']
                    child_is_value = True
                else:
                    rospy.logerr("_RelatedFrame.import_yaml: No child name or value!")
                rf.frame_element_relations.append(_FrameElementRelation(parent,
                                                                        parent_is_value,
                                                                        child,
                                                                        child_is_value))
        return rf



class _FrameElementRelation:
    # A relationship between 2 frame elements
    def __init__(self, parent, parent_is_value, child, child_is_value):
        self.parent = parent # string
        self.parent_is_value = parent_is_value # boolean.  Parent is either a value or a name.
        if child_is_value:
            self.child = map(lambda c: c, listify(child)) # listof(string) if child_is_value
        else:
            self.child = child # string if not child_is_value
        self.child_is_value = child_is_value # boolean.  Child is either a value or a name.






###############################################################################
### Helpers

def listify(x):
    return x if isinstance(x, list) else [x]

# Retrives frame from the database.
# Returns a _SemanticFrame, or False if not found.
def get_semantic_frame(frame_name):
    global semantic_frames
    if frame_name in semantic_frames:
        return semantic_frames[frame_name]
    else:
        rospy.logwarn("get_semantic_frame: " +
                      _SemanticFrame.to_str(frame_name) +
                      " not found.  (This may be normal.  Or not.)")
        return False


# Stores semantic frame in the database.
# Returns True on success and False on failure.
def store_semantic_frame(frame):
    global semantic_frames
    if not isinstance(frame, _SemanticFrame):
        rospy.logerr("store_semantic_frame: Given object (" + str(frame) +
                     ") is not a _SemanticFrame object!  Ignoring request.")
        return False
    else:
        if frame.name in semantic_frames:
            rospy.logwarn("store_semantic_frame: Frame with name '" +
                          str(frame.name) + "' previously stored in" +
                          " semantic_frames.  Overwriting!")
        semantic_frames[frame.name] = frame
        rospy.loginfo("Added new semantic frame " + str(frame) + ".")
        return True


# Loads semantic frame and stores it in the database.
# Returns True on success and False on failure.
def load_semantic_frame_yaml(filename):
    try:
        frame_yaml = yaml.load(open(filename, 'r'))
    except:
        rospy.logerr("Error loading file '%s': %s" % (str(filename), str(sys.exc_info())))
        return False
    else:
        return store_semantic_frame(_SemanticFrame.import_yaml(frame_yaml))



# filled_lexical_unit is FilledLexicalUnit
# returns listof(listof(_FilledSemanticFrame)) (list of sequences of _FilledSemanticFrames to accomplish task)
def fill_semantic_frame_sequences_from_filled_lexical_unit(filled_lexical_unit):
    FilledSemanticFrame_sequences = []
    filled_sem_frame = fill_semantic_frame_from_filled_lexical_unit(filled_lexical_unit)
    if filled_sem_frame:
        fsf_sequences = fill_semantic_frame_descendants(filled_sem_frame)
        FilledSemanticFrame_sequences.extend(fsf_sequences)
    else:
        rospy.logwarn("cb_GetFrames: No filled_sem_frame for filled_lexical_unit:\n" + str(filled_lexical_unit))
    return FilledSemanticFrame_sequences

# filled_lexical_unit is FilledLexicalUnit
# returns a _FilledSemanticFrame
def fill_semantic_frame_from_filled_lexical_unit(filled_lexical_unit):
    semantic_frame = get_semantic_frame(filled_lexical_unit.frame_name)
    if semantic_frame:
        filled_semantic_frame = _FilledSemanticFrame(semantic_frame)
        # Fill
        for frame_element in filled_lexical_unit.frame_elements:
            # See if the filled_lexical_unit has an argument for each frame
            #   element.  Otherwise, the argument will be "".
            if not filled_semantic_frame.fill_frame_element(frame_element.name,
                                                            frame_element.argument):
                rospy.logwarn("fill_semantic_frame_from_filled_lexical_unit:" +
                              " Frame element parameter '" + frame_element.name +
                              "' could not be filled (argument '" +
                              frame_element.argument + "') in frame " +
                              str(filled_semantic_frame) + ".")
        # Check all core elements have been filled
        for fe in filled_semantic_frame.frame_elements:
            if fe.is_core and not fe.argument:
                rospy.logwarn("fill_semantic_frame_from_filled_lexical_unit:" +
                              " Core element '" + fe.name + "' has no" +
                              " argument! Offending filled lexical unit:\n" +
                              str(filled_lexical_unit))
                return False
        # Success!
        return filled_semantic_frame
    else:
        rospy.logwarn("fill_semantic_frame_from_filled_lexical_unit: Requested frame " +
                      _SemanticFrame.to_str(filled_lexical_unit.frame_name) +
                      " does not exist!")
        return False

# filled_sem_frame is _FilledSemanticFrame
# returns listof(listof(_FilledSemanticFrame)), a list of possible sequences which can fulfill the
#   given filled_sem_frame, including the sequence of just the filled_sem_frame itself.
def fill_semantic_frame_descendants(filled_sem_frame):
    filled = [[filled_sem_frame]]
    for rf_child_list in filled_sem_frame.children:
        filled_child_frames = []
        for rf_child in rf_child_list:
            filled_child_frame = fill_semantic_frame_from_parent(filled_sem_frame, rf_child)
            if filled_child_frame:
                filled_child_frames.append(filled_child_frame)
        # See if all elements of this sequence were successfully filled in.
        all_filled = (len(filled_child_frames) == len(rf_child_list))
        # If so, find all combinations that describe each child (including the
        #   child itself.
        if all_filled:
            child_combinations = [] # listof( listof(listof(_FilledSemanticFrame)) )
            for filled_child_frame in filled_child_frames:
                child_combinations.append(fill_semantic_frame_descendants(filled_child_frame))
            all_combinations = map(lambda combo: reduce(lambda a,b: list_join(a,b), combo),
                                   all_combinations_of_lists(child_combinations))
            filled.extend(all_combinations)
    return filled

# Returns a new list which is the joining of the two input lists
def list_join(a,b):
    c = []
    c.extend(a)
    c.extend(b)
    return c

# [[1,2],[3,4]] -> [[1,3],[1,4],[2,3],[2,4]]
def all_combinations_of_lists(lists):
    def all_combinations_of_lists_recursion(lists, index):
        if len(lists) == index: return [[]]
        combinations = []
        recursed_combinations = all_combinations_of_lists_recursion(lists, index+1)
        for item in lists[index]:
            for recursed_combination in recursed_combinations:
                r = [item]
                r.extend(recursed_combination)
                combinations.append(r)
        return combinations
    return all_combinations_of_lists_recursion(lists,0)





# filled_parent is a _FilledSemanticFrame
# rf_child is a _RelatedFrame
# returns a _FilledSemanticFrame or False
def fill_semantic_frame_from_parent(filled_parent, rf_child):
    # Check that child frame exists
    child = get_semantic_frame(rf_child.name)
    if not child: return False
    filled_child = _FilledSemanticFrame(child)
    # Fill in frame elements of filled_child
    for reln in rf_child.frame_element_relations:
        # The parent is either an argument from the parent frame or a new argument.
        parent_value = (reln.parent
                        if reln.parent_is_value
                        else filled_parent.get_frame_element(reln.parent).argument)
        # Check that requirements are met if the child is a value
        if reln.child_is_value:
            if not (parent_value in reln.child):
                rospy.loginfo("Could not fill frame " + str(child) +
                              " from frame " + str(filled_parent) +
                              ": Parent " + ("" if reln.parent_is_value else reln.parent + " with ") +
                              "value " + parent_value +
                              " is not in list of required values " + str(reln.child) +
                              ".  (This is normal.)")
                return False
        # Otherwise, try to fill in the child with its name
        else: # not reln.child_is_value (ie, child is a blank)
            # Check coreness if there's no value
            fe = filled_child.get_frame_element(reln.child)
            if (not parent_value) and fe and fe.is_core:
                rospy.loginfo("Could not fill frame " + str(child) +
                              " from frame " + str(filled_parent) +
                              ": Cannot fill core element '" + reln.child +
                              "'.  (This is normal.)")
                return False
            # Otherwise, try filling it in.
            success = filled_child.fill_frame_element(reln.child, parent_value)
            if not success:
                print reln.child
                rospy.logwarn("Aborting filling frame " + str(child) +
                              " from frame " + str(filled_parent) +
                              ": Cannot fill element '" + reln.child +
                              "' from element '" + reln.parent +
                              "'.  (This should not happen.)")
                return False
    rospy.loginfo("Filled frame " + str(child) +
                  " from frame " + str(filled_parent) + ".")
    return filled_child




###############################################################################
### ROS Callbacks

def cb_AddFrame(req):
    response = AddFrameResponse()
    response.success = load_semantic_frame_yaml(req.absolute_file_path)
    return response


def cb_FilledLexicalUnits(pub, msg):
    def frame_list_has_callbacks(frame_list):
        if len(frame_list) == 0: return True
        return (len(frame_list[0].action_server_topics) > 0 and
                frame_list_has_callbacks(frame_list[1:]))
    def frame_sequences_str(frame_sequences):
        return '\n'.join(map(lambda sequence:
                                 ('[' +
                                  ', '.join(map(lambda frame: frame.name,
                                                sequence.frames)) +
                                  ']'),
                      frame_sequences))
    filled_lexical_units = msg.lexical_units
    rospy.loginfo("frame_registrar: Received " +
                  str(len(filled_lexical_units)) +
                  " filled lexical units:\n" +
                  '\n'.join(map(lambda flu:
                                    flu.verb + "\t->\t" + flu.frame_name,
                                filled_lexical_units)) +
                  '\n')
    frame_sequences = FilledSemanticFrameListList() # A list of sequences of FilledSemanticFrame
    frame_sequences.frame_lists = []
    for flu in filled_lexical_units:
        flu_frame_sequences = \
            fill_semantic_frame_sequences_from_filled_lexical_unit(flu)
        # Remove sequences that don't have callbacks.
        for flu_frame_sequence in flu_frame_sequences:
            frame_sequence = FilledSemanticFrameList()
            frame_sequence.frames = map(lambda frame:
                                            frame.to_ros_FilledSemanticFrame(),
                                        flu_frame_sequence)
            frame_sequences.frame_lists.append(frame_sequence)
    #todo remove duplicates
    # Filter sequences which don't have callbacks.
    rospy.loginfo("frame_registrar: Converted filled lexical units to " +
                  str(len(frame_sequences.frame_lists)) +
                  " sequence(s) of semantic frames:\n" +
                  frame_sequences_str(frame_sequences.frame_lists) + '\n')
    frame_sequences.frame_lists = \
        filter(lambda sequence: frame_list_has_callbacks(sequence.frames),
               frame_sequences.frame_lists)
    rospy.loginfo("frame_registrar: There are " +
                  str(len(frame_sequences.frame_lists)) +
                  " sequence(s) which have callbacks:\n" +
                  frame_sequences_str(frame_sequences.frame_lists) + '\n')
    # Publish if any sequences remain
    if frame_sequences.frame_lists:
        pub.publish(frame_sequences)
    else:
        rospy.logwarn("frame_registrar: No publishable frame sequences!  " +
                      "(Aborting publishing.)")


def cb_GetEmptyFrame(req):
    resp = GetEmptyFrameResponse()
    frame = req.frame_name
    resp.frame = get_semantic_frame(req.frame_name).to_ros_EmptySemanticFrame()
    return resp


def cb_RegisterWithFrame(req):
    response = RegisterWithFrameResponse()
    frame = get_semantic_frame(req.frame_name)
    if frame:
        frame.callbacks.append(req.callback_topic)
        rospy.loginfo("cb_RegisterWithFrame: Registered callback with topic '" +
                      req.callback_topic + "' to frame " +
                      _SemanticFrame.to_str(req.frame_name))
        response.success = True
    else:
        rospy.logwarn("cb_RegisterWithFrame: Frame " + _SemanticFrame.to_str(req.frame_name) +
                      " is not yet available.  (Callback has topic '" + req.callback_topic +
                      "'.)  Ignoring request.")
        response.success = False
    return response




###############################################################################
### Main

def main():
    rospy.init_node('frame_registrar')
    # Load semantic frames and lexical units
    # (Make sure to use os.path.abspath when requesting through a message that
    #   a semantic frame or lexical unit be loaded.)
    frame_relpath = '../frames' # relative to this binary
    frame_filenames = filter(lambda fn: fn[-5:] == '.yaml', os.listdir(frame_relpath))
    rospy.loginfo("frame_registrar: Loading semantic frames...")
    for frame_filename in frame_filenames:
        load_semantic_frame_yaml(os.path.join(frame_relpath, frame_filename))
    rospy.loginfo("frame_registrar: Done loading semantic frames.")
    # Register subscriptions and services
    pub = rospy.Publisher('filled_semantic_frames', FilledSemanticFrameListList)
    rospy.Subscriber('filled_lexical_units', FilledLexicalUnitList,  lambda msg: cb_FilledLexicalUnits(pub, msg))
    rospy.Service('add_frame',           AddFrame,          cb_AddFrame)
    rospy.Service('get_empty_frame',     GetEmptyFrame,     cb_GetEmptyFrame)
    rospy.Service('register_with_frame', RegisterWithFrame, cb_RegisterWithFrame)
    rospy.loginfo("frame_registrar: Services up.")
    rospy.spin()

if __name__ == '__main__':
    main()