def run(self, cycle, verbose=2):
#self.ObserveWorld()
detectionEvents = self.mem.get_and_clear(self.mem.ROS_OBJS_DETECTED)
detecttionBlockState = self.mem.get_and_clear(self.mem.ROS_OBJS_STATE)
utteranceEvents = self.mem.get_and_clear(self.mem.ROS_WORDS_HEARD)
feedback = self.mem.get_and_clear(self.mem.ROS_FEEDBACK)
world = self.mem.get_and_lock(self.mem.STATE)
if not detectionEvents:
detectionEvents = []
if not detecttionBlockState:
detecttionBlockState = []
if not utteranceEvents:
utteranceEvents = []
if not feedback:
feedback = []
for event in detectionEvents:
event.time = midcatime.now()
world.sighting(event)
for blockstate in detecttionBlockState:
blockstate.time = midcatime.now()
world.position(blockstate)
for event in utteranceEvents:
event.time = midcatime.now()
world.utterance(event)
for msg in feedback:
d = rosrun.msg_as_dict(msg)
d['received_at'] = float(midcatime.now())
self.mem.add(self.mem.FEEDBACK, d)
self.mem.unlock(self.mem.STATE)
if verbose > 1:
print "World observed:", len(
detectionEvents), "new detection event(s),", len(
utteranceEvents), "utterance(s) and", len(
feedback), "feedback msg(s)"
def send_point(self):
lastLocReport = get_last_location(self.mem, self.objectOrID)
t = midcatime.now()
if not lastLocReport:
if verbose >= 1:
print "No object location found, so action:", self, "will fail."
self.status = FAILED
return
if t - lastLocReport[1] > self.maxAllowedLag:
if verbose >= 1:
print "Last object location report is too old -",
str((t - lastLocReport[1]
).total_seconds()), "seconds - so action:", self,
"will fail."
self.status = FAILED
return
self.msgDict = {
'x': lastLocReport[0].x,
'y': lastLocReport[0].y,
'z': lastLocReport[0].z,
'midcatime': self.startTime,
'cmd_id': self.msgID
}
print "trying to send"
sent = rosrun.send_msg(self.topic, rosrun.dict_as_msg(self.msgDict))
if not sent:
if verbose >= 1:
# print "Unable to send msg; ", msg, "on topic", topic, " Action", self,
# "assumed failed."
print "fail!!!!!"
self.status = FAILED
def completion_check(self): t = midcatime.now() if t - self.startTime > self.maxDuration: if verbose >= 1: print "max midcatime exceeded for action:", self, "- changing status to failed." self.status = FAILED return False lastLocReport = get_last_location(self.mem, self.objectOrID) if not lastLocReport: return False return t - lastLocReport[1] <= self.maxAllowedLag
def execute(self): if not self.startTime: self.startTime = midcatime.now() self.status = IN_PROGRESS if not self.executeFunc: return try: self.executeFunc(self.mem, self.midcaAction, self.status) except: if verbose >= 2: print "Error executing action", self, ":\n", traceback.format_exc(), "\n\nAction assumed to be failed" self.status = FAILED
def run(self, cycle, verbose = 2):
detectionEvents = self.mem.get_and_clear(self.mem.ROS_OBJS_DETECTED)
utteranceEvents = self.mem.get_and_clear(self.mem.ROS_WORDS_HEARD)
feedback = self.mem.get_and_clear(self.mem.ROS_FEEDBACK)
world = self.mem.get_and_lock(self.mem.STATE)
if not detectionEvents:
detectionEvents = []
if not utteranceEvents:
utteranceEvents = []
if not feedback:
feedback = []
for event in detectionEvents:
event.time = midcatime.now()
world.sighting(event)
for event in utteranceEvents:
event.time = midcatime.now()
world.utterance(event)
for msg in feedback:
d = rosrun.msg_as_dict(msg)
d['received_at'] = float(midcatime.now())
self.mem.add(self.mem.FEEDBACK, d)
self.mem.unlock(self.mem.STATE)
if verbose > 1:
print "World observed:", len(detectionEvents), "new detection event(s),", len(utteranceEvents), "utterance(s) and", len(feedback), "feedback msg(s)"
def current_location(self, objectOrID, maxTimeSinceSeen): ''' If the given object or an object with the given ID has been sighted within the last maxTimeSinceSeen at a specific location, will return that location. Otherwise will return None. ''' currentTime = midcatime.now() object = self.get_object(objectOrID) if object and object in sightings: for detectionEvent in sightings[object].reverse(): if detectionEvent.time - currentTime > maxTimeSinceSeen: return None #too long since last sighting if detectionEvent.loc != None: return detectionEvent.loc return None else: return None
def current_location(self, objectOrID, maxTimeSinceSeen):
'''
If the given object or an object with the given ID has been sighted within the
last maxTimeSinceSeen at a specific location, will return that location. Otherwise
will return None.
'''
currentTime = midcatime.now()
object = self.get_object(objectOrID)
if object and object in sightings:
for detectionEvent in sightings[object].reverse():
if detectionEvent.time - currentTime > maxTimeSinceSeen:
return None #too long since last sighting
if detectionEvent.loc != None:
return detectionEvent.loc
return None
else:
return None
def check_complete(self): if not self.startTime: self.startTime = midcatime.now() if not self.check_complete: return try: complete = self.isComplete(self.mem, self.midcaAction, self.status) if verbose >= 2 and not complete: print "Action", self, "not complete." if verbose >= 1 and complete: print "Action", self, "complete." if complete: self.status = COMPLETE return complete except: if verbose >= 1: print "Error checking completion status for action", self, " - Assuming \ failure:\n", traceback.format_exc() self.status = FAILED
def msg_as_dict(msg):
info = {}
info["received_at"] = midcatime.now()
for pair in msg.split("|"):
pair = pair.strip()
if not pair:
continue #ignore whitespace; e.g. after final '|'
try:
key, value = pair.split(":")
except:
raise Exception("Improperly formatted feedback received: " + s)
return
key = key.strip()
value = value.strip()
try:
value = float(value)
except:
pass #not a number; fine.
info[key] = value
return info
def check_confirmation(self): checkTime = self.lastCheck self.lastCheck = midcatime.now() feedback = self.mem.get(self.mem.FEEDBACK) if not feedback: return False for item in reversed(feedback): #if all items have been checked, either in this check or previous, return if item['received_at'] - checkTime < 0: return False #else see if item is a completion or failure message with id == self.msgID if item[CMD_ID_KEY] == self.msgID: if item[FEEDBACK_KEY] == COMPLETE: return True elif item[FEEDBACK_KEY] == FAILED: self.status = FAILED if verbose >= 1: print "MIDCA received feedback that action", self, "has failed" return False return False
def run(self, cycle, verbose = 2):
world = self.mem.get(self.mem.STATE)
i = len(world.utterances)
while i > 0:
if self.lastTime - world.utterances[i - 1].time > 0:
break
i -= 1
newUtterances = [utterance.utterance for utterance in world.utterances[i:]]
#now add goals based on new utterances
for utterance in newUtterances:
if verbose >= 2:
print "received utterance:", utterance
if utterance == "point to the quad" or utterance == "point at max":
goal = goals.Goal(objective = "show-loc", subject = "self",
directObject = "quad", indirectObject = "observer")
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(goal), "but it is already in the \
goal graph"
self.lastTime = midcatime.now()
def send_point(self):
lastLocReport = get_last_location(self.mem, self.objectOrID)
t = midcatime.now()
if not lastLocReport:
if verbose >= 1:
print "No object location found, so action:", self, "will fail."
self.status = FAILED
return
if t - lastLocReport[1] > self.maxAllowedLag:
if verbose >= 1:
print "Last object location report is too old -",
(t - lastLocReport[1]).total_seconds(), "seconds - so action:", self,
"will fail."
self.status = FAILED
return
self.msgDict = {'x': lastLocReport[0].x, 'y': lastLocReport[0].y,
'z': lastLocReport[0].z, 'midcatime': self.startTime, 'cmd_id': self.msgID}
print "trying to send"
sent = rosrun.send_msg(self.topic, rosrun.dict_as_msg(self.msgDict))
if not sent:
if verbose >= 1:
print "Unable to send msg; ", msg, "on topic", topic, " Action", self,
"assumed failed."
self.status = FAILED
def run(self, cycle, verbose=2):
world = self.mem.get(self.mem.STATE)
i = len(world.utterances)
while i > 0:
if self.lastTime - world.utterances[i - 1].time > 0:
break
i -= 1
newUtterances = [
utterance.utterance for utterance in world.utterances[i:]
]
#now add goals based on new utterances
for utterance in newUtterances:
if verbose >= 2:
print "received utterance:", utterance
if utterance == "point to the quad" or utterance == "goodbye baxter":
goal = goals.Goal(objective="show-loc",
subject="self",
directObject="quad",
indirectObject="observer")
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "get the red block":
goal = goals.Goal(objective="holding",
subject="self",
directObject="red block",
indirectObject="observer",
pos='red block:arm')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "get the green block":
goal = goals.Goal(objective="holding",
subject="self",
directObject="green block",
indirectObject="observer",
pos='green block:arm')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "get the blue block":
goal = goals.Goal(objective="holding",
subject="self",
directObject="blue block",
indirectObject="observer",
pos='blue block:arm')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "put the green block on table":
goal = goals.Goal(objective="moving",
subject="self",
directObject="green block",
indirectObject="observer",
pos='green block:table')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "put the blue block on table":
goal = goals.Goal(objective="moving",
subject="self",
directObject="blue block",
indirectObject="observer",
pos='blue block:table')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "put the red block on table":
goal = goals.Goal(objective="moving",
subject="self",
directObject="red block",
indirectObject="observer",
pos='red block:table')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "stack the green block on the red block":
goal = goals.Goal(objective="stacking",
subject="self",
directObject="red block",
indirectObject="observer",
pos='green block:red block')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "stack the blue block on the red block":
goal = goals.Goal(objective="stacking",
subject="self",
directObject="red block",
indirectObject="observer",
pos='blue block:red block')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "stack the blue block on the green block":
goal = goals.Goal(objective="stacking",
subject="self",
directObject="green block",
indirectObject="observer",
pos='blue block:green block')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "stack the red block on the blue block":
goal = goals.Goal(objective="stacking",
subject="self",
directObject="blue block",
indirectObject="observer",
pos='red block:blue block')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "stack the green block on the blue block":
goal = goals.Goal(objective="stacking",
subject="self",
directObject="blue block",
indirectObject="observer",
pos='green block:blue block')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \ goal graph"
if utterance == "stack the red block on the green block":
goal = goals.Goal(objective="stacking",
subject="self",
directObject="green block",
indirectObject="observer",
pos='red block:green block')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
if utterance == "stack":
goal = goals.Goal(objective="stacking",
subject="self",
directObject="green block",
indirectObject="observer",
pos='red block:green block')
added = self.mem.get(self.mem.GOAL_GRAPH).insert(goal)
if verbose >= 2:
if added:
print "adding goal:", str(goal)
else:
print "generated goal:", str(
goal), "but it is already in the \
goal graph"
# else:
# print "message is unknown"
self.lastTime = midcatime.now()
def __init__(self, utterance):
self.time = midcatime.now()
self.utterance = utterance
def __init__(self, id=None, type=None, loc=None, **kwargs):
self.time = midcatime.now()
self.id = id
self.type = type
self.loc = loc
def __init__(self, id, position, isclear):
self.time = midcatime.now()
self.id = id
self.position = position
self.isclear = isclear
def init(self, world, mem):
self.mem = mem
self.lastTime = midcatime.now()
def __init__(self, utterance): self.time = midcatime.now() self.utterance = utterance
def __init__(self, id = None, type = None, loc = None, **kwargs): self.time = midcatime.now() self.id = id self.type = type self.loc = loc
