def testUnthread():
OriginPosition = [418.4, 0, 629.89, 0, -90, -180]
# GoPosition = [424.686, -169.017, 297.854, 152.65, -4.135, 174.468]
# while True:
# radius = random.uniform(-30, 30)
# GoPosition[0] += radius
# GoPosition[1] += radius
# GoPosition[2] += radius
GoPosition = [560.501, -33.202, 390.669, 152.65, -4.135, 174.468]
while True:
# radius = random.uniform(-30, 10)
# GoPosition[0] += radius
# GoPosition[1] += radius
# GoPosition[2] += radius
# print GoPosition
TargetOk = Com.get_status(ShareMem, "TARGET_OK", GoPosition)
if TargetOk[0] != 0:
print "************Error!!!can not go to the position!!***************"
continue
else:
print "GoPosition1 = ", GoPosition
Com.command_to_robot(ShareMem, "GO", GoPosition, 0)
# time.sleep(2)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "ActualPosition1 = ", ActualPosition
# GoPosition[1] = copy.deepcopy(-GoPosition[1])
print "GoPosition2 = ", OriginPosition
Com.command_to_robot(ShareMem, "GO", OriginPosition, 0)
# time.sleep(2)
ActualPosition2 = Com.get_curr_pos(ShareMem)[0:6]
print "ActualPosition2 = ", ActualPosition2
def testTarget_OK(self, position):
"""
test the point whether can get to or not
input: position
return: 0 or -1, 0: TargetOk; -1: position input error
"""
return Com.get_status(ShareMem, "TARGET_OK", position)[0]
def getStatus(self, io):
while self.__IsGettingStatus:
self._my_logger.info('command ' + 'io' + str(io) + 'is waiting...')
time.sleep(0.01)
self.__IsGettingStatus = True
io_status = Com.get_status(shm_obj=self.__ShmBuf,
enq_str="SW",
param_list=io)
self.__IsGettingStatus = False
time.sleep(0.01)
return io_status[0]
def isPoseOK(self, robotPos):
assert isinstance(robotPos, (np.ndarray, list, tuple))
assert self.__ShmBuf is not None, 'Shm is not connected!'
while self.__IsGettingStatus:
self._my_logger.info('command isPoseOK' + 'is waiting...')
time.sleep(0.01)
self.__IsGettingStatus = True
if isinstance(robotPos, np.ndarray):
GoPosition = robotPos.reshape(-1).tolist()
else:
GoPosition = list(robotPos)
TargetOk = Com.get_status(self.__ShmBuf, "TARGET_OK", GoPosition)
self._my_logger.info('TargetOk?%s', TargetOk[0])
self.__IsGettingStatus = False
time.sleep(0.01)
return (0 == TargetOk[0])
def testMove():
OriginPosition = [418.4, 0, 629.89, 0, -90, -180]
GoPosition = [464.686, -169.017, 297.854, 152.65, -4.135, 174.468]
while True:
radius = random.uniform(-30, 30)
GoPosition[0] += radius
GoPosition[1] += radius
GoPosition[2] += radius
print GoPosition
TargetOk = Com.get_status(ShareMem, "TARGET_OK", GoPosition)
if TargetOk[0] != 0:
print "************Error!!!can not go to the position!!***************"
else:
Com.command_to_robot(ShareMem, "MOVE", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition[1] = -GoPosition[1]
Com.command_to_robot(ShareMem, "MOVE", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition = [464.686, -169.017, 297.854, 152.65, -4.135, 174.468]
def testThread():
OriginPosition = [418.4, 0, 629.89, 0, -90, -180]
GoPosition = [424.686, -169.017, 297.854, 152.65, -4.135, 174.468]
while True:
radius = random.uniform(-30, 30)
GoPosition[0] += radius
GoPosition[1] += radius
GoPosition[2] += radius
print GoPosition
TargetOk = Com.get_status(ShareMem, "TARGET_OK", GoPosition)
if TargetOk[0] != 0:
print "************Error!!!can not go to the position!!***************"
else:
Com.command_to_robot(ShareMem, "GO", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition[1] = -GoPosition[1]
Com.command_to_robot(ShareMem, "MOVE", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition = [464.686, -169.017, 297.854, 152.65, -4.135, 174.468]
JumpP1 = [
474.39855489, 205.01568053, 115.40855304, 152.63334917,
-4.30779311, 174.62465658
]
JumpP2 = [
479.14825634, 207.09563971, 395.88019415, 152.40037482,
-2.2939525, 172.08569708
]
JumpP3 = [
479.3525761, -209.63551643, 395.92318385, 152.40334516,
-2.29067504, 172.0860196
]
JumpP4 = [
479.39361121, -209.6325469, 146.89025608, 152.39699665,
-2.27772204, 172.07260997
]
Com.command_to_robot(ShareMem, "GO", JumpP1)
JUMP3Position = [JumpP2, JumpP3, JumpP4, 0, 0, 0]
Com.command_to_robot(ShareMem, "JUMP3", JUMP3Position)
randomNum = random.uniform(-40, 40)
JumpP1[0] = JumpP1[0] + randomNum
JumpP1[1] = JumpP1[1] + randomNum
JumpP1[2] = JumpP1[2] + randomNum
randomNum = random.uniform(-40, 40)
JumpP2[0] = JumpP2[0] + randomNum
JumpP2[1] = JumpP2[1] + randomNum
JumpP2[2] = JumpP2[2] + randomNum
randomNum = random.uniform(-40, 40)
JumpP3[0] = JumpP3[0] + randomNum
JumpP3[1] = JumpP3[1] + randomNum
JumpP3[2] = JumpP3[2] + randomNum
randomNum = random.uniform(-40, 40)
JumpP4[0] = JumpP4[0] + randomNum
JumpP4[1] = JumpP4[1] + randomNum
JumpP4[2] = JumpP4[2] - randomNum
Com.command_to_robot(ShareMem, "GO", JumpP1)
JUMP3Position = [JumpP2, JumpP3, JumpP4, 0, 0, 0]
Com.command_to_robot(ShareMem, "JUMP3", JUMP3Position)
def getConveyorDis_mm(self):
ConveyorDis = Com.get_status(self.__ShmBuf, "CONV_POS")
self._my_logger.info('get Conveyor Dis(mm): ' +
str(math.pi * ConveyorDis[0]))
return math.pi * ConveyorDis[0]