void Update()
{
if (!ShowLine)
{
return;
}
// Set the input position
// Find the Camera object
GameObject marker = GameObject.Find("ARUWP Controller");
// access the ARCamera script of Camera
ARUWPMarker markerHiro = marker.GetComponent <ARUWPMarker>();
// read the transformation matrix
Matrix4x4 myPose = markerHiro.transMatrix;
// read pivot calibration from txt file
Vector4 tipMarker;
tipMarker = new Vector4((float)(-3.276183 / 100.0), (float)(-187.381409 / 100.0), (float)(2.641751 / 100.0), 1);
// multiply the tip position with the transformation
Vector4 tipHoloLens;
tipHoloLens = myPose * tipMarker;
// convert Vector4 tipMarker to 3D
EndPosition.x = tipHoloLens.x;
EndPosition.y = tipHoloLens.y;
EndPosition.z = tipHoloLens.z;
// For now we don't know the start position yet, should come from classifier
Vector3 startPosition = new Vector3(0, 0, 0);
// if no target position yet, means that the merker is not seen, no rendering
// endPosition is in fact the position of the tool tip
if (EndPosition == null)
{
seenMarker = false;
}
// if the marker could be seen, render
if (seenMarker)
{
LineRenderer lineRenderer = GetComponent <LineRenderer>();
// if we don't have any result from point cloud classifier, make one for now
StartPosition.x = (float)(EndPosition.x);
StartPosition.y = (float)(EndPosition.y + 0.03);
StartPosition.z = (float)(EndPosition.z);
// lineRenderer.transform.position = startPosition;
lineRenderer.useWorldSpace = true;
// make a vector of Vector3 for SetPositions
var points = new Vector3[2];
points[0] = StartPosition;
points[1] = EndPosition;
lineRenderer.SetPositions(points);
}
}
// Update is called once per frame
void Update()
{
// Load the markers if not done yet and the controller is ready
if (LoadedMarkers == false && _controller.status == ARUWP.ARUWP_STATUS_RUNNING)
{
Debug.Log("Size of markers from controller:" + ARUWPController.markers.Count);
foreach (var key in ARUWPController.markers.Keys)
{
ARUWPMarker marker = ARUWPController.markers[key];
if (marker.type == ARUWPMarker.MarkerType.single_barcode && marker.target != null)
{
// Get the barcode ID, target GameObject
if (DictBarcodeFileNames.ContainsKey(marker.singleBarcodeID))
{
string fName = DictBarcodeFileNames[marker.singleBarcodeID];
GameObject gObj = marker.target;
//gObj.SetActive(false);
// Load the material for reference (they share the common material)
if (marker.singleBarcodeID == 0)
{
MeshRenderer meshRenderer = gObj.GetComponentInChildren <MeshRenderer>();
RefPage.SetPrintedMaterial(meshRenderer.sharedMaterial);
Debug.Log("Found printed item material");
// Debugging
//gObj.SetActive(true);
//_refPrintedMaterial.color = new Color(_refPrintedMaterial.color.r, _refPrintedMaterial.color.g, _refPrintedMaterial.color.b, 0.1f);
}
if (marker.singleBarcodeID >= 52)
{
// Sticker
gObj.SetActive(true);
}
else if (marker.singleBarcodeID >= 41)
{
// Board
gObj.SetActive(true);
DictBarcodeToARUWPMarker.Add(marker.singleBarcodeID, key);
}
else
{
// Other files
gObj.SetActive(false);
TitlePage.PreloadTags(fName);
}
DictPrintedDocObjects.Add(fName, gObj);
Debug.Log("Found the GameObject for:" + fName);
}
else
{
Debug.Log("Error: barcode not found:" + marker.singleBarcodeID);
}
}
}
LoadedMarkers = true;
}
}
// Use this for initialization
void Start()
{
tcpManager = GetComponent <TCPManager>();
aruwpMarker = GetComponent <ARUWPMarker>();
holoLensCamera = GameObject.Find("HoloLensCamera");
robotCoordinateOrientationToUnityMatrix.SetRow(0, new Vector4(1, 0, 0, 0));
robotCoordinateOrientationToUnityMatrix.SetRow(1, new Vector4(0, -1, 0, 0));
robotCoordinateOrientationToUnityMatrix.SetRow(2, new Vector4(0, 0, 1, 0));
robotCoordinateOrientationToUnityMatrix.SetRow(3, new Vector4(0, 0, 0, 1));
dummyGameObject = new GameObject("Dummy");
dummyGameObject.transform.SetParent(holoLensCamera.transform);
findForceVisualObjects();
findTFCoordinateVisualObjects();
for (int i = 0; i < TCPManager.TCPPackageConstants.TOTAL_NUM_TF; i++)
{
_tfDataArr[i] = new TCPManager.tfData();
robotPoseToHololensMatrices[i] = new Matrix4x4();
}
mediaMaterial = previewPlane.GetComponent <MeshRenderer>().material;
// params: width, height,
mediaTexture = new Texture2D(image_width, image_height, TextureFormat.BGRA32, false);
mediaMaterial.mainTexture = mediaTexture;
sliderX = forceBarGraph.transform.Find("BarX").GetComponent <Slider>();
sliderY = forceBarGraph.transform.Find("BarY").GetComponent <Slider>();
sliderZ = forceBarGraph.transform.Find("BarZ").GetComponent <Slider>();
left_gripper_force_target.SetActive(isShowingForceVisual);
left_gripper_target.SetActive(isShowingLeftGripper);
tf_coordindates_target.SetActive(isShowingTFCoordinates);
previewPlane.SetActive(isShowingCameraView);
warningSign.SetActive(false);
plannerCanvas.SetActive(false);
left_gripper_target.SetActive(false);
actionListCanvas.SetActive(false);
forceBarGraph.SetActive(false);
}
public void Init(ref RosSharp.RosBridgeClient.RosConnector rosConnector)
{
this.rosConnector = rosConnector;
publisher = new RosSharp.RosBridgeClient.NonMono.Publisher <RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped>(ref this.rosConnector, "/hololens/" + Config.MarkerPose);
GameObject aruwpController = GameObject.Find("ARUWPController");
if (aruwpController == null)
{
Debug.Log("MarkerPosePublisher: GameObject ARUWPController could not be found!");
Application.Quit();
}
else
{
marker = aruwpController.GetComponent <ARUWPMarker>();
}
if (marker == null)
{
Debug.Log("MarkerPosePublisher: GameObject ARUWPController has no ARUWPMarker component!");
Application.Quit();
}
}
// Update is called once per frame
void Update()
{
if (UseExternalCamera)
{
//********************* Initial Method that takes everything form RGBD Camera *************************************
// link the received data
DataTransfer = GameObject.Find("DataTransfer");
ReceiveData receivedData = DataTransfer.GetComponent <ReceiveData>();
// Obtain the received data, which are start and end positions wrt RGBD camera
RGBDStartPosition3 = receivedData.StartPosition;
RGBDEndPosition3 = receivedData.EndPosition;
MarkerToRGBD = receivedData.MarkerToCamera;
//Debug.Log("Start Position wrt RGBD is: \n" + RGBDStartPosition3.x.ToString() + " " + RGBDStartPosition3.y.ToString() + " " + RGBDStartPosition3.z.ToString());
//Debug.Log("End Position wrt RGBD is: \n" + RGBDEndPosition3.x.ToString() + " " + RGBDEndPosition3.y.ToString() + " " + RGBDEndPosition3.z.ToString());
// link the transformation obtained within HoloLens
ARUWPController = GameObject.Find("ARUWP Controller");
ARUWPMarker trackedMarker = ARUWPController.GetComponent <ARUWPMarker>();
// Obtain the tracking done by HoloLens
MarkerToHOLO = trackedMarker.transMatrix;
//Debug.Log("Transformation from Marker to HoloLens is: \n" + MarkerToHOLO.ToString());
// Now we've collected what we need
// Calculate the transformation from RGBD to HoloLens - Note that it is To_T_From
// HOLO_T_RGBD = HOLO_T_MARKER * MARKER_T_RGBD = MarkerToHolo * RGBDToMarker = MarkerToHOLO * MarkerToRGBD.inverse
RGBDToHOLO = MarkerToHOLO * MarkerToRGBD.inverse;
// Then calculate the positions from wrt RGBD to wrt HoloLens
Vector4 RGBDStartPosition4 = new Vector4(RGBDStartPosition3.x, RGBDStartPosition3.y, RGBDStartPosition3.z, 1.0f);
Vector4 RGBDEndPosition4 = new Vector4(RGBDEndPosition3.x, RGBDEndPosition3.y, RGBDEndPosition3.z, 1.0f);
Vector4 HOLOStartPosition4 = RGBDToHOLO * RGBDStartPosition4;
Vector4 HOLOEndPosition4 = RGBDToHOLO * RGBDEndPosition4;
HOLOStartPosition = new Vector3(HOLOStartPosition4.x, HOLOStartPosition4.y, HOLOStartPosition4.z);
HOLOEndPosition = new Vector3(HOLOEndPosition4.x, HOLOEndPosition4.y, HOLOEndPosition4.z);
// Use HoloCalib to decrease the Holographic display offsets
if (UseHoloCalibration)
{
HOLOStartPosition = HoloCalib(HOLOStartPosition);
HOLOEndPosition = HoloCalib(HOLOEndPosition);
}
// Now we transfer the position from wrt to HoloLens to wrt world (local to world)
GameObject MainCamera = GameObject.Find("Main Camera");
Camera myCamera = MainCamera.GetComponent <Camera>();
HOLOStartPosition = myCamera.transform.TransformPoint(HOLOStartPosition);
HOLOEndPosition = myCamera.transform.TransformPoint(HOLOEndPosition);
//Debug.Log("Start Position wrt HoloLens is: \n" + HOLOStartPosition.x.ToString() + " " + HOLOStartPosition.y.ToString() + " " + HOLOStartPosition.z.ToString());
//Debug.Log("End Position wrt HoloLens is: \n" + HOLOEndPosition.x.ToString() + " " + HOLOEndPosition.y.ToString() + " " + HOLOEndPosition.z.ToString());
// Now work on linking the text
StartPosText = GameObject.Find("/ARDisplayHUD/HOLOStartPos").GetComponent <Text>();
EndPosText = GameObject.Find("/ARDisplayHUD/HOLOEndPos").GetComponent <Text>();
StartPosText.text = "Start Position: " + "[" + HOLOStartPosition.x.ToString() + ", " + HOLOStartPosition.y.ToString() + ", " + HOLOStartPosition.z.ToString() + "]";
EndPosText.text = "End Position: " + "[" + HOLOEndPosition.x.ToString() + " " + HOLOEndPosition.y.ToString() + " " + HOLOEndPosition.z.ToString() + "]";
//************************ Initial Method ends here ***************************************************************
}
else
{
// *********************** Alternative method - calculate the tracking by itself *************************************
// Local end position (tool tip) wrt to the marker, obtained from pivot caibration
Vector3 localEnd;
// below is for the pen model pivot calibration result
// localEnd.x = (float)(-3.276183 / 1000 /*+ 0.013*/);
// localEnd.y = (float)(-187.381409 / 1000 /*- 0.02*/);
// localEnd.z = (float)(2.641751 / 1000 /*- 0.035*/);
// Vector3 localStart;
// localStart.x = localEnd.x;
// localStart.y = (float)(localEnd.y + 0.12);
// localStart.z = localEnd.z;
// below is for the needle model pivot calibration result
localEnd.x = (float)(-4.7846 / 1000);
localEnd.y = (float)(-110.211241 / 1000);
localEnd.z = (float)(0.024813 / 1000);
// get the local start position
Vector3 localStart;
localStart.x = localEnd.x;
localStart.y = (float)(localEnd.y + 0.08);
localStart.z = localEnd.z;
// get the target from ARUWPMarker.cs
ARUWPController = GameObject.Find("ARUWP Controller");
ARUWPMarker trackedMarker = ARUWPController.GetComponent <ARUWPMarker>();
if (trackedMarker.visible)
{
//Debug.Log("The marker is visible");
// get the transformation from ARUWPMarker, which is the transformation from Marker to HoloLens
Matrix4x4 MarkerToHOLO = trackedMarker.transMatrix;
// transfer both local start and local end to HoloLens coordinate
Vector4 localStart4 = new Vector4(localStart.x, localStart.y, localStart.z, 1.0f);
Vector4 localEnd4 = new Vector4(localEnd.x, localEnd.y, localEnd.z, 1.0f);
Vector4 HoloStart4 = MarkerToHOLO * localStart4;
Vector4 HoloEnd4 = MarkerToHOLO * localEnd4;
// save to a Vector3
HOLOStartPosition = new Vector3(HoloStart4.x, HoloStart4.y, HoloStart4.z);
HOLOEndPosition = new Vector3(HoloEnd4.x, HoloEnd4.y, HoloEnd4.z);
// Use HoloCalib to decrease the Holographic display offsets
if (UseHoloCalibration)
{
HOLOStartPosition = HoloCalib(HOLOStartPosition);
HOLOEndPosition = HoloCalib(HOLOEndPosition);
}
// Now we transfer the position from wrt to HoloLens to wrt world (Holo to world)
GameObject MainCamera = GameObject.Find("Main Camera");
Camera myCamera = MainCamera.GetComponent <Camera>();
HOLOStartPosition = myCamera.transform.TransformPoint(HOLOStartPosition);
HOLOEndPosition = myCamera.transform.TransformPoint(HOLOEndPosition);
//Debug.Log("Transformation from Marker to HoloLens is: \n" + MarkerToHOLO.ToString());
//Debug.Log("Start Position wrt HoloLens is: \n" + HOLOStartPosition.x.ToString() + " " + HOLOStartPosition.y.ToString() + " " + HOLOStartPosition.z.ToString());
//Debug.Log("End Position wrt HoloLens is: \n" + HOLOEndPosition.x.ToString() + " " + HOLOEndPosition.y.ToString() + " " + HOLOEndPosition.z.ToString());
// Now work on linking the text
StartPosText = GameObject.Find("/ARDisplayHUD/HOLOStartPos").GetComponent <Text>();
EndPosText = GameObject.Find("/ARDisplayHUD/HOLOEndPos").GetComponent <Text>();
StartPosText.text = "Start Position: " + "[" + HOLOStartPosition.x.ToString() + ", " + HOLOStartPosition.y.ToString() + ", " + HOLOStartPosition.z.ToString() + "]";
EndPosText.text = "End Position: " + "[" + HOLOEndPosition.x.ToString() + " " + HOLOEndPosition.y.ToString() + " " + HOLOEndPosition.z.ToString() + "]";
}
}
}
// Update is called once per frame
void Update()
{
if (boardStatus != BoardStatus.COMPLETE && PageOrg && PageOrg.LoadedMarkers &&
PageOrganizer.DictBarcodeToARUWPMarker.ContainsKey(MarkerTopLeftID))
//&& PageOrganizer.DictBarcodeToARUWPMarker.ContainsKey(MarkerBotRightID))
{
//foreach (var key in ARUWPController.markers.Keys)
// Debug.Log("key:" + key);
//Debug.Log(PageOrganizer.DictBarcodeToARUWPMarker[MarkerTopLeftID] + "," + PageOrganizer.DictBarcodeToARUWPMarker[MarkerBotRightID]);
ARUWPMarker marker1 = ARUWPController.markers[PageOrganizer.DictBarcodeToARUWPMarker[MarkerTopLeftID]];
//ARUWPMarker marker2 = ARUWPController.markers[PageOrganizer.DictBarcodeToARUWPMarker[MarkerBotRightID]];
bool found1 = marker1.GetMarkerVisibility();
//bool found2 = marker2.GetMarkerVisibility();
//BoardHintText.text = marker1.GetMarkerVisibility() + "," + marker2.GetMarkerVisibility();
switch (boardStatus)
{
case BoardStatus.NOT_INIT:
if (found1)
{
// Ready for moving and further confirmation
//BoardBorder.SetActive(true);
boardImgMove.SetActive(true);
boardHintText.text = "Tap to confirm";
if (topLeftObj == null)
{
topLeftObj = marker1.target;
}
//AdaptBoardSize(NoteBoardWidth, NoteBoardHeight);
boardStatus = BoardStatus.MOVING;
Debug.Log("NOT_INIT -> MOVING");
}
break;
case BoardStatus.MOVING:
/*
* if (found2)
* {
* // Ready to adjust again
* // TODO: other visual elements.
* boardHintText.text = "Tap to confirm (resizing)";
* if (botRightObj == null)
* botRightObj = marker2.target;
* boardStatus = BoardStatus.BOTH_ON;
* Debug.Log("FIRST_ON -> BOTH_ON");
* }
* else if (found1)
* {
* // Adjust the position of the origin. Size untouched.
* gameObject.transform.position = topLeftObj.transform.position;
* gameObject.transform.rotation = topLeftObj.transform.rotation;
* gameObject.transform.Rotate(Vector3.left, 180);
* gameObject.transform.localEulerAngles = GetStableAngle(gameObject.transform.localEulerAngles);
*
* boardHintText.text = "Tap to confirm\n (Debug info:" + gameObject.transform.position + ")";
* // Reset the timer
* markerLostCounter = 3.0f;
* }
*/
if (found1)
{
markerLostCounter = 3.0f;
}
else
{
// 00: See if it is time to go back to NOT_INIT
markerLostCounter -= Time.deltaTime;
if (markerLostCounter <= 0)
{
//BoardBorder.SetActive(false);
boardHintText.text = "";
boardImgMove.SetActive(false);
boardStatus = BoardStatus.NOT_INIT;
Debug.Log("MOVING -> NOT_INIT due to time out.");
}
}
break;
/*
* case BoardStatus.BOTH_ON:
* if (found1 || found2)
* {
* float testDirection = Vector3.Dot(topLeftObj.transform.forward, botRightObj.transform.forward);
* // Test if the two labels are both facing to the same direction
* if (testDirection >= 0.9f)
* {
* // Accept and resize.
* Vector3 relativeBotRight = topLeftObj.transform.InverseTransformPoint(botRightObj.transform.position);
* currBoardWidth = Math.Abs(relativeBotRight.x);
* currBoardHeight = Math.Abs(relativeBotRight.y);
* boardHintText.fontSize = GetProperFont(currBoardWidth);
*
* gameObject.transform.position = topLeftObj.transform.position;
* gameObject.transform.rotation = topLeftObj.transform.rotation;
* gameObject.transform.Rotate(Vector3.left, 180);
* gameObject.transform.localEulerAngles = GetStableAngle(gameObject.transform.localEulerAngles);
*
* panelBoardObj.transform.localPosition = new Vector3(currBoardWidth / 2, -currBoardHeight / 2, 0);
* panelBoardObj.GetComponent<RectTransform>().sizeDelta = new Vector2(currBoardWidth, currBoardHeight);
* BoardBorder.GetComponent<RectTransform>().sizeDelta = new Vector2(currBoardWidth, currBoardHeight);
*
* boardHintText.text = string.Format("Tap to confirm\n(debug info: {0} x {1}, from {2})", currBoardWidth, currBoardHeight, topLeftObj.transform.position);
* }
* else
* boardHintText.text = "Tap to confirm\nDirection not aligned:" + testDirection;
* // Reset the timer
* markerLostCounter = 3.0f;
* }
* else
* {
* // 00: See if it is time to go back to NOT_INIT
* markerLostCounter -= Time.deltaTime;
* if (markerLostCounter <= 0)
* {
* BoardBorder.SetActive(false);
* boardStatus = BoardStatus.NOT_INIT;
* Debug.Log("BOTH_ON -> NOT_INIT due to time out.");
* }
* }
* break;
*
* case BoardStatus.COMPLETE:
* // Note: the user taps to transfer from MOVING/FIRST_ON/BOTH_ON to COMPLETE,
* // so this transition is not in this switch-logic.
* //if (found1)
* //{
* // // update location based on marker 1
* // gameObject.transform.position = topLeftObj.transform.position;
* // gameObject.transform.rotation = topLeftObj.transform.rotation;
* // gameObject.transform.Rotate(Vector3.left, 180);
* // gameObject.transform.localEulerAngles = GetStableAngle(gameObject.transform.localEulerAngles);
*
* // panelBoardObj.transform.localPosition = new Vector3(currBoardWidth / 2, -currBoardHeight / 2, 0);
*
* //}
* //else if (found2)
* //{
* // // update location based on marker 2
* // gameObject.transform.position = botRightObj.transform.position;
* // gameObject.transform.rotation = botRightObj.transform.rotation;
* // gameObject.transform.Rotate(Vector3.left, 180);
* // gameObject.transform.localEulerAngles = GetStableAngle(gameObject.transform.localEulerAngles);
* // // Move to correct position
* // gameObject.transform.Translate(new Vector3(-currBoardWidth, currBoardHeight, 0), Space.Self);
*
* // panelBoardObj.transform.localPosition = new Vector3(currBoardWidth / 2, -currBoardHeight / 2, 0);
* //}
* break;
*/
}
}
}
