def Execute_Mission():
global GetInfoFlag, GetKeyFlag, ExecuteFlag, MotionKey, MotionStep, MotionSerialKey, MissionEndFlag, CurrentMissionType, Strategy_flag, Arm_state_flag
# print("strategy :" ,Strategy_flag)
# print("state :" ,Arm_state_flag)
# if Arm_state_flag == 0 and Strategy_flag == 1:
# Strategy_flag = 0
if ArmTask.Arm_state_flag.Arm_feedback == 0 and ArmTask.Strategy_state_flag.Strategy_feedback == 1:
print(11111)
ArmTask.Strategy_state_flag(0)
if MotionKey[MotionStep] == ArmMotionCommand.Arm_Stop:
if MissionEndFlag == True:
CurrentMissionType = MissionType.Mission_End
GetInfoFlag = False
GetKeyFlag = False
ExecuteFlag = False
print("Mission_End")
elif CurrentMissionType == MissionType.PushBall:
GetInfoFlag = False
GetKeyFlag = True
ExecuteFlag = False
MotionStep = 0
print("PushBall")
else:
GetInfoFlag = True
GetKeyFlag = False
ExecuteFlag = False
MotionStep = 0
else:
MotionItem(MotionSerialKey[MotionStep])
MotionStep += 1
def MotionItem(ItemNo):
global angle_SubCue, SpeedValue, PushFlag, LinePtpFlag, MissionEndFlag
SpeedValue = 5
for case in switch(ItemNo): #傳送指令給socket選擇手臂動作
if case(ArmMotionCommand.Arm_Stop):
MoveFlag = False
print("Arm_Stop")
break
if case(ArmMotionCommand.Arm_StopPush):
MoveFlag = False
PushFlag = True #重新掃描物件
print("Arm_StopPush")
break
if case(ArmMotionCommand.Arm_MoveToTargetUpside):
pos.x = 10
pos.y = 36.8
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 10
MoveFlag = True
LinePtpFlag = False
SpeedValue = 10
print("Arm_MoveToTargetUpside")
break
if case(ArmMotionCommand.Arm_LineUp):
pos.z = ObjAboveHeight
MoveFlag = True
LinePtpFlag = True
SpeedValue = 5
print("Arm_LineUp")
break
if case(ArmMotionCommand.Arm_LineDown):
pos.z = PushBallHeight
MoveFlag = True
LinePtpFlag = True
SpeedValue = 5
print("Arm_LineDown")
break
if case(ArmMotionCommand.Arm_PushBall):
pos.x = -10
pos.y = 36.8
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = -10
SpeedValue = 10 ##待測試up
MoveFlag = True
LinePtpFlag = False
print("Arm_PushBall")
break
if case(ArmMotionCommand.Arm_MoveVision):
pos.x = 0
pos.y = 36.8
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
SpeedValue = 10
MoveFlag = True
LinePtpFlag = False
##任務結束旗標
MissionEndFlag = True
print("Arm_MoveVision")
break
if case(ArmMotionCommand.Arm_MoveFowardDown):
pos.x = 0
pos.y = 36.8
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
MoveFlag = True
LinePtpFlag = False
print("Arm_MoveFowardDown")
break
if case(): # default, could also just omit condition or 'if True'
print("something else!")
# No need to break here, it'll stop anyway
if MoveFlag == True:
if LinePtpFlag == False:
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
#strategy_client_Arm_Mode(0,1,0,30,2)#action,ra,grip,vel,both
ArmTask.strategy_client_Arm_Mode(2, 1, 0, SpeedValue,
2) #action,ra,grip,vel,both
ArmTask.strategy_client_pos_move(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
elif LinePtpFlag == True:
#strategy_client_Arm_Mode(0,1,0,40,2)#action,ra,grip,vel,both
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.strategy_client_Arm_Mode(3, 1, 0, SpeedValue,
2) #action,ra,grip,vel,both
ArmTask.strategy_client_pos_move(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.strategy_client_Arm_Mode(3, 1, 0, SpeedValue,
2) #action,ra,grip,vel,both
ArmTask.strategy_client_pos_move(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
#action: ptp line
#ra : abs rel
#grip 夾爪
#vel speed
#both : Ctrl_Mode
##-------------strategy end ------------
def myhook():
print("shutdown time!")
if __name__ == '__main__':
argv = rospy.myargv()
rospy.init_node('strategy', anonymous=True)
GetInfoFlag = True #Test no data
arm_state_server()
ArmTask.strategy_client_Arm_Mode(0, 1, 0, 20, 2) #action,ra,grip,vel,both
while 1:
Mission_Trigger()
if CurrentMissionType == MissionType.Mission_End:
ArmTask.rospy.on_shutdown(myhook)
ArmTask.rospy.spin()
rospy.spin()
strategy.strategy_client_Arm_Mode(3, 1, 0, SpeedValue,
2) #action,ra,grip,vel,both
strategy.strategy_client_pos_move(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
#action: ptp line
#ra : abs rel
#grip 夾爪
#vel speed
#both : Ctrl_Mode
##-------------strategy end ------------
def myhook():
print("shutdown time!")
if __name__ == '__main__':
argv = rospy.myargv()
rospy.init_node('strategy', anonymous=True)
GetInfoFlag = True #Test no data
strategy.strategy_client_Arm_Mode(0, 1, 0, 20, 2) #action,ra,grip,vel,both
strategy.strategy_client_Speed_Mode(1)
strategy.strategy_client_Arm_Mode(4, 1, 0, SpeedValue,
2) #action,ra,grip,vel,both
while 1:
Mission_Trigger()
if CurrentMissionType == MissionType.Mission_End:
strategy.rospy.on_shutdown(myhook)
strategy.rospy.spin()
rospy.spin()
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
strategy.strategy_client_Arm_Mode(3, 1, 0, SpeedValue,
2) #action,ra,grip,vel,both
strategy.strategy_client_pos_move(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
#action: ptp line
#ra : abs rel
#grip 夾爪
#vel speed
#both : Ctrl_Mode
##-------------strategy end ------------
def myhook():
print("shutdown time!")
if __name__ == '__main__':
argv = rospy.myargv()
rospy.init_node('ros_socket', anonymous=True)
strategy.strategy_server()
GetInfoFlag = True #Test no data
strategy.strategy_client_Arm_Mode(0, 1, 0, 20, 2) #action,ra,grip,vel,both
while 1:
Mission_Trigger()
if CurrentMissionType == MissionType.Mission_End:
rospy.on_shutdown(myhook)
#rospy.spin()
rospy.spin()
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
strategy.strategy_client_Arm_Mode(3, 1, 0, SpeedValue,
2) #action,ra,grip,vel,both
strategy.strategy_client_pos_move(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
#action: ptp line
#ra : abs rel
#grip 夾爪
#vel speed
#both : Ctrl_Mode
##-------------strategy end ------------
def myhook():
print("shutdown time!")
if __name__ == '__main__':
argv = rospy.myargv()
rospy.init_node('ros_socket', anonymous=True)
strategy.strategy_server()
GetInfoFlag = True #Test no data
strategy_client_Arm_Mode(0, 1, 0, 20, 2) #action,ra,grip,vel,both
while 1:
Mission_Trigger()
if CurrentMissionType == MissionType.Mission_End:
rospy.on_shutdown(myhook)
#rospy.spin()
rospy.spin()
