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