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()
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)
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()