//TODO cannot jump by index, but through dictionary //private int GetCorrectIndexOfPreviousInstruction(int i) { // //1st decrement - replay current instruction // //2nd decrement - replay previous instruction // //3rd decrement - only if replayed instruction was place to pose // //skip first instruction // if (i > 0) { // i--; // //if instruction before should be PLACE_TO_POSE than go one more instruction backwards to have something to place (PickFromFeeder or PickFromPolygon) // //it would be nonsense to replay place to pose when robot has nothing in gripper to place // if (programToVisualize.ElementAt(i).Value.gameObject.tag.Equals("PLACE_TO_POSE")) { // i--; // } // } // //if it was first instruction - replay it // //if it was any other instruction - replay previous instruction // i--; // return i; //} //clears all variables and destroyes program instructions.. should be called only when "Back to blocks" pressed public void ClearProgram() { foreach (var block in program.Values) { foreach (var programItem in block.Values) { Destroy(programItem); } } program.Clear(); programHelper.Clear(); programMsg = null; programBlockMsg = null; }
public ProgramBlockMsg GetBlockByID(UInt16 id) { ProgramBlockMsg searchedBlock = null; foreach (ProgramBlockMsg block in _blocks) { if (block.GetID() == id) { searchedBlock = block; break; } } return(searchedBlock); }
// Update is called once per frame void Update() { if (SystemStarter.Instance.calibrated) { if (start_visualization && !visualization_running) { if (programMsg != null) { if (interfaceStateMsg.GetProgramID() == programMsg.GetHeader().GetID()) { if (visualize_program) { Debug.Log("VISUALIZING PROGRAM " + interfaceStateMsg.GetProgramID()); buildingProgram = true; StartCoroutine(BuildProgram()); runProgramCoroutine = StartCoroutine(RunProgram()); visualization_running = true; } else if (visualize_block) { programBlockMsg = GetProgramBlock(); if (programBlockMsg != null) { Debug.Log("VISUALIZING PROGRAM " + interfaceStateMsg.GetProgramID() + " and its block " + programBlockMsg.GetID()); buildingProgram = true; StartCoroutine(BuildJustBlock()); runProgramCoroutine = StartCoroutine(RunProgram()); visualization_running = true; } } } } } else if (replay_visualization && !visualization_running) { Debug.Log("REPLAYING PROGRAM " + interfaceStateMsg.GetProgramID()); runProgramCoroutine = StartCoroutine(RunProgram()); visualization_running = true; } } }
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); }
// Update is called once per frame void Update() { if (SystemStarter.Instance.calibrated) { if (interfaceStateMsg != null) { //pick from polygon editing if (interfaceStateMsg.GetSystemState() == InterfaceStateMsg.SystemState.STATE_LEARNING && programItemMsg.GetIType() == "PlaceToPose" && interfaceStateMsg.GetEditEnabled() == true) { //check that object type is set if (programMsg == null && !serviceCalled) { ROSCommunicationManager.Instance.ros.CallService("/art/db/program/get", "{\"id\": " + interfaceStateMsg.GetProgramID() + "}"); serviceCalled = true; } else if (programMsg != null && !objTypeReferenceSet) { ProgramBlockMsg programBlockMsg = programMsg.GetBlockByID(interfaceStateMsg.GetBlockID()); ProgramItemMsg refItem = programBlockMsg.GetProgramItemByID(programItemMsg.GetRefID()[0]); if (refItem.GetObject().Count == 0 && !sayUnknownObjectType) { //TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_PickIsNotProgrammed); sayUnknownObjectType = true; } else if (refItem.GetObject().Count > 0) { objTypeReferenceSet = true; } } //if object type is set if (!sayAdjustArea && objTypeReferenceSet) { //TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_DragOBjectOutline); sayAdjustArea = true; } //show hand and play it's animation if (!pointingHand.activeSelf && !animationShowed) { //pointingHand.SetActive(true); originalPoseMsg = programItemMsg.GetPose()[0].GetPose(); //get middle point of bottom line spawnPoint = new Vector3(programItemMsg.GetPose()[0].GetPose().GetPosition().GetX(), -programItemMsg.GetPose()[0].GetPose().GetPosition().GetY(), programItemMsg.GetPose()[0].GetPose().GetPosition().GetZ()); //ARTABLE BUG - place pose not actualizing interface state.. initially set to 0.. if so, set spawn point on the middle of the table (where it appears) if (spawnPoint.Equals(new Vector3(0f, 0f, 0f))) { spawnPoint = spawnPointOnTable; } movePoint = spawnPoint + new Vector3(0f, 0.15f, 0f); //pointingHand.transform.localPosition = spawnPoint; pointingHand.GetComponent <PointingHandMover>().Run(spawnPoint, movePoint); } //if pose points are same, then user didn't moved with it.. so play hand animation if (originalPoseMsg.ToYAMLString().Equals(programItemMsg.GetPose()[0].GetPose().ToYAMLString())) { //pointingHand.transform.localPosition = Vector3.Lerp(pointingHand.transform.localPosition, movePoint, Time.deltaTime * 1.5f); //if (ObjectInPosition(pointingHand, movePoint, 0.0005f)) { // pointingHand.transform.localPosition = spawnPoint; //} } else { //pointingHand.SetActive(false); pointingHand.GetComponent <PointingHandMover>().Stop(); animationShowed = true; } //check if everything is set for this instruction if (objTypeReferenceSet && programItemMsg.GetPose()[0].GetPose()._position.GetX() != 0.0f) { instructionProgrammed = true; } } //reset all variables else { if (instructionProgrammed) { //TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_GoodJob); instructionProgrammed = false; pointingHand.GetComponent <PointingHandMover>().Stop(); } //if just object type reference is set but not place pose else if (objTypeReferenceSet) { //TextToSpeechManager.Instance.Speak(Texts.PlaceToPoseIE_ForgotPlacePose); pointingHand.GetComponent <PointingHandMover>().Stop(); } sayAdjustArea = false; sayUnknownObjectType = false; objTypeReferenceSet = false; serviceCalled = false; animationShowed = false; programMsg = null; } } } }