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.");
}
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);
}
