//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;
}
}
}
}
