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())));
}
public void SetInterfaceStateMsgFromROS(InterfaceStateMsg msg)
{
interfaceStateMsg = msg;
programItemMsg = msg.GetProgramCurrentItem();
}
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);
}
}