// Update is called once per frame
public void UpdatePlacePose(Vector3 placePosition, Quaternion placeOrientation)
{
if (ROSUnityCoordSystemTransformer.AlmostEqual(currentPlacePosition, placePosition, 0.001f) &&
ROSUnityCoordSystemTransformer.AlmostEqual(currentPlaceOrientation, placeOrientation, 0.001f))
{
return;
}
currentPlacePosition = placePosition;
currentPlaceOrientation = placeOrientation;
if (learning)
{
TimeMsg currentTime = ROSTimeHelper.GetCurrentTime();
InterfaceStateMsg msg = new InterfaceStateMsg(
"HOLOLENS",
InterfaceStateMsg.SystemState.STATE_LEARNING,
currentTime,
interfaceStateMsg.GetProgramID(),
interfaceStateMsg.GetBlockID(),
new ProgramItemMsg(
programItemMsg.GetID(),
programItemMsg.GetOnSuccess(),
programItemMsg.GetOnFailure(),
programItemMsg.GetIType(),
programItemMsg.GetName(),
new List <string>(),
new List <PoseStampedMsg>()
{
new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"),
new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(currentPlacePosition)),
new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(currentPlaceOrientation))))
},
new List <PolygonStampedMsg>(),
programItemMsg.GetRefID(),
programItemMsg.GetFlags(),
programItemMsg.GetDoNotClear(),
programItemMsg.GetLabels()),
interfaceStateMsg.GetFlags(),
true,
InterfaceStateMsg.ErrorSeverity.NONE,
InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN);
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 (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;
}
}
}
// 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 IEnumerator SaveToROS()
{
robotArm.GetComponent <PR2GripperController>().SetArmActive(false);
ROSActionHelper.OnLearningActionResult += OnLearningActionResult;
Debug.Log("PICK FROM FEEDER SAVED");
//Save parameters for PICK_FROM_FEEDER
TimeMsg currentTime = ROSTimeHelper.GetCurrentTime();
InterfaceStateMsg i_msg = new InterfaceStateMsg(
"HOLOLENS",
InterfaceStateMsg.SystemState.STATE_LEARNING,
currentTime,
interfaceStateMsg.GetProgramID(),
interfaceStateMsg.GetBlockID(),
new ProgramItemMsg(
programItemMsg.GetID(),
programItemMsg.GetOnSuccess(),
programItemMsg.GetOnFailure(),
programItemMsg.GetIType(),
programItemMsg.GetName(),
new List <string>()
{
objectTypeToPick
},
new List <PoseStampedMsg>()
{
new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"),
new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(robotArmPosition)),
new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(robotArmRotation))))
},
new List <PolygonStampedMsg>(),
programItemMsg.GetRefID(),
programItemMsg.GetFlags(),
programItemMsg.GetDoNotClear(),
programItemMsg.GetLabels()),
interfaceStateMsg.GetFlags(),
true,
InterfaceStateMsg.ErrorSeverity.NONE,
InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN);
ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), i_msg);
yield return(new WaitForSecondsRealtime(0.1f));
//Request brain for goal DONE
currentTime = ROSTimeHelper.GetCurrentTime();
currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.DONE, ROSTimeHelper.ToSec(currentTime));
LearningRequestActionGoalMsg requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""),
new GoalIDMsg(ROSTimeHelper.GetCurrentTime(), currentRequestID),
new LearningRequestGoalMsg(learning_request_goal.DONE));
ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg);
waiting_for_action_response = true;
//Wait for action server response
yield return(new WaitWhile(() => waiting_for_action_response == true));
Debug.Log("SERVER RESPONDED!");
yield return(new WaitForSecondsRealtime(0.1f));
//Switch to next instruction (should be PLACE_TO_POSE)
//InteractiveProgrammingManager.Instance.PlaceToPoseLearningOverride = true;
currentTime = ROSTimeHelper.GetCurrentTime();
i_msg = new InterfaceStateMsg(
"HOLOLENS",
InterfaceStateMsg.SystemState.STATE_LEARNING,
currentTime,
interfaceStateMsg.GetProgramID(),
interfaceStateMsg.GetBlockID(),
ProgramHelper.GetProgramItemById(interfaceStateMsg.GetBlockID(), programItemMsg.GetOnSuccess()),
interfaceStateMsg.GetFlags(),
false,
InterfaceStateMsg.ErrorSeverity.NONE,
InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN);
ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), i_msg);
yield return(new WaitForSecondsRealtime(0.1f));
//Request brain for goal GET_READY for learning PLACE_TO_POSE
currentTime = ROSTimeHelper.GetCurrentTime();
currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.GET_READY_WITHOUT_ROBOT, ROSTimeHelper.ToSec(currentTime));
requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""),
new GoalIDMsg(currentTime, currentRequestID),
new LearningRequestGoalMsg(learning_request_goal.GET_READY_WITHOUT_ROBOT));
ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg);
waiting_for_action_response = true;
//Wait for action server response
yield return(new WaitWhile(() => waiting_for_action_response == true));
Debug.Log("SERVER RESPONDED!");
yield return(new WaitForSecondsRealtime(0.1f));
ROSActionHelper.OnLearningActionResult -= OnLearningActionResult;
yield return(null);
//robotArm.transform.parent = world_anchor.transform;
robotArm.GetComponent <FollowTransform>().enabled = false;
robotArm.GetComponent <PR2GripperController>().CloseGripperInstantly();
robotArm.GetComponent <PR2GripperController>().PlaceGripperToInit();
robotArm.GetComponent <PR2GripperController>().SetCollidersActive(true);
robotArm.GetComponent <PR2GripperController>().SetArmActive(false);
spatial_mapping.SetActive(true);
}
// 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;
}
}
}
}
}
}
}
public static void SetInterfaceStateMsgFromROS(InterfaceStateMsg msg)
{
if (CheckIfInterfaceStateChanged(interfaceStateMsg, msg))
{
//Debug.Log("PH interface state changed");
//Debug.Log(interfaceStateMsg.GetProgramID() != msg.GetProgramID());
//Debug.Log(interfaceStateMsg.GetProgramID());
//Debug.Log(msg.GetProgramID());
//load new program if current has changed
if (LoadProgram)
{
LoadingProgram = true;
ROSCommunicationManager.Instance.ros.CallService(ROSCommunicationManager.programGetService, "{\"id\": " + msg.GetProgramID() + "}");
}
interfaceStateMsg = msg;
//if currently loading program.. call the action after program was successfully loaded (e.g. in SetProgramMsgFromROS())
if (OnInterfaceStateChanged != null && !LoadingProgram)
{
OnInterfaceStateChanged(interfaceStateMsg);
}
}
}