//called when user says "Resume"
public void OnResume()
{
//resume can be called only from PAUSE state
if (visualizationState == visualization_state.VISUALIZATION_PAUSE)
{
ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), new InterfaceStateMsg("PROJECTED UI", interfaceStateMsg.GetSystemState(), interfaceStateMsg.GetTimestamp(),
interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), currentProgramItem, new List <KeyValueMsg>()
{
new KeyValueMsg("HOLOLENS_VISUALIZATION", "PAUSE")
},
interfaceStateMsg.GetEditEnabled(), interfaceStateMsg.GetErrorSeverity(), interfaceStateMsg.GetErrorCode()));
}
else if (visualizationState == visualization_state.VISUALIZATION_RUN || visualizationState == visualization_state.VISUALIZATION_RESUME ||
visualizationState == visualization_state.VISUALIZATION_REPLAY)
{
TextToSpeechManager.Instance.Speak(Texts.Vis_YouHaveToPauseFirst);
}
else if (visualizationState == visualization_state.VISUALIZATION_STOP)
{
TextToSpeechManager.Instance.Speak(Texts.Vis_NotEvenRunning);
}
else
{
TextToSpeechManager.Instance.Speak(Texts.Vis_NothingToResume);
}
}
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);
}
// 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);
}
}
//called when user says "Stop"
public void OnStop()
{
//stop can be called from all states except STOP state
if (visualizationState == visualization_state.VISUALIZATION_RUN || visualizationState == visualization_state.VISUALIZATION_RESUME ||
visualizationState == visualization_state.VISUALIZATION_REPLAY || visualizationState == visualization_state.VISUALIZATION_PAUSE)
{
ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), new InterfaceStateMsg("PROJECTED UI", interfaceStateMsg.GetSystemState(), interfaceStateMsg.GetTimestamp(),
interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), currentProgramItem, new List <KeyValueMsg>()
{
new KeyValueMsg("HOLOLENS_VISUALIZATION", "STOP")
},
interfaceStateMsg.GetEditEnabled(), interfaceStateMsg.GetErrorSeverity(), interfaceStateMsg.GetErrorCode()));
}
else if (visualizationState == visualization_state.VISUALIZATION_STOP)
{
TextToSpeechManager.Instance.Speak(Texts.Vis_VisualizationAlreadyStopped);
}
else
{
TextToSpeechManager.Instance.Speak(Texts.Vis_ThereIsNothingToStop);
}
}
public IEnumerator SaveToROS()
{
ROSActionHelper.OnLearningActionResult += OnLearningActionResult;
//save instruction to ROS
Debug.Log("PLACE TO POSE SAVED");
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(PlacePosition)),
new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(PlaceOrientation))))
},
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);
yield return(new WaitForSecondsRealtime(0.1f));
currentTime = ROSTimeHelper.GetCurrentTime();
currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.DONE, ROSTimeHelper.ToSec(currentTime));
LearningRequestActionGoalMsg requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""),
new GoalIDMsg(currentTime, 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));
ROSActionHelper.OnLearningActionResult -= OnLearningActionResult;
//if(InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride) {
// InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride = false;
// InteractiveProgrammingManager.Instance.CurrentState = InteractiveProgrammingManager.ProgrammingManagerState.place_to_pose_vis;
// Visualize();
//}
yield return(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);
}
