public void Visualize()
{
if (ProgramHelper.ItemLearned(programItemMsg))
{
//TODO get reference ID of previous PICK and get ObjectType
ProgramItemMsg referenceItem = ProgramHelper.GetProgramItemById(interfaceStateMsg.GetBlockID(), programItemMsg.GetRefID()[0]);
Vector3 placePose = ROSUnityCoordSystemTransformer.ConvertVector(programItemMsg.GetPose()[0].GetPose().GetPosition().GetPoint());
Quaternion placeOrientation = ROSUnityCoordSystemTransformer.ConvertQuaternion(programItemMsg.GetPose()[0].GetPose().GetOrientation().GetQuaternion());
Vector3 objectDims = ObjectsManager.Instance.GetObjectTypeDimensions(referenceItem.GetObject()[0]);
//objectToPlaceUnmanipulatable = Instantiate(BasicObjectUnmanipulatablePrefab, world_anchor.transform);
//objectToPlaceUnmanipulatable.transform.localPosition = placePose;
//objectToPlaceUnmanipulatable.transform.localRotation = placeOrientation;
//objectToPlaceUnmanipulatable.transform.GetChild(0).transform.localScale = objectDims;
if (objectToPlace == null || objectToPlace.Equals(null))
{
objectToPlace = Instantiate(BasicObjectManipulatablePrefab, world_anchor.transform);
Debug.Log("Instantiating object");
}
objectToPlace.GetComponent <ObjectManipulationEnabler>().DisableManipulation();
objectToPlace.transform.localPosition = placePose;
objectToPlace.transform.localRotation = placeOrientation;
objectToPlace.transform.GetChild(0).transform.localScale = objectDims;
}
}
public void StartLearning()
{
learning = true;
Debug.Log("PLACE_TO_POSE STARTED LEARNING STANDALONE");
//if (objectToPlace == null) {
// objectToPlace = Instantiate(BasicObjectManipulatablePrefab, world_anchor.transform);
//}
Visualize();
UISoundManager.Instance.PlaySnap();
objectToPlace.GetComponent <ObjectManipulationEnabler>().EnableManipulation();
objectToPlace.transform.GetChild(0).GetComponent <Collider>().enabled = false;
objectToPlace.GetComponent <PlaceRotateConfirm>().snapLocalRotation = objectToPlace.transform.rotation;
objectToPlace.transform.parent = cursor.transform;
objectToPlace.transform.localPosition = new Vector3(0, 0, objectToPlace.transform.GetChild(0).transform.localScale.x / 2);
//objectToPlace.transform.localEulerAngles = new Vector3(0f, 90f, 0f);
ProgramItemMsg referenceItem = ProgramHelper.GetProgramItemById(interfaceStateMsg.GetBlockID(), programItemMsg.GetRefID()[0]);
objectToPlace.GetComponent <PlaceRotateConfirm>().Arm =
(ROSUnityCoordSystemTransformer.ConvertVector(referenceItem.GetPose()[0].GetPose().GetPosition().GetPoint()).x > MainMenuManager.Instance.currentSetup.GetTableWidth() / 2) ?
RobotHelper.RobotArmType.LEFT_ARM : RobotHelper.RobotArmType.RIGHT_ARM;
objectToPlace.GetComponent <PlaceRotateConfirm>().object_attached = true;
//objectToPlace.transform.localPosition = placePose;
//objectToPlace.transform.localRotation = placeOrientation;
//objectToPlace.transform.GetChild(0).transform.localScale = objectDims;
}
public void OnFocusEnter()
{
if (InteractiveProgrammingManager.Instance.CurrentState == InteractiveProgrammingManager.ProgrammingManagerState.pick_from_feeder_learn &&
FakeFeederObjectsPositions.CheckIfObjectIsInFeeder(type, ROSUnityCoordSystemTransformer.ConvertVector(position)))
{
wireframeMat.SetColor("_WireColor", blue);
PickFromFeederIP.Instance.StaringAtObject(true);
}
focus_stay = true;
}
// 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 void Visualize()
{
if (ProgramHelper.ItemLearned(programItemMsg))
{
Debug.Log("Visualizing PICK_FROM_FEEDER");
//if (objectToPickUnmanipulatable != null)
// Destroy(objectToPickUnmanipulatable);
//convert ros coordinate system to unity coordinate system
Vector3 robotArmPosition = ROSUnityCoordSystemTransformer.ConvertVector(programItemMsg.GetPose()[0].GetPose().GetPosition().GetPoint());
Quaternion robotArmRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(programItemMsg.GetPose()[0].GetPose().GetOrientation().GetQuaternion());
//show robot arm only if it's not grasping object
if (!RobotHelper.HasArmGraspedObject(robotArmPosition.x > MainMenuManager.Instance.currentSetup.GetTableWidth() / 2f ?
RobotHelper.RobotArmType.LEFT_ARM : RobotHelper.RobotArmType.RIGHT_ARM))
{
robotArm.transform.localPosition = robotArmPosition;
robotArm.transform.localRotation = robotArmRotation;
robotArm.GetComponent <PR2GripperController>().SetArmActive(true);
robotArm.GetComponent <PR2GripperController>().CloseGripperInstantly();
}
Vector3 objectPosition = FakeFeederObjectsPositions.GetObjectPositionInFeeder(programItemMsg.GetObject()[0],
(robotArmPosition.x > MainMenuManager.Instance.currentSetup.GetTableWidth() / 2f ? FakeFeederObjectsPositions.FeederType.right_feeder : FakeFeederObjectsPositions.FeederType.left_feeder));
Quaternion objectOrientation = FakeFeederObjectsPositions.GetObjectOrientationInFeeder(programItemMsg.GetObject()[0],
(robotArmPosition.x > MainMenuManager.Instance.currentSetup.GetTableWidth() / 2f ? FakeFeederObjectsPositions.FeederType.right_feeder : FakeFeederObjectsPositions.FeederType.left_feeder));
Vector3 objectDims = ObjectsManager.Instance.GetObjectTypeDimensions(programItemMsg.GetObject()[0]);
//objectToPickUnmanipulatable = Instantiate(BasicObjectUnmanipulatablePrefab, world_anchor.transform);
//objectToPickUnmanipulatable.transform.localPosition = ROSUnityCoordSystemTransformer.ConvertVector(objectPosition);
//objectToPickUnmanipulatable.transform.localRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(objectOrientation);
//objectToPickUnmanipulatable.transform.GetChild(0).transform.localScale = objectDims;
Debug.Log("BEFORE INSTANTIATING");
if (objectToPick == null || objectToPick.Equals(null))
{
Debug.Log("INSTANTIATING");
objectToPick = Instantiate(BasicObjectManipulatablePrefab, world_anchor.transform);
}
objectToPick.GetComponent <ObjectManipulationEnabler>().DisableManipulation();
objectToPick.transform.localPosition = ROSUnityCoordSystemTransformer.ConvertVector(objectPosition);
objectToPick.transform.localRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(objectOrientation);
objectToPick.transform.GetChild(0).transform.localScale = objectDims;
}
}
private GameObject InitObjectInFeeder(FakeFeederObjectsPositions.FeederType feederType, string objectType)
{
GameObject objectInFeeder = Instantiate(feederObjectPrefab, world_anchor.transform);
objectInFeeder.transform.localPosition = ROSUnityCoordSystemTransformer.ConvertVector(FakeFeederObjectsPositions.GetObjectPositionInFeeder(objectType, feederType));
objectInFeeder.transform.localRotation = ROSUnityCoordSystemTransformer.ConvertQuaternion(FakeFeederObjectsPositions.GetObjectOrientationInFeeder(objectType, feederType));
objectInFeeder.GetComponent <DetectedObject>().SetObject(
ROSUnityCoordSystemTransformer.ConvertVector(FakeFeederObjectsPositions.GetObjectPositionInFeeder(objectType, feederType)),
ROSUnityCoordSystemTransformer.ConvertQuaternion(FakeFeederObjectsPositions.GetObjectOrientationInFeeder(objectType, feederType)),
ObjectsManager.Instance.GetObjectTypeDimensions(objectType),
objectType,
0);
objectInFeeder.name = feederType + "_" + objectType;
objectInFeeder.tag = "feeder_object";
objectInFeeder.SetActive(false);
return(objectInFeeder);
}
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);
}
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);
}
}
public void OnInputClicked(InputClickedEventData eventData)
{
if (InteractiveProgrammingManager.Instance.CurrentState == InteractiveProgrammingManager.ProgrammingManagerState.pick_from_feeder_learn &&
FakeFeederObjectsPositions.CheckIfObjectIsInFeeder(GetComponent <DetectedObject>().type, ROSUnityCoordSystemTransformer.ConvertVector(GetComponent <DetectedObject>().position)))
{
PickFromFeederIP.Instance.MarkClickedArea(GetComponent <DetectedObject>());
UISoundManager.Instance.PlaySnap();
}
}
public void SetBaseLinkPosition(Vector3 position)
{
BaseLinkPosition = ROSUnityCoordSystemTransformer.ConvertVector(position);
}
