def send_point(self):
point = self.mem.get(self.mem.PUTTING_POINT)
point1 = copy.deepcopy(point)
point2 = copy.deepcopy(point1)
point1 = check_location_clear(self.mem,point1,1)
if (point1.x == point2.x) and (point1.y == point2.y) :
#print("The points are same")
x = point1.x
y = point1.y
else:
point1 = check_location_clear(self.mem,point1,2)
# print("The points are different")
# print(point1)
x = point1.x
y = point1.y
# print(str(self.mem.get(self.mem.PUTTING_POINT)))
# raw_input("enter")
#print("the final point is ")
#print(point)
self.msgDict = {'x': x, 'y': y,
'z': point.z, 'time': self.startTime, 'cmd_id': self.msgID}
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):
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):
#we need to find the target block?
lastLocReport = get_last_location(self.mem, self.midcaAction[2])
t = time.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
x = lastLocReport[0].x
y = lastLocReport[0].y
z = lastLocReport[0].Z
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 reach the block using the position of the block
lastLocReport = get_last_location(self.mem, self.objectOrID)
t = time.now()
if not lastLocReport:
if verbose >= 1:
print("No object location found, so action:", self, "will fail.")
self.status = FAILED
return
x = lastLocReport[0].x
y = lastLocReport[0].y
z = lastLocReport[0].z
#z = 0.02477944410983878
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 ros_msg(self, topic, d): ''' arg d should be a dictionary that contains the key/value pairs to be sent. ''' sent = rosrun.send_msg(topic, rosrun.dict_as_msg) if not sent: if verbose >= 1: print "Unable to send msg; ", d, "on topic", topic, " Action", self, "assumed failed." self.status = FAILED
def send_msg(self):
self.msgDict = {'cmd': "release it", 'time': self.startTime, 'cmd_id': self.msgID}
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):
#0.6480168766398825, 0.4782503847940384, 0.289534050209461
raising_point = self.mem.get(self.mem.RAISING_POINT)
self.msgDict = {'x': raising_point.x, 'y': raising_point.y,
'z': raising_point.z, 'time': self.startTime, 'cmd_id': self.msgID}
# self.msgDict = {'x': 0.6480168766398825, 'y': 0.4782503847940384,
# 'z': 0.289534050209461, 'time': self.startTime, 'cmd_id': self.msgID}
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):
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 send_point(self):
#0.6480168766398825, 0.4782503847940384, 0.289534050209461
# self.msgDict = {'x': 0.6480168766398825, 'y': 0.4782503847940384,
# 'z': 04665006901665164, 'time': self.startTime, 'cmd_id': self.msgID}
point = self.mem.get(self.mem.PUTTING_POINT)
# point = Point(0.7450848313136519, 0.11634406023548731, -0.15821251824917773)
point1 = copy.deepcopy(point)
point2 = copy.deepcopy(point1)
point1 = check_location_clear(self.mem, point1, 1)
'''
print("got point")
print(point1)
print("given point")
print(point2)
'''
if (point1.x == point2.x) and (point1.y == point2.y):
#print("The points are same")
x = point1.x
y = point1.y
else:
point1 = check_location_clear(self.mem, point1, 2)
# print("The points are different")
# print(point1)
x = point1.x
y = point1.y
# raw_input("enter")
'''
print(point1)
print("given point is ")
print(str(x) + "," + str(y) + "," + str(z))
raw_input("enter")
'''
'''
if not (point1.x == x and point1.y == y) :
print("the point i got if it is not clear ")
temps= check_location_clear(self.mem,point1,2)
print("the point i got if it is further clear ")
print(str(x) ,str(y) ,str(z))
x = temps.x
y = temps.y
z = temps.z
#raw_input("enter")
'''
# print(str(self.mem.get(self.mem.PUTTING_POINT)))
# raw_input("enter")
#print("the final point is ")
#print(point)
self.msgDict = {
'x': x,
'y': y,
'z': point.z,
'time': self.startTime,
'cmd_id': self.msgID
}
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 = time.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 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 = time.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
