案例/模板说明
============================

机械臂主控主函数说明
++++++++++++++++++++++++++++++++

以下为机械臂主控模板代码，注意对工控机返回的 err_code 进行判断。

.. code-block:: 

        def xyzMain():
        xyzMasterConnect() #连接工控机
        xyzTest() #集成测试
        xyzMasterClose() #断开连接
        end

        ##########################Test Code##############################

        # This function is for testing all api
        def xyzTest():
        xyzTestSwitchApp()
        xyzTestSwitchObj()
        xyzTestSwitchTool()
        # Because of token, the next 3 test sequence can not be changed
        xyzTestImg()
        xyzTestGrasp()
        xyzTestObj()
        xyzTestResetVision()
        xyzTestSendJointCartExtJoint()
        xyzTestReqPickPlace()
        xyzTestGetPickPlace()
        xyzTestSwitchStrat()
        xyzTestUpdate()
        xyzTestGetObjPoseType()
        xyzTestRestPalletStatus()
        end

        def xyzTestSwitchApp():
        app_name = "111"
        error_code = xyzSwitchApp(app_name)
        app_name = "error_app"
        error_code = xyzSwitchApp(app_name)
        end

        def xyzTestSwitchObj():
        obj_name = "222"
        error_code = xyzSwitchObj(obj_name)
        obj_name = "error_obj"
        error_code = xyzSwitchObj(obj_name)
        end

        def xyzTestSwitchTool():
        tool_name = "333"
        error_code = xyzSwitchTool(tool_name)
        tool_name = "error_tool"
        error_code = xyzSwitchTool(tool_name)
        end

        def xyzTestImg():
        ws_id = 0
        token = 0
        # token in robot server will increase after every call
        error_code = xyzReqCapImg(ws_id)
        error_code = xyzGetCapImg(token)
        ws_id = 0
        error_code = xyzCapImg(ws_id)
        end

        def xyzTestGrasp():
        ws_id = 0
        pose_num = 0
        pose_type = 0
        grasp_pose = p[0,0,0,0,0,0]
        token = 0
        # token in robot server will increase after every call
        error_code = xyzReqGraspPose(ws_id)
        token = 2
        error_code = xyzGetGraspPose(token)
        end

        def xyzTestObj():
        pose_num = 0
        pose_type = 0
        obj_pose = p[0,0,0,0,0,0]
        ws_id = 0
        token = 0
        # token in robot server will increase after every call
        error_code = xyzReqObjPose(ws_id)
        token = 3
        error_code = xyzGetObjPose(token)
        end

        def xyzTestResetVision():
        ws_id = 0
        error_code = xyzResetVision(ws_id)
        end

        def xyzTestSendJointCartExtJoint():
        error_code = xyzSendCurrentJoints()
        error_code = xyzSendCurrentCartPose()
        end

        def xyzTestReqPickPlace():
        error_code = xyzReqPick()
        error_code = xyzReqPlace()
        error_code = xyzReqPickPlace()
        end

        def xyzTestGetPickPlace():
        pose_type = 0
        pose_num = 0
        # please check the waypoint data before xyzExecuteTraj() !
        error_code = xyzGetPickin()
        xyzExecuteTraj()
        error_code = xyzGetPickout()
        xyzExecuteTraj()
        error_code = xyzGetPlacein()
        xyzExecuteTraj()
        error_code = xyzGetPlaceout()
        xyzExecuteTraj()
        end

        def xyzTestSwitchStrat():
        strat_name = "strat1"
        error_code = xyzSwitchStrat(strat_name)
        strat_name = "error_strat"
        error_code = xyzSwitchStrat(strat_name)
        end

        def xyzTestUpdate():
        tote_pose = p[0,0,0,0,0,0]
        pose_type = 0
        pose_num = 0
        error_code = xyzUpdateTotePose()
        error_code = xyzUpdateObjPoseOnHand()
        error_code = xyzUpdateObjPoseToHand()
        end

        def xyzTestGetObjPoseType():
        pose_type = 0
        error_code = xyzGetObjPoseType()
        end

        def xyzTestRestPalletStatus():
        error_code = xyzResetPalletStatus()
        end

        # This is the main function for test 主函数
        xyzMain()