def check_confirmation(self):
speed = abs(self.GraceAct.estimateDepthRate(0.5))#"implement function to get speed" arguement is time used to wait to estimate speed
print("speed is" + str(speed))
if speed and self.time:
if self.action.args[2] == "veryshallow":
if not self.time_taken:
self.time_taken = 0.4/speed
elif self.action.args[2] == "shallow":
if not self.time_taken:
self.time_taken = 0.9/speed
elif self.action.args[2] == "medium":
if not self.time_taken:
self.time_taken = 0.9/speed
elif self.action.args[2] == "deep":
if not self.time_taken:
self.time_taken = 0.9/speed
elif self.action.args[2] == "verydeep":
if not self.time_taken:
self.time_taken = 0.9/speed
if self.time:
print("Time taken is" + str(self.time_taken))
print("Time elapsed is" + str(midcatime.now() - self.time))
if (midcatime.now() - self.time) >= self.time_taken:
return True
return False
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 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 -", end=' ')
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 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)
history = self.mem.get_and_lock(self.mem.STATE_HISTORY)
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)
# if there are any change in events remember
history = self.check_with_history(world, history, detecttionBlockState)
self.mem.unlock(self.mem.STATE_HISTORY)
if history:
if len(history) > 5:
history = history[:5]
self.mem.set(self.mem.STATE_HISTORY, history)
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 implement_action(self):
self.time = midcatime.now()
self.depth = self.mem.get(self.mem.SENSE_DEPTH)
if self.depth:
self.GraceAct.communicateDepth(self.depth)
else:
global FAILED
return FAILED
pass
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 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 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 implement_action(self):
self.time = midcatime.now()
# to implement dive action once.
dive_flag = self.mem.get(self.mem.DIVE_FLAG)
if not dive_flag:
"""
Implement the dive action to go to bottom
"""
self.GraceAct.stopRegulation()
self.GraceAct.gotToDepth(6) #x in meters, so units need to be converted to meters
self.mem.set(self.mem.DIVE_FLAG, True)
else:
# to set raise flag to false after implementing dive.
self.mem.set(self.mem.RAISE_FLAG, False)
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 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 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 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 __init__(self, id, position, isclear):
self.time = midcatime.now()
self.id = id
self.position = position
self.isclear = isclear
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, utterance):
self.time = midcatime.now()
self.utterance = utterance
def __init__(self, id, position, isclear): self.time = midcatime.now() self.id = id self.position = position self.isclear = isclear
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 send_point(self):
#we reach the block using the position of the block
lastLocReport = get_last_location(self.mem, self.objectOrID)
check2stack = check_2height_stack(self.mem, self.objectOrID)
check3stack = False
if check2stack:
lastLocReport = get_last_location(self.mem, check2stack)
check3stack = check_2height_stack(self.mem, check2stack)
if check3stack:
lastLocReport = get_last_location(self.mem, check3stack)
print lastLocReport
'''
if pos_of_block == 'table':
lastLocReport = get_last_location(self.mem, self.objectOrID)
else:
lastLocReport = get_last_location(self.mem, pos_of_block)
'''
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
x = lastLocReport[0].x
y = lastLocReport[0].y
z = self.mem.get(self.mem.UNSTACK_Z)
#z = 0.02477944410983878
if self.mem.get(self.mem.CALIBRATION_MATRIX).any():
H = self.mem.get(self.mem.CALIBRATION_MATRIX)
floor_point = pixel_to_floor(H,[x, y])
x = floor_point[0]
y = floor_point[1]
# y = y -0.02
#
if check3stack:
z = self.mem.get(self.mem.UNSTACK_3Z)
# if check3stack == 'blue block':
# y = y - 0.01
# else:
# if(self.objectOrID == 'blue block'):
# y = y + 0.01
elif check2stack:
z = self.mem.get(self.mem.UNSTACK_Z)
# y=y+0.01
# if check2stack == 'blue block':
# y = y - 0.01
# else:
# if(self.objectOrID == 'blue block'):
# y = y + 0.01
self.msgDict = {'x': x, 'y': y,
'z': z, 'time': self.startTime, 'cmd_id': self.msgID}
print self.msgDict
print "trying to send"
print self.topic
sent = rosrun.send_msg(self.topic, rosrun.dict_as_msg(self.msgDict))
if not sent:
if verbose >= 1:
print "Fail"
# print "Unable to send msg; ", msg, "on topic", topic, " Action", self,
# "assumed failed."
self.status = FAILED
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, utterance): self.time = midcatime.now() self.utterance = utterance
def send_point(self):
#we reach the block using the position of the block
lastLocReport = get_last_location(self.mem, self.objectOrID)
check2stack = check_2height_stack(self.mem, self.objectOrID)
check3stack = False
if check2stack:
lastLocReport = get_last_location(self.mem, check2stack)
check3stack = check_2height_stack(self.mem, check2stack)
if check3stack:
lastLocReport = get_last_location(self.mem, check3stack)
print(lastLocReport)
'''
if pos_of_block == 'table':
lastLocReport = get_last_location(self.mem, self.objectOrID)
else:
lastLocReport = get_last_location(self.mem, pos_of_block)
'''
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
x = lastLocReport[0].x
y = lastLocReport[0].y
z = self.mem.get(self.mem.UNSTACK_Z)
#z = 0.02477944410983878
if self.mem.get(self.mem.CALIBRATION_MATRIX).any():
H = self.mem.get(self.mem.CALIBRATION_MATRIX)
floor_point = pixel_to_floor(H, [x, y])
x = floor_point[0]
y = floor_point[1]
# y = y -0.02
#
if check3stack:
z = self.mem.get(self.mem.UNSTACK_3Z)
# if check3stack == 'blue block':
# y = y - 0.01
# else:
# if(self.objectOrID == 'blue block'):
# y = y + 0.01
elif check2stack:
z = self.mem.get(self.mem.UNSTACK_Z)
# y=y+0.01
# if check2stack == 'blue block':
# y = y - 0.01
# else:
# if(self.objectOrID == 'blue block'):
# y = y + 0.01
self.msgDict = {
'x': x,
'y': y,
'z': z,
'time': self.startTime,
'cmd_id': self.msgID
}
print(self.msgDict)
print("trying to send")
print(self.topic)
sent = rosrun.send_msg(self.topic, rosrun.dict_as_msg(self.msgDict))
if not sent:
if verbose >= 1:
print("Fail")
# print "Unable to send msg; ", msg, "on topic", topic, " Action", self,
# "assumed failed."
self.status = FAILED
def send_point(self):
#we need to find the target block?
check3stack = check_2height_stack(self.mem, self.midcaAction[2])
lastLocReport = get_last_location(self.mem, self.midcaAction[2])
if check3stack:
lastLocReport = get_last_location(self.mem, check3stack)
#print(check3stack)
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 -", end=' ')
str((t - lastLocReport[1]
).total_seconds()), "seconds - so action:", self,
"will fail."
self.status = FAILED
return
#[0.6763038647265367, 0.30295363707695533, 0.018148563732166244]
x = lastLocReport[0].x
y = lastLocReport[0].y
z = self.mem.get(self.mem.STACK_Z)
# y=y+0.01
#print("the z is " + str(z))
'''
print(z)
print("z3")
print(self.mem.get(self.mem.STACK_3Z))
print("z")
print(self.mem.get(self.mem.STACK_3Z))
'''
#z = 0.018148563732166244
if self.mem.get(self.mem.CALIBRATION_MATRIX).any():
H = self.mem.get(self.mem.CALIBRATION_MATRIX)
floor_point = pixel_to_floor(H, [x, y])
x = floor_point[0]
y = floor_point[1]
# y = y -0.02
if check3stack:
print("it is of 3 height")
z = self.mem.get(self.mem.STACK_3Z)
# y= y + 0.01
#if check3stack == 'blue block':
#y= y-0.02
# else:
# if self.midcaAction[2] == 'blue block':
# y= y - 0.02
self.msgDict = {
'x': x,
'y': y,
'z': z,
'time': self.startTime,
'cmd_id': self.msgID
}
print(self.msgDict)
print("trying to send")
print(self.topic)
sent = rosrun.send_msg(self.topic, rosrun.dict_as_msg(self.msgDict))
if not sent:
if verbose >= 1:
print("Fail")
# print "Unable to send msg; ", msg, "on topic", topic, " Action", self,
# "assumed failed."
self.status = FAILED
def implement_action(self):
self.time = midcatime.now()
depth = self.GraceAct.senseDepth()
self.mem.set(self.mem.SENSE_DEPTH, depth)
pass
def send_point(self):
#we need to find the target block?
check3stack = check_2height_stack(self.mem, self.midcaAction[2])
lastLocReport = get_last_location(self.mem, self.midcaAction[2])
if check3stack:
lastLocReport = get_last_location(self.mem, check3stack)
#print(check3stack)
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
#[0.6763038647265367, 0.30295363707695533, 0.018148563732166244]
x = lastLocReport[0].x
y = lastLocReport[0].y
z = self.mem.get(self.mem.STACK_Z)
# y=y+0.01
#print("the z is " + str(z))
'''
print(z)
print("z3")
print(self.mem.get(self.mem.STACK_3Z))
print("z")
print(self.mem.get(self.mem.STACK_3Z))
'''
#z = 0.018148563732166244
if self.mem.get(self.mem.CALIBRATION_MATRIX).any():
H = self.mem.get(self.mem.CALIBRATION_MATRIX)
floor_point = pixel_to_floor(H,[x, y])
x = floor_point[0]
y = floor_point[1]
# y = y -0.02
if check3stack:
print("it is of 3 height")
z = self.mem.get(self.mem.STACK_3Z)
# y= y + 0.01
#if check3stack == 'blue block':
#y= y-0.02
# else:
# if self.midcaAction[2] == 'blue block':
# y= y - 0.02
self.msgDict = {'x': x, 'y': y,
'z': z, 'time': self.startTime, 'cmd_id': self.msgID}
print self.msgDict
print "trying to send"
print self.topic
sent = rosrun.send_msg(self.topic, rosrun.dict_as_msg(self.msgDict))
if not sent:
if verbose >= 1:
print "Fail"
# print "Unable to send msg; ", msg, "on topic", topic, " Action", self,
# "assumed failed."
self.status = FAILED
def init(self, world, mem):
self.mem = mem
self.lastTime = midcatime.now()
def implement_action(self):
self.time = midcatime.now()
self.GraceAct.stopRegulation()
self.mem.set(self.mem.RAISE_FLAG, False)
self.mem.set(self.mem.DIVE_FLAG, False)
pass
