Esempio n. 1
0
 def load_automaton(self, aut_path, rooms, sensors, actuators, custom_props, room_map,
                    ltl_generator, is_follow, test_mode=False):
     """Load an automaton from a given path."""
     print "Loading automaton from", aut_path
     self.rooms = rooms
     self.sensors = sensors
     self.actuators = actuators
     self.custom_props = custom_props
     self.room_map = room_map
     
     # Initialize all sensors to zero if needed and load the automaton
     if not self.sensor_handler:
         self.sensor_handler = SensorHandler(dict([(sensor, 0) for sensor in sensors]))        
     if not self.motion_handler:
         self.motion_handler = MotionHandler(self, test_mode)
     self.aut = Automaton(self.rooms, self.sensor_handler, ActuatorHandler(self), 
                          self.motion_handler)
     self.aut.loadFile(aut_path, sensors, actuators, custom_props, ltl_generator, is_follow)
Esempio n. 2
0
 def load_automaton(self, aut_path, rooms, sensors, actuators, custom_props, room_map,
                    ltl_generator, is_follow, test_mode=False):
     """Load an automaton from a given path."""
     print "Loading automaton from", aut_path
     self.rooms = rooms
     self.sensors = sensors
     self.actuators = actuators
     self.custom_props = custom_props
     self.room_map = room_map
     
     # Initialize all sensors to zero if needed and load the automaton
     if not self.sensor_handler:
         self.sensor_handler = SensorHandler(dict([(sensor, 0) for sensor in sensors]))        
     if not self.motion_handler:
         self.motion_handler = MotionHandler(self, test_mode)
     self.aut = Automaton(self.rooms, self.sensor_handler, ActuatorHandler(self), 
                          self.motion_handler)
     self.aut.loadFile(aut_path, sensors, actuators, custom_props, ltl_generator, is_follow)
Esempio n. 3
0
class StateManager:
    """Provide semantics/LTL interface and automaton management."""

    def __init__(self, nlmaster=None):
        # The automaton and publisher are filled in later so the state manager
        # can start up early
        self.aut = None
        self.pub = None
        self.basedir = "."
        
        self.rooms = None
        self.sensors = None
        self.actuators = None
        self.custom_props = None
        self.sensor_handler = None
        self.motion_handler = None
        self.location = None
        self.room_adjacency = None
        self.room_map = None
        self.last_orders = None
        self.last_other_commands = []
        self.waiting = False
        
        # Thread safety
        self.resynthesizing = False
        self.motion_lock = Lock()
        
        # Connection to a knowledge object, used to check on tells. Will be set by
        # others if neded
        self.world_knowledge = None
        # Similarly, the nlmaster
        self.nlmaster = nlmaster
        
    def load_test_automaton(self, aut_path, movement_test_mode):
        """Load the testing automaton with preset values."""
        rooms = ["r_" + room for room in ("hall", "classroom", "outer_lab")]
        sensors = ["bomb", "user1", "hostage", "search_done"]
        actuators = ["get_defuser", "search"]
        custom_props = ["work", "done", "s_classroom", "s_outer_lab", "have_defuser"]
        self.load_automaton(aut_path, rooms, sensors, actuators, custom_props, None, False,
                            movement_test_mode)

    def load_automaton(self, aut_path, rooms, sensors, actuators, custom_props, room_map,
                       ltl_generator, is_follow, test_mode=False):
        """Load an automaton from a given path."""
        print "Loading automaton from", aut_path
        self.rooms = rooms
        self.sensors = sensors
        self.actuators = actuators
        self.custom_props = custom_props
        self.room_map = room_map
        
        # Initialize all sensors to zero if needed and load the automaton
        if not self.sensor_handler:
            self.sensor_handler = SensorHandler(dict([(sensor, 0) for sensor in sensors]))        
        if not self.motion_handler:
            self.motion_handler = MotionHandler(self, test_mode)
        self.aut = Automaton(self.rooms, self.sensor_handler, ActuatorHandler(self), 
                             self.motion_handler)
        self.aut.loadFile(aut_path, sensors, actuators, custom_props, ltl_generator, is_follow)

    def stop(self):
        """Stop the currently executing automaton and unload it."""
        self.aut.stop()
        self.aut = None

    def set_publisher(self, pub):
        """Set the ROS publisher."""
        self.pub = pub

    def set_basedir(self, path):
        """Set the base directory to call LTLGeneration scripts from."""
        self.basedir = path
    
    def process_orders(self, orders, init_props=None, test_mode=False, world_map=None, stop=True):
        """Process the given orders."""
        # Stop if we need to
        if stop and self.aut:
            self.stop()

        # Put in empty init_props
        if not init_props:
            init_props = ()
                    
        print "Processing orders:", str(orders)
        
        # Update rooms first or use test data
        print "Getting updated world map..."
        self.rooms, self.room_adjacency = get_adjacent_rooms() if not world_map else world_map

        # Check for a bad room
        if self.location not in self.rooms:
            result = "can't process orders from the invalid room %s." % str(self.location)
            return (False, FAILURE + result)

        # Group order by command and validate
        follow_commands = [arguments[THEME] for command, arguments in orders 
                           if command == FOLLOW_ACTION and THEME in arguments]
        if len(follow_commands) > 1:
            result = "can't follow more than one target."
            return (False, FAILURE + result)
        
        follow_command = follow_commands[0] if follow_commands else None
        print "Follow:", follow_command
        
        # Deduplicate these
        search_commands = list(set([arguments[LOCATION] for command, arguments in orders 
                                    if command == SEARCH_ACTION and LOCATION in arguments]))
        print "Search:", search_commands
        
        go_commands = [arguments[LOCATION] for command, arguments in orders 
                       if command == GO_ACTION and LOCATION in arguments]
        if len(go_commands) > 1:
            result = "can only satisfy one go command at a time."
            return (False, FAILURE + result)
        
        go_command = go_commands[0] if go_commands else None
        print "Go:", go_command
        
        get_commands = list(set([(arguments[SOURCE], arguments[THEME]) for command, arguments in orders 
                        if command == GET_ACTION and SOURCE in arguments and THEME in arguments]))
        print "Get:", get_commands
        
        # Check for underspecified gets
        for command in get_commands:
            if UNDERSPECIFIED in command:
                result = "can't carry out underspecified the get command %s." % str(command)
                return (False, FAILURE + result)
        
        # Get other commands, but ignore them if we can't make sense of them
        other_commands = []
        for command, arguments in orders:
            if command not in KNOWN_ACTIONS:
                try:
                    other_commands.append((command, arguments[THEME]))
                except KeyError:
                    print "Warning: Ignoring command %s, don't know how to do it." % \
                        repr(command)

        other_commands = list(set(other_commands))
        print "Other:", other_commands
        
        # Carry over standing orders
        if self.last_other_commands:
            print "Carrying over standing orders:", self.last_other_commands
            other_commands = list(set(other_commands + self.last_other_commands))
        
        # Note whether we're exploring
        exploring = search_commands == ['floor']

        # Check whether there's nothing to be done
        if not (go_command or follow_commands or search_commands or exploring):
            result = "I'd like to carry out your orders, but you haven't told me anything about where to go."
            return (True, result)      

        # Rule out mutually exclusive commands
        if follow_command and (go_command or search_commands):
            result = "can't follow and go/search in the same commands."
            return (False, FAILURE + result)

        # Check for going to places we can't handle yet
        rooms_to_visit = search_commands + [self.location] + ([go_command] if go_command else [])
        bad_rooms = [room for room in rooms_to_visit if room not in self.rooms]
        # Skip check if we're exploring
        if bad_rooms and not exploring:
            result = "can't plan using the rooms %s because they aren't on the map." % str(bad_rooms)
            return (False, FAILURE + result)
        
        # Wait until we stop moving
        print "Waiting for motion lock..."
        with self.motion_lock:
            print "Got motion lock."
        
            # Set the resynth flag so we stop processing environment updates
            self.resynthesizing = True
            
            # Generate and load        
            ltl_gen = LTLGenerator(self.rooms, self.room_adjacency, go_command, get_commands, 
                                   search_commands, follow_command, other_commands)
            # Check for tells before generating
            tells = self.world_knowledge and self.world_knowledge.watch_list
            try:
                (aut_path, self.custom_props, self.sensors, self.actuators, room_map) = \
                    ltl_gen.generate(self.location, init_props, OUT_FILE_PREFIX, self.basedir, tells)
            except UnrealizableError:
                result = "specification is unrealizable."
                print result
                self.resynthesizing = False
                return (False, FAILURE + result)
            
            # Load it up, noting whether it's in follow mode
            follow_mode = bool(follow_command) or exploring
            self.load_automaton(aut_path, self.rooms, self.sensors, self.actuators, self.custom_props, 
                                room_map, ltl_gen, follow_mode, test_mode)
            # Stash away the orders
            self.last_orders = orders
            self.last_other_commands = other_commands
            self.resynthesizing = False
            return (True, OKAY)

    def process_sensor_data(self, msg, world_map=None, test_mode=False):
        """Process updates to the environment and return whether the automaton is done"""
        # If we're resynthesizing, ignore the data
        if self.resynthesizing:
            print "Ignoring environment data during resynthesis."
            return False
                       
        # Processing sensors happens right away so that the current room is always updated
        visible_items = msg.scene.visible
        current_room = None
        activated_sensors = []
        for item in visible_items:
            if item.type == Fiducial.TYPE_REGION:
                current_room = item.id
            elif item.type in SENSOR_MAPPING:
                activated_sensors.append(SENSOR_MAPPING[item.type])
            else:
                # Support things without real types
                activated_sensors.append(item.id)
        
        # Update the room
        if current_room and self.location != current_room:
            print "Setting current room to", current_room
            self.location = current_room
                    
        # Check the map server to see if we're up to date
        map_changed = False
        new_rooms, new_room_adjacency = get_adjacent_rooms() if not world_map else world_map
        # If we're filling in data for the first time, note it
        if not self.room_adjacency or adjacency_changed(self.room_adjacency, new_room_adjacency):
            if not self.room_adjacency:
                print "Received map information for the first time:"
            else:
                print "World map has changed:"
            print new_room_adjacency
            self.rooms = new_rooms
            self.room_adjacency = new_room_adjacency
            map_changed = True          
        
        # If we don't have an automaton, exit at this point
        if not self.aut:
            if not self.waiting or map_changed:
                print "No automaton loaded, so ignoring sensor status."
                self.waiting = True
            return False
        
        if self.aut.isInitialized() and self.aut.isDone():
            # We're already done, so return True
            return True

        # Build dict of environment
        env = dict([(item.lower(), 1) for item in activated_sensors 
                    if item.lower() in self.sensors])

        # Fill in rest with defaults
        for sensor in self.sensors:
            if sensor not in env:
                env[sensor] = 0
        
        self.sensor_handler.updateSensors(env)
        
        # If the map has changed, resynthesize
        if map_changed:
            print "Resynthesizing due to map change..."
            current_props = self.aut.getCurrentOuputs()
            print "Current outputs are:", current_props
            self.process_orders(self.last_orders, current_props, test_mode, world_map, stop=False)
        
        # Initialize the automaton if it needs to be done, and then step
        if not self.aut.isInitialized():
            if current_room:
                # The env is different in this case: keep only the ones that are supposed
                # to be true
                init_sensors = [sensor for sensor in env if env[sensor]]
                # Add on the current props if we resynthesized
                if map_changed:
                    init_sensors += current_props
                    
                # If we're following, lie about the room
                init_room = current_room if not self.aut.is_follow else None
                print "Initializing automaton in room", init_room
                self.aut.chooseInitialState(init_room, init_sensors, map_changed)
            else:
                print ("Warning: Cannot initialize automaton without knowing the location. "
                       "Waiting for another message...")
                self.waiting = False
                return False

        if self.aut.runIteration():
            # If something changed as a result of the iteration, print about it
            print "Transition was made based on environment data:"
            print [item for item in activated_sensors]
            print "Current location is", self.location
            print
Esempio n. 4
0
class StateManager:
    """Provide semantics/LTL interface and automaton management."""

    def __init__(self, nlmaster=None):
        # The automaton and publisher are filled in later so the state manager
        # can start up early
        self.aut = None
        self.pub = None
        self.basedir = "."
        
        self.rooms = None
        self.sensors = None
        self.actuators = None
        self.custom_props = None
        self.sensor_handler = None
        self.motion_handler = None
        self.location = None
        self.room_adjacency = None
        self.room_map = None
        self.last_orders = None
        self.last_other_commands = []
        self.waiting = False
        
        # Thread safety
        self.resynthesizing = False
        self.motion_lock = Lock()
        
        # Connection to a knowledge object, used to check on tells. Will be set by
        # others if neded
        self.world_knowledge = None
        # Similarly, the nlmaster
        self.nlmaster = nlmaster
        
    def load_test_automaton(self, aut_path, movement_test_mode):
        """Load the testing automaton with preset values."""
        rooms = ["r_" + room for room in ("hall", "classroom", "outer_lab")]
        sensors = ["bomb", "user1", "hostage", "search_done"]
        actuators = ["get_defuser", "search"]
        custom_props = ["work", "done", "s_classroom", "s_outer_lab", "have_defuser"]
        self.load_automaton(aut_path, rooms, sensors, actuators, custom_props, None, False,
                            movement_test_mode)

    def load_automaton(self, aut_path, rooms, sensors, actuators, custom_props, room_map,
                       ltl_generator, is_follow, test_mode=False):
        """Load an automaton from a given path."""
        print "Loading automaton from", aut_path
        self.rooms = rooms
        self.sensors = sensors
        self.actuators = actuators
        self.custom_props = custom_props
        self.room_map = room_map
        
        # Initialize all sensors to zero if needed and load the automaton
        if not self.sensor_handler:
            self.sensor_handler = SensorHandler(dict([(sensor, 0) for sensor in sensors]))        
        if not self.motion_handler:
            self.motion_handler = MotionHandler(self, test_mode)
        self.aut = Automaton(self.rooms, self.sensor_handler, ActuatorHandler(self), 
                             self.motion_handler)
        self.aut.loadFile(aut_path, sensors, actuators, custom_props, ltl_generator, is_follow)

    def stop(self):
        """Stop the currently executing automaton and unload it."""
        self.aut.stop()
        self.aut = None

    def set_publisher(self, pub):
        """Set the ROS publisher."""
        self.pub = pub

    def set_basedir(self, path):
        """Set the base directory to call LTLGeneration scripts from."""
        self.basedir = path
    
    def process_orders(self, orders, init_props=None, test_mode=False, world_map=None, stop=True):
        """Process the given orders."""
        # Stop if we need to
        if stop and self.aut:
            self.stop()

        # Put in empty init_props
        if not init_props:
            init_props = ()
                    
        print "Processing orders:", str(orders)
        
        # Update rooms first or use test data
        print "Getting updated world map..."
        self.rooms, self.room_adjacency = get_adjacent_rooms() if not world_map else world_map

        # Check for a bad room
        if self.location not in self.rooms:
            result = "can't process orders from the invalid room %s." % str(self.location)
            return (False, FAILURE + result)

        # Group order by command and validate
        follow_commands = [arguments[THEME] for command, arguments in orders 
                           if command == FOLLOW_ACTION and THEME in arguments]
        if len(follow_commands) > 1:
            result = "can't follow more than one target."
            return (False, FAILURE + result)
        
        follow_command = follow_commands[0] if follow_commands else None
        print "Follow:", follow_command
        
        # Deduplicate these
        search_commands = list(set([arguments[LOCATION] for command, arguments in orders 
                                    if command == SEARCH_ACTION and LOCATION in arguments]))
        print "Search:", search_commands
        
        go_commands = [arguments[LOCATION] for command, arguments in orders 
                       if command == GO_ACTION and LOCATION in arguments]
        if len(go_commands) > 1:
            result = "can only satisfy one go command at a time."
            return (False, FAILURE + result)
        
        go_command = go_commands[0] if go_commands else None
        print "Go:", go_command
        
        get_commands = list(set([(arguments[SOURCE], arguments[THEME]) for command, arguments in orders 
                        if command == GET_ACTION and SOURCE in arguments and THEME in arguments]))
        print "Get:", get_commands
        
        # Check for underspecified gets
        for command in get_commands:
            if UNDERSPECIFIED in command:
                result = "can't carry out underspecified the get command %s." % str(command)
                return (False, FAILURE + result)
        
        # Get other commands, but ignore them if we can't make sense of them
        other_commands = []
        for command, arguments in orders:
            if command not in KNOWN_ACTIONS:
                try:
                    other_commands.append((command, arguments[THEME]))
                except KeyError:
                    print "Warning: Ignoring command %s, don't know how to do it." % \
                        repr(command)

        other_commands = list(set(other_commands))
        print "Other:", other_commands
        
        # Carry over standing orders
        if self.last_other_commands:
            print "Carrying over standing orders:", self.last_other_commands
            other_commands = list(set(other_commands + self.last_other_commands))
        
        # Note whether we're exploring
        exploring = search_commands == ['floor']

        # Check whether there's nothing to be done
        if not (go_command or follow_commands or search_commands or exploring):
            result = "I'd like to carry out your orders, but you haven't told me anything about where to go."
            return (True, result)      

        # Rule out mutually exclusive commands
        if follow_command and (go_command or search_commands):
            result = "can't follow and go/search in the same commands."
            return (False, FAILURE + result)

        # Check for going to places we can't handle yet
        rooms_to_visit = search_commands + [self.location] + ([go_command] if go_command else [])
        bad_rooms = [room for room in rooms_to_visit if room not in self.rooms]
        # Skip check if we're exploring
        if bad_rooms and not exploring:
            result = "can't plan using the rooms %s because they aren't on the map." % str(bad_rooms)
            return (False, FAILURE + result)
        
        # Wait until we stop moving
        print "Waiting for motion lock..."
        with self.motion_lock:
            print "Got motion lock."
        
            # Set the resynth flag so we stop processing environment updates
            self.resynthesizing = True
            
            # Generate and load        
            ltl_gen = LTLGenerator(self.rooms, self.room_adjacency, go_command, get_commands, 
                                   search_commands, follow_command, other_commands)
            # Check for tells before generating
            tells = self.world_knowledge and self.world_knowledge.watch_list
            try:
                (aut_path, self.custom_props, self.sensors, self.actuators, room_map) = \
                    ltl_gen.generate(self.location, init_props, OUT_FILE_PREFIX, self.basedir, tells)
            except UnrealizableError:
                result = "specification is unrealizable."
                print result
                self.resynthesizing = False
                return (False, FAILURE + result)
            
            # Load it up, noting whether it's in follow mode
            follow_mode = bool(follow_command) or exploring
            self.load_automaton(aut_path, self.rooms, self.sensors, self.actuators, self.custom_props, 
                                room_map, ltl_gen, follow_mode, test_mode)
            # Stash away the orders
            self.last_orders = orders
            self.last_other_commands = other_commands
            self.resynthesizing = False
            return (True, OKAY)

    def process_sensor_data(self, msg, world_map=None, test_mode=False):
        """Process updates to the environment and return whether the automaton is done"""
        # If we're resynthesizing, ignore the data
        if self.resynthesizing:
            print "Ignoring environment data during resynthesis."
            return False
                       
        # Processing sensors happens right away so that the current room is always updated
        visible_items = msg.scene.visible
        current_room = None
        activated_sensors = []
        for item in visible_items:
            if item.type == Fiducial.TYPE_REGION:
                current_room = item.id
            elif item.type in SENSOR_MAPPING:
                activated_sensors.append(SENSOR_MAPPING[item.type])
            else:
                # Support things without real types
                activated_sensors.append(item.id)
        
        # Update the room
        if current_room and self.location != current_room:
            print "Setting current room to", current_room
            self.location = current_room
                    
        # Check the map server to see if we're up to date
        map_changed = False
        new_rooms, new_room_adjacency = get_adjacent_rooms() if not world_map else world_map
        # If we're filling in data for the first time, note it
        if not self.room_adjacency or adjacency_changed(self.room_adjacency, new_room_adjacency):
            if not self.room_adjacency:
                print "Received map information for the first time:"
            else:
                print "World map has changed:"
            print new_room_adjacency
            self.rooms = new_rooms
            self.room_adjacency = new_room_adjacency
            map_changed = True          
        
        # If we don't have an automaton, exit at this point
        if not self.aut:
            if not self.waiting or map_changed:
                print "No automaton loaded, so ignoring sensor status."
                self.waiting = True
            return False
        
        if self.aut.isInitialized() and self.aut.isDone():
            # We're already done, so return True
            return True

        # Build dict of environment
        env = dict([(item.lower(), 1) for item in activated_sensors 
                    if item.lower() in self.sensors])

        # Fill in rest with defaults
        for sensor in self.sensors:
            if sensor not in env:
                env[sensor] = 0
        
        self.sensor_handler.updateSensors(env)
        
        # If the map has changed, resynthesize
        if map_changed:
            print "Resynthesizing due to map change..."
            current_props = self.aut.getCurrentOuputs()
            print "Current outputs are:", current_props
            self.process_orders(self.last_orders, current_props, test_mode, world_map, stop=False)
        
        # Initialize the automaton if it needs to be done, and then step
        if not self.aut.isInitialized():
            if current_room:
                # The env is different in this case: keep only the ones that are supposed
                # to be true
                init_sensors = [sensor for sensor in env if env[sensor]]
                # Add on the current props if we resynthesized
                if map_changed:
                    init_sensors += current_props
                    
                # If we're following, lie about the room
                init_room = current_room if not self.aut.is_follow else None
                print "Initializing automaton in room", init_room
                self.aut.chooseInitialState(init_room, init_sensors, map_changed)
            else:
                print ("Warning: Cannot initialize automaton without knowing the location. "
                       "Waiting for another message...")
                self.waiting = False
                return False

        if self.aut.runIteration():
            # If something changed as a result of the iteration, print about it
            print "Transition was made based on environment data:"
            print [item for item in activated_sensors]
            print "Current location is", self.location
            print
        else:
            print "\rWaiting for a state transition, sensors:", \
                [item for item in activated_sensors],

        # Return whether or not the automaton is done executing
        if self.aut.isDone():
            print "Automaton has completed execution. Future environment messages will be ignored."
            print "Mission accomplished!"
            self.notify_done()
            return True
        else:
            self.waiting = False
            return False

    def notify_actuator(self, action, target):
        """Notify the commander that we're actuating."""
        # Skip notification on explore and search
        if action in (EXPLORE_PROP, SEARCH_PROP):
            return

        response = ("I'm now going to %s the %s in the %s." % (action, target, self.location) 
                    if target else  "I'm now going to %s in the %s." % (action, self.location))
        if self.nlmaster:
            self.nlmaster.send_response(response)
        else:
            print response

    def notify_done(self):
        """Notify the commander that we're done."""
        response = "I'm done and I'm in the %s." % self.location
        if self.nlmaster:
            self.nlmaster.send_response(response)
        else:
            print response