private void SendInterfaceStateChangeToROS(ProgramItemMsg programItemMsg, UInt16 blockID)
{
InterfaceStateMsg msg = new InterfaceStateMsg("PROJECTED UI", interfaceStateMsg.GetSystemState(), interfaceStateMsg.GetTimestamp(),
interfaceStateMsg.GetProgramID(), blockID, programItemMsg, interfaceStateMsg.GetFlags(), interfaceStateMsg.GetEditEnabled(),
interfaceStateMsg.GetErrorSeverity(), interfaceStateMsg.GetErrorCode());
ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), msg);
}
public static bool CheckIfInterfaceStateChanged(InterfaceStateMsg currentState, InterfaceStateMsg newState)
{
//Debug.Log("CHECKING INTERFACE STATE CHANGE");
//Debug.Log("old_state: " + currentState.ToYAMLString());
//Debug.Log("new_state: " + newState.ToYAMLString());
return(!(currentState.GetSystemState() == newState.GetSystemState() &&
currentState.GetProgramID() == newState.GetProgramID() &&
currentState.GetBlockID() == newState.GetBlockID() &&
currentState.GetEditEnabled() == newState.GetEditEnabled() &&
currentState.GetProgramCurrentItem().ToYAMLString().Equals(newState.GetProgramCurrentItem().ToYAMLString())));
}
// Update is called once per frame
void Update()
{
if (SystemStarter.Instance.calibrated)
{
if (interfaceStateMsg != null)
{
//state learning
if (interfaceStateMsg.GetSystemState() == InterfaceStateMsg.SystemState.STATE_LEARNING)
{
//TODO: kontrola jestli se interfaceState zmenil..
if (interfaceStateChanged)
{
Debug.Log("InterfaceState changed!");
//already handled current interface state
interfaceStateChanged = false;
}
}
}
}
}
// 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;
}
}
}
}
private void InterfaceStateChanged(InterfaceStateMsg interfaceStateMsg)
{
if (SystemStarter.Instance.calibrated)
{
if (CurrentState != ProgrammingManagerState.def)
{
VisualizationClear();
}
Debug.Log("NEW INTERFACE STATE! " + interfaceStateMsg.ToYAMLString());
if (interfaceStateMsg.GetSystemState() == InterfaceStateMsg.SystemState.STATE_LEARNING)
{
switch (interfaceStateMsg.GetProgramCurrentItem().GetIType())
{
case ProgramTypes.PICK_FROM_FEEDER:
PickFromFeederIP.Instance.SetInterfaceStateMsgFromROS(interfaceStateMsg);
if (interfaceStateMsg.GetEditEnabled() && holoLearningEnabled)
{
CurrentState = ProgrammingManagerState.pick_from_feeder_learn;
PickFromFeederIP.Instance.StartLearning();
followedLearningPlacePoseOverride = true;
}
else if (!interfaceStateMsg.GetEditEnabled() && !followedLearningPlacePoseOverride)
{
CurrentState = ProgrammingManagerState.pick_from_feeder_vis;
PickFromFeederIP.Instance.Visualize();
}
break;
case ProgramTypes.PLACE_TO_POSE:
PlaceToPoseIP.Instance.SetInterfaceStateMsgFromROS(interfaceStateMsg);
if (interfaceStateMsg.GetEditEnabled() && holoLearningEnabled && followedLearningPlacePoseOverride &&
CurrentState != ProgrammingManagerState.place_to_pose_learn_followed)
{
CurrentState = ProgrammingManagerState.place_to_pose_learn_followed;
PlaceToPoseIP.Instance.StartLearningContinuous();
//followedLearningPlacePoseOverride = false;
}
else if (interfaceStateMsg.GetEditEnabled() && holoLearningEnabled && !followedLearningPlacePoseOverride &&
CurrentState != ProgrammingManagerState.place_to_pose_learn)
{
CurrentState = ProgrammingManagerState.place_to_pose_learn;
PlaceToPoseIP.Instance.StartLearning();
}
else if (interfaceStateMsg.GetEditEnabled() && holoLearningEnabled &&
(CurrentState == ProgrammingManagerState.place_to_pose_learn_followed || CurrentState == ProgrammingManagerState.place_to_pose_learn))
{
PlaceToPoseIP.Instance.UpdatePlacePoseFromROS(ROSUnityCoordSystemTransformer.ConvertVector(interfaceStateMsg.GetProgramCurrentItem().GetPose()[0].GetPose().GetPosition().GetPoint()),
ROSUnityCoordSystemTransformer.ConvertQuaternion(interfaceStateMsg.GetProgramCurrentItem().GetPose()[0].GetPose().GetOrientation().GetQuaternion()));
}
else if (!interfaceStateMsg.GetEditEnabled() && !followedLearningPlacePoseOverride)
{
CurrentState = ProgrammingManagerState.place_to_pose_vis;
PlaceToPoseIP.Instance.Visualize();
}
else if (!interfaceStateMsg.GetEditEnabled() && CurrentState == ProgrammingManagerState.place_to_pose_learn_followed)
{
CurrentState = ProgrammingManagerState.place_to_pose_vis;
followedLearningPlacePoseOverride = false;
PlaceToPoseIP.Instance.Visualize();
}
break;
default:
CurrentState = ProgrammingManagerState.def;
break;
}
}
else
{
CurrentState = ProgrammingManagerState.def;
followedLearningPlacePoseOverride = false;
}
Debug.Log(CurrentState);
}
}
// 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() == "PickFromPolygon" &&
interfaceStateMsg.GetEditEnabled() == true)
{
//if no object is selected
if (programItemMsg.GetObject().Count == 0)
{
//if no objects are placed on the table
//TODO: pokud je objekt detekovan v podavaci.. pocita se.. nemel by.. zkusit kontrolovat aj z souradnici
if (!ObjectsManager.Instance.AnyObjectIsOnTable() && !sayPlaceObject)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_NoObjectsOnTable);
sayPlaceObject = true;
//in case that user removes, places and removes objects from the table
saySelectObject = false;
}
//some object is placed on the table
if (ObjectsManager.Instance.AnyObjectIsOnTable())
{
if (!saySelectObject)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_SelectObjectType);
saySelectObject = true;
//in case that user removes objects from the table
sayPlaceObject = false;
//init pointing hands
objectsOnTable = ObjectsManager.Instance.GetObjectsFromTable();
SpawnAndRunPointingHands();
}
}
sayReward = true;
}
//if some object is selected
else if (programItemMsg.GetObject().Count > 0)
{
StopAndDestroyPointingHands();
if (sayReward)
{
//TextToSpeechManager.Instance.Speak(Texts.Perfect);
sayReward = false;
}
if (!sayAdjustArea)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_AdjustPickArea);
sayAdjustArea = true;
}
//show hand and play it's animation
if (!pointingHand.activeSelf && !animationShowed)
{
//pointingHand.SetActive(true);
originalPolygonMsg = programItemMsg.GetPolygon()[0].GetPolygon();
// 4-------3
// | |
// | |
// 1-------2
//load first point of polygon
Vector3 firstPoint = new Vector3(programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[0].GetX(),
-programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[0].GetY(),
programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[0].GetZ());
Vector3 secondPoint = new Vector3(programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[1].GetX(),
-programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[1].GetY(),
programItemMsg.GetPolygon()[0].GetPolygon().GetPoints()[1].GetZ());
//get middle point of bottom line
spawnPoint = firstPoint + (secondPoint - firstPoint) / 2 + new Vector3(0f, -0.05f, 0f);
movePoint = spawnPoint + new Vector3(0f, 0.15f, 0f);
//pointingHand.transform.localPosition = spawnPoint;
pointingHand.GetComponent <PointingHandMover>().Run(spawnPoint, movePoint);
}
//if polygon points are same, then user didn't moved with it.. so play hand animation
if (originalPolygonMsg.ToYAMLString().Equals(programItemMsg.GetPolygon()[0].GetPolygon().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
instructionProgrammed = true;
}
}
//reset all variables
else
{
if (instructionProgrammed)
{
//TextToSpeechManager.Instance.Speak(Texts.PickFromPolygonIE_GoodJob);
instructionProgrammed = false;
pointingHand.GetComponent <PointingHandMover>().Stop();
}
//if user ended programming without doing anything
if (pointingHandsList.Count > 0)
{
StopAndDestroyPointingHands();
}
sayReward = false;
sayPlaceObject = false;
saySelectObject = false;
sayAdjustArea = false;
animationShowed = false;
}
}
}
}
// Update is called once per frame
void Update()
{
if (SystemStarter.Instance.calibrated)
{
if (interfaceStateMsg != null)
{
if (interfaceStateMsg.GetSystemState() == InterfaceStateMsg.SystemState.STATE_VISUALIZE)
{
if (hololensStateMsg != null)
{
if (hololensStateMsg.GetHololensState() == hololens_state.STATE_VISUALIZING)
{
//sets current visualization state for program manager
ProgramManager.Instance.SetVisualizationState(hololensStateMsg.GetVisualizationState());
switch (hololensStateMsg.GetVisualizationState())
{
case visualization_state.VISUALIZATION_DISABLED:
Debug.Log("VISUALIZATION_DISABLED");
//when 'back to blocks' pressed
ProgramManager.Instance.ClearProgram();
hololensStateMsg = null;
visualization_running = false;
break;
case visualization_state.VISUALIZATION_RUN:
if (!visualization_running)
{
Debug.Log("VISUALIZATION_RUN");
ProgramManager.Instance.StartVisualization(interfaceStateMsg, hololensStateMsg.GetVisualizeWholeProgram());
//call for service to load current program.. response comes in ROSCommunicationManager's method ServiceCallBack()
ROSCommunicationManager.Instance.ros.CallService("/art/db/program/get", "{\"id\": " + interfaceStateMsg.GetProgramID() + "}");
visualization_running = true;
visualization_stopped = false;
hololensStateMsg = null;
}
break;
case visualization_state.VISUALIZATION_PAUSE:
if (visualization_running)
{
Debug.Log("VISUALIZATION_PAUSE");
Time.timeScale = 0;
visualization_running = false;
hololensStateMsg = null;
}
break;
case visualization_state.VISUALIZATION_RESUME:
if (!visualization_running)
{
Debug.Log("VISUALIZATION_RESUME");
Time.timeScale = 1;
visualization_running = true;
hololensStateMsg = null;
}
break;
case visualization_state.VISUALIZATION_STOP:
if (!visualization_stopped)
{
Debug.Log("VISUALIZATION_STOP");
Time.timeScale = 1;
ProgramManager.Instance.StopVisualization();
visualization_running = false;
visualization_stopped = true;
hololensStateMsg = null;
}
break;
case visualization_state.VISUALIZATION_REPLAY:
if (!visualization_running)
{
Debug.Log("VISUALIZATION_REPLAY");
ProgramManager.Instance.ReplayVisualization();
visualization_running = true;
visualization_stopped = false;
hololensStateMsg = null;
}
break;
}
}
}
}
}
}
}