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 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)
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
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