Пример #1
0
    private IEnumerator RunProgram()
    {
        //wait while program is building
        yield return(new WaitWhile(() => buildingProgram));

        Dictionary <UInt16, GameObject> programToVisualize = new Dictionary <UInt16, GameObject>();
        Dictionary <UInt16, UInt16>     programOrderHelper = new Dictionary <UInt16, UInt16>();

        foreach (var block in program)
        {
            programToVisualize = block.Value;
            //load programOrderHelper dictionary
            if (!programHelper.TryGetValue(block.Key, out programOrderHelper))
            {
                break;
            }

            UInt16 currentID    = programOrderHelper.First().Key;
            UInt16 on_successID = programOrderHelper.First().Value;

            while (true)
            {
                GameObject programItem;
                //load programItem by currentID key
                if (!programToVisualize.TryGetValue(currentID, out programItem))
                {
                    break;
                }

                Debug.Log("Visualizing: " + currentID.ToString() + ".. On success will be: " + on_successID.ToString());

                //execute programItem
                switch (programItem.gameObject.tag)
                {
                case "PICK_FROM_POLYGON":
                    Debug.Log("Starting pick from polygon");
                    PickFromPolygon pfg = programItem.GetComponent <PickFromPolygon>();
                    //sends change of interface to ARTable - for states switching during visualization
                    SendInterfaceStateChangeToROS(pfg.programItem, block.Key);
                    currentProgramItem = pfg.programItem;
                    pfg.Run();
                    //wait until instruction finishes or until user says "Next" or "Previous"
                    yield return(new WaitWhile(() => pfg.IsRunning() && !next && !previous));

                    //if Next is said, instruction skips to its end state
                    if (next)
                    {
                        pfg.OnNextInstruction();
                        next = false;
                    }
                    ////if Previous is said, instruction skips to its initial state and decrements iterator twice to play previous instruction
                    //if (previous) {
                    //    pfg.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending pick from polygon");
                    break;

                case "PICK_FROM_FEEDER":
                    Debug.Log("Starting pick from feeder");
                    PickFromFeeder pff = programItem.GetComponent <PickFromFeeder>();
                    SendInterfaceStateChangeToROS(pff.programItem, block.Key);
                    currentProgramItem = pff.programItem;
                    pff.Run();
                    yield return(new WaitWhile(() => pff.IsRunning() && !next && !previous));

                    if (next)
                    {
                        pff.OnNextInstruction();
                        next = false;
                    }
                    //if (previous) {
                    //    pff.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending pick from feeder");
                    break;

                case "PLACE_TO_POSE":
                    Debug.Log("Starting place to pose");
                    PlaceToPose ptp = programItem.GetComponent <PlaceToPose>();
                    SendInterfaceStateChangeToROS(ptp.programItem, block.Key);
                    currentProgramItem = ptp.programItem;
                    ptp.Run();
                    yield return(new WaitWhile(() => ptp.IsRunning() && !next && !previous));

                    if (next)
                    {
                        ptp.OnNextInstruction();
                        next = false;
                    }
                    //if (previous) {
                    //    ptp.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending place to pose");
                    break;

                case "DRILL_POINTS":
                    Debug.Log("Starting drill points");
                    DrillPoints dp = programItem.GetComponent <DrillPoints>();
                    SendInterfaceStateChangeToROS(dp.programItem, block.Key);
                    currentProgramItem = dp.programItem;
                    dp.Run();
                    yield return(new WaitWhile(() => dp.IsRunning() && !next && !previous));

                    if (next)
                    {
                        dp.OnNextInstruction();
                        next = false;
                    }
                    //if (previous) {
                    //    dp.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending drill points");
                    break;

                case "GET_READY":
                    Debug.Log("Starting get ready");
                    GetReady gr = programItem.GetComponent <GetReady>();
                    SendInterfaceStateChangeToROS(gr.programItem, block.Key);
                    currentProgramItem = gr.programItem;
                    gr.Run();
                    yield return(new WaitWhile(() => gr.IsRunning() && !next && !previous));

                    if (next)
                    {
                        gr.OnNextInstruction();
                        next = false;
                    }
                    //if (previous) {
                    //    gr.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending get ready");
                    break;

                case "WAIT_UNTIL_USER_FINISHES":
                    Debug.Log("Starting wait until user finishes");
                    WaitUntilUserFinishes wuuf = programItem.GetComponent <WaitUntilUserFinishes>();
                    SendInterfaceStateChangeToROS(wuuf.programItem, block.Key);
                    currentProgramItem = wuuf.programItem;
                    wuuf.Run();
                    yield return(new WaitWhile(() => wuuf.IsRunning() && !next && !previous));

                    if (next)
                    {
                        wuuf.OnNextInstruction();
                        next = false;
                    }
                    //if (previous) {
                    //    wuuf.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending wait until user finishes");
                    break;

                case "VISUAL_INSPECTION":
                    Debug.Log("Starting visual inspection");
                    VisualInspection vs = programItem.GetComponent <VisualInspection>();
                    SendInterfaceStateChangeToROS(vs.programItem, block.Key);
                    currentProgramItem = vs.programItem;
                    vs.Run();
                    yield return(new WaitWhile(() => vs.IsRunning() && !next && !previous));

                    if (next)
                    {
                        vs.OnNextInstruction();
                        next = false;
                    }
                    //if (previous) {
                    //    vs.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending visual inspection");
                    break;

                case "PLACE_TO_CONTAINER":
                    Debug.Log("Starting place to container");
                    PlaceToContainer ptc = programItem.GetComponent <PlaceToContainer>();
                    SendInterfaceStateChangeToROS(ptc.programItem, block.Key);
                    currentProgramItem = ptc.programItem;
                    ptc.Run();
                    yield return(new WaitWhile(() => ptc.IsRunning() && !next && !previous));

                    if (next)
                    {
                        ptc.OnNextInstruction();
                        next = false;
                    }
                    //if (previous) {
                    //    ptc.OnPreviousInstruction();
                    //    i = GetCorrectIndexOfPreviousInstruction(i);
                    //    previous = false;
                    //}
                    Debug.Log("Ending place to container");
                    break;

                default:
                    break;
                }

                //end program execution
                if (on_successID == 0)
                {
                    break;
                }

                //load next ID into currentID
                currentID = on_successID;

                //try to load nex on_successID
                if (!programOrderHelper.TryGetValue(currentID, out on_successID))
                {
                    break;
                }
            }
        }

        //speechManager.Say("Visualization of block " + interfaceStateMsg.GetBlockID().ToString() + " in program " + interfaceStateMsg.GetProgramID().ToString() + " ended.");
        visualization_running = false;
        start_visualization   = false;
        replay_visualization  = false;
        visualize_program     = false;
        visualize_block       = false;
        VisualizationManager.Instance.VisualizationInProcess(false);
        //ClearProgram();
        Debug.Log("Visualization of program " + interfaceStateMsg.GetProgramID().ToString() + " ended.");
    }
Пример #2
0
    private IEnumerator BuildBlock(ProgramBlockMsg programBlock)
    {
        ushort blockID = programBlock.GetID();
        Dictionary <UInt16, GameObject> programToVisualize = new Dictionary <UInt16, GameObject>();
        Dictionary <UInt16, UInt16>     programOrderHelper = new Dictionary <UInt16, UInt16>();

        foreach (var programItem in programBlock.GetProgramItems())
        {
            Debug.Log(programItem.GetType());
            switch (programItem.GetIType())
            {
            case ProgramTypes.PICK_FROM_POLYGON:
                //1. moznost jak zjistit rozmery aktualniho objektu .. nutno ale pockat na odpoved
                //2. moznost bude to napevno naprat do kodu
                ROSCommunicationManager.Instance.ros.CallService("/art/db/object_type/get", "{\"name\": \"" + programItem.GetObject()[0] + "\"}");
                //wait until ROS sends object dimensions.. az podminka nabude true, pujde se dal
                yield return(new WaitUntil(() => ObjectsManager.Instance.ObjectIsKnown(programItem.GetObject()[0])));

                GameObject pickFromPolygonInstr = new GameObject();
                pickFromPolygonInstr.AddComponent <PickFromPolygon>();
                pickFromPolygonInstr.transform.parent = gameObject.transform;
                pickFromPolygonInstr.tag = "PICK_FROM_POLYGON";

                PickFromPolygon pfg = pickFromPolygonInstr.GetComponent <PickFromPolygon>();
                pfg.programItem           = programItem;
                pfg.visualizeObjectPrefab = visualizeObjectPrefab;

                programToVisualize.Add(programItem.GetID(), pickFromPolygonInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());

                Debug.Log("PICK_FROM_POLYGON Instruction created");

                break;

            case ProgramTypes.PICK_FROM_FEEDER:
                Debug.Log("Building PICK_FROM_FEEDER");
                GameObject pickFromFeederInstr = new GameObject();
                pickFromFeederInstr.AddComponent <PickFromFeeder>();
                pickFromFeederInstr.transform.parent = gameObject.transform;
                pickFromFeederInstr.tag = "PICK_FROM_FEEDER";

                PickFromFeeder pff = pickFromFeederInstr.GetComponent <PickFromFeeder>();
                pff.programItem           = programItem;
                pff.visualizeObjectPrefab = visualizeObjectPrefab;

                //if ref id references some previously created instruction Pick From Feeder, then add aditional info to ProgramMsg (copy it from referenced instruction)
                if (programItem.GetRefID().Count > 0)
                {
                    if (programToVisualize.ContainsKey(programItem.GetRefID()[0]))
                    {
                        if (programToVisualize[programItem.GetRefID()[0]].tag == "PICK_FROM_FEEDER")
                        {
                            Debug.Log("Instantiating existing PICK_FROM_FEEDER");
                            PickFromFeeder refInstr = programToVisualize[programItem.GetRefID()[0]].GetComponent <PickFromFeeder>();
                            pff.programItem = new ProgramItemMsg(pff.programItem.GetID(), pff.programItem.GetOnSuccess(), pff.programItem.GetOnFailure(), pff.programItem.GetIType(), pff.programItem.GetName(),
                                                                 refInstr.programItem.GetObject(), refInstr.programItem.GetPose(), refInstr.programItem.GetPolygon(), pff.programItem.GetRefID(), pff.programItem.GetFlags(),
                                                                 pff.programItem.GetDoNotClear(), pff.programItem.GetLabels());

                            ROSCommunicationManager.Instance.ros.CallService("/art/db/object_type/get", "{\"name\": \"" + pff.programItem.GetObject()[0] + "\"}");
                            yield return(new WaitUntil(() => ObjectsManager.Instance.ObjectIsKnown(pff.programItem.GetObject()[0])));
                        }
                    }
                }


                programToVisualize.Add(programItem.GetID(), pickFromFeederInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());

                Debug.Log("PICK_FROM_FEEDER Instruction created");

                break;

            case ProgramTypes.PLACE_TO_POSE:
                GameObject placeToPoseInstr = new GameObject();
                placeToPoseInstr.AddComponent <PlaceToPose>();
                placeToPoseInstr.transform.parent = gameObject.transform;
                placeToPoseInstr.tag = "PLACE_TO_POSE";

                PlaceToPose ptp = placeToPoseInstr.GetComponent <PlaceToPose>();
                ptp.programItem = programItem;

                if (programItem.GetRefID().Count > 0)
                {
                    if (programToVisualize.ContainsKey(programItem.GetRefID()[0]))
                    {
                        ptp.referenceItem = programToVisualize[programItem.GetRefID()[0]];
                    }
                }

                programToVisualize.Add(programItem.GetID(), placeToPoseInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());

                Debug.Log("PLACE_TO_POSE Instruction created");
                break;

            case ProgramTypes.DRILL_POINTS:
                ROSCommunicationManager.Instance.ros.CallService("/art/db/object_type/get", "{\"name\": \"" + programItem.GetObject()[0] + "\"}");
                yield return(new WaitUntil(() => ObjectsManager.Instance.ObjectIsKnown(programItem.GetObject()[0])));

                GameObject drillPointsInstr = new GameObject();
                drillPointsInstr.AddComponent <DrillPoints>();
                drillPointsInstr.transform.parent = gameObject.transform;
                drillPointsInstr.tag = "DRILL_POINTS";

                DrillPoints dp = drillPointsInstr.GetComponent <DrillPoints>();
                dp.programItem           = programItem;
                dp.visualizeObjectPrefab = visualizeObjectPrefab;

                programToVisualize.Add(programItem.GetID(), drillPointsInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());

                Debug.Log("DRILL_POINTS Instruction created");
                break;

            case ProgramTypes.GET_READY:
                GameObject getReadyInstr = new GameObject();
                getReadyInstr.AddComponent <GetReady>();
                getReadyInstr.transform.parent = gameObject.transform;
                getReadyInstr.tag = "GET_READY";

                GetReady gr = getReadyInstr.GetComponent <GetReady>();
                gr.programItem = programItem;

                programToVisualize.Add(programItem.GetID(), getReadyInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());
                Debug.Log("GET_READY Instruction created");
                break;

            case ProgramTypes.WAIT_UNTIL_USER_FINISHES:
                GameObject waitUntilUserFinishesInstr = new GameObject();
                waitUntilUserFinishesInstr.AddComponent <WaitUntilUserFinishes>();
                waitUntilUserFinishesInstr.transform.parent = gameObject.transform;
                waitUntilUserFinishesInstr.tag = "WAIT_UNTIL_USER_FINISHES";

                WaitUntilUserFinishes wuuf = waitUntilUserFinishesInstr.GetComponent <WaitUntilUserFinishes>();
                wuuf.programItem = programItem;

                programToVisualize.Add(programItem.GetID(), waitUntilUserFinishesInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());
                Debug.Log("WAIT_UNTIL_USER_FINISHES Instruction created");
                break;

            case ProgramTypes.VISUAL_INSPECTION:
                GameObject visualInspectionInstr = new GameObject();
                visualInspectionInstr.AddComponent <VisualInspection>();
                visualInspectionInstr.transform.parent = gameObject.transform;
                visualInspectionInstr.tag = "VISUAL_INSPECTION";

                VisualInspection vs = visualInspectionInstr.GetComponent <VisualInspection>();
                vs.programItem = programItem;

                if (programItem.GetRefID().Count > 0)
                {
                    if (programToVisualize.ContainsKey(programItem.GetRefID()[0]))
                    {
                        vs.referenceItem = programToVisualize[programItem.GetRefID()[0]];
                    }
                }

                programToVisualize.Add(programItem.GetID(), visualInspectionInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());

                Debug.Log("VISUAL_INSPECTION Instruction created");
                break;

            case ProgramTypes.PLACE_TO_CONTAINER:
                GameObject placeToContainerInstr = new GameObject();
                placeToContainerInstr.AddComponent <PlaceToContainer>();
                placeToContainerInstr.transform.parent = gameObject.transform;
                placeToContainerInstr.tag = "PLACE_TO_CONTAINER";

                PlaceToContainer ptc = placeToContainerInstr.GetComponent <PlaceToContainer>();
                ptc.programItem = programItem;

                if (programItem.GetRefID().Count > 0)
                {
                    if (programToVisualize.ContainsKey(programItem.GetRefID()[0]))
                    {
                        ptc.referenceItem = programToVisualize[programItem.GetRefID()[0]];
                    }
                }

                programToVisualize.Add(programItem.GetID(), placeToContainerInstr);
                programOrderHelper.Add(programItem.GetID(), programItem.GetOnSuccess());

                Debug.Log("PLACE_TO_CONTAINER Instruction created");
                break;

            default:
                break;
            }
        }

        program.Add(blockID, programToVisualize);
        programHelper.Add(blockID, programOrderHelper);

        yield return(null);
    }