Example #1
0
    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)"
Example #2
0
 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
Example #3
0
	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
Example #4
0
	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
Example #5
0
 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)"
Example #6
0
	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
Example #7
0
    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
Example #8
0
	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
Example #9
0
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
Example #10
0
	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
Example #11
0
 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()
Example #12
0
	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
Example #13
0
    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()
Example #14
0
 def __init__(self, utterance):
     self.time = midcatime.now()
     self.utterance = utterance
Example #15
0
 def __init__(self, id=None, type=None, loc=None, **kwargs):
     self.time = midcatime.now()
     self.id = id
     self.type = type
     self.loc = loc
Example #16
0
 def __init__(self, id, position, isclear):
     self.time = midcatime.now()
     self.id = id
     self.position = position
     self.isclear = isclear
Example #17
0
 def init(self, world, mem):
     self.mem = mem
     self.lastTime = midcatime.now()
Example #18
0
	def __init__(self, utterance):
		self.time = midcatime.now()
		self.utterance = utterance
Example #19
0
	def __init__(self, id = None, type = None, loc = None, **kwargs):
		self.time = midcatime.now()
		self.id = id
		self.type = type
		self.loc = loc