/// <summary>
/// Update the list returned by <code>GetCachedADFList</code>.
/// </summary>
/// <returns>Returns TANGO_SUCCESS on success, or TANGO_ERROR on failure to retrieve the list.</returns>
public static int RefreshADFList()
{
int returnValue = Common.ErrorType.TANGO_ERROR;
IntPtr tempData = IntPtr.Zero;
returnValue = PoseProviderAPI.TangoService_getAreaDescriptionUUIDList(ref tempData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".RefreshADFList() Could not get ADF list from device.");
}
else
{
byte[] charBuffer = new byte[sizeof(char)];
List <byte> dataHolder = new List <byte>();
Marshal.Copy(tempData, charBuffer, 0, 1);
while (charBuffer[0] != 0 && charBuffer[0] != '\n')
{
dataHolder.Add(charBuffer[0]);
tempData = new IntPtr(tempData.ToInt64() + 1);
Marshal.Copy(tempData, charBuffer, 0, 1);
}
string uuidList = System.Text.Encoding.UTF8.GetString(dataHolder.ToArray());
m_adfList.PopulateUUIDList(uuidList);
if (!m_adfList.HasEntries())
{
Debug.Log(CLASS_NAME + ".RefreshADFList() No area description files found on device.");
}
}
return(returnValue);
}
/// <summary>
/// Takes care of importing a adf file from the specified path. Important: make sure that the filepath
/// does not contain ADF files already present on device, otherwise it will return an error, as duplicates
/// can't be imported.
/// </summary>
/// <returns><c>Common.ErrorType.TANGO_SUCCESS</c> if the UUID was imported successfully.</returns>
/// <param name="adfID">The <c>UUIDUnityHolder</c> object that will contain information about the retrieved ADF.</param>
/// <param name="filePath">File path containing the ADF we want to export.</param>
public static int ImportAreaDescriptionFromFile(UUIDUnityHolder adfID, string filePath)
{
if (adfID == null)
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR,
CLASS_NAME +
".ImportAreaDescription() Could not import area description. UUID Holder object specified is not initialized");
return(Common.ErrorType.TANGO_ERROR);
}
IntPtr uuidHolder = Marshal.AllocHGlobal(Common.UUID_LENGTH);
int returnValue = PoseProviderAPI.TangoService_importAreaDescription(filePath, uuidHolder);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR,
CLASS_NAME + ".ImportAreaDescription() Could not import area description at path: " + filePath);
}
else
{
byte[] tempDataBuffer = new byte[Common.UUID_LENGTH];
Marshal.Copy(uuidHolder, tempDataBuffer, 0, Common.UUID_LENGTH);
adfID.SetDataUUID(tempDataBuffer);
}
return(returnValue);
}
/// <summary>
/// Populates the Metadata key/value pairs of a given metadataPointer.
///
/// metaDataPointer should be initialized to a valid Metadata by calling the getAreaDescriptionMetaData().
/// </summary>
/// <returns>TANGO_SUCCESS if successful, else TANGO_INVALID or TANGO_ERROR.</returns>
/// <param name="metadataPointer">Metadata pointer.</param>
/// <param name="keyValuePairs">Dictionary of key/value pairs stored in the metadata.</param>
public static int PopulateAreaDescriptionMetaDataKeyValues(IntPtr metadataPointer, ref Dictionary <string, string> keyValuePairs)
{
IntPtr keyList = IntPtr.Zero;
if (metadataPointer == IntPtr.Zero)
{
Debug.Log(CLASS_NAME + "metadata pointer is null, cannot save metadata to this ADF!");
return(Common.ErrorType.TANGO_ERROR);
}
int returnValue = PoseProviderAPI.TangoAreaDescriptionMetadata_listKeys(metadataPointer, ref keyList);
if (returnValue != Tango.Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log("Could not read metadata keys list");
}
string metadataKeys = Marshal.PtrToStringAuto(keyList);
string[] keys = metadataKeys.Split(new char[] { ',' });
string[] values = new string[keys.Length];
for (int i = 0; i < values.Length; i++)
{
uint valuesize = 0;
IntPtr valuePointer = IntPtr.Zero;
PoseProviderAPI.TangoAreaDescriptionMetadata_get(metadataPointer, keys[i], ref valuesize, ref valuePointer);
byte[] valueByteArray = new byte[valuesize];
Marshal.Copy(valuePointer, valueByteArray, 0, (int)valuesize);
values[i] = System.Text.Encoding.UTF8.GetString(valueByteArray);
Debug.Log("Key Values are- " + keys[i] + ": " + values[i]);
keyValuePairs.Add(keys[i], values[i]);
}
return(returnValue);
}
/// <summary>
/// Saves an area description to device based on the UUID object contained in the adfID object holder.
/// </summary>
/// <returns><c>Common.ErrorType.TANGO_SUCCESS</c> if saving was successfull.</returns>
/// <param name="adfID">The UUIDUnityHolder object that contains the desired UUID object.</param>
public static int SaveAreaDescription(UUIDUnityHolder adfUnityHolder)
{
// is learning mode on
// are we localized?
if (adfUnityHolder == null)
{
Debug.Log(CLASS_NAME + ".SaveAreaDescription() Could not save area description. UUID Holder object specified is not initialized");
return(Common.ErrorType.TANGO_ERROR);
}
IntPtr idData = Marshal.AllocHGlobal(Common.UUID_LENGTH);
int returnValue = PoseProviderAPI.TangoService_saveAreaDescription(idData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".SaveAreaDescripton() Could not save area description with ID: " + adfUnityHolder.GetStringDataUUID());
}
else
{
byte[] tempDataBuffer = new byte[Common.UUID_LENGTH];
Marshal.Copy(idData, tempDataBuffer, 0, Common.UUID_LENGTH);
adfUnityHolder.SetDataUUID(tempDataBuffer);
}
return(returnValue);
}
/// <summary>
/// Saves the metadata associated with a single area description unique ID.
/// </summary>
/// <returns>
/// Returns TANGO_SUCCESS on successful save, or TANGO_ERROR on failure, or if the service needs to be
/// initialized.
/// </returns>
/// <param name="adfUnityHolder">The metadata and associated UUID to save.</param>
public static int SaveAreaDescriptionMetaData(UUIDUnityHolder adfUnityHolder)
{
if (adfUnityHolder == null)
{
Debug.Log(CLASS_NAME + ".SaveAreaDescription() Could not save area description. UUID Holder object specified is not initialized");
return(Common.ErrorType.TANGO_ERROR);
}
if (string.IsNullOrEmpty(adfUnityHolder.GetStringDataUUID()))
{
Debug.Log(CLASS_NAME + ".MetaData cannot be retrived for the area description as UUIDUnityHolder object was empty or null.");
return(Common.ErrorType.TANGO_ERROR);
}
if (adfUnityHolder.uuidMetaData.meta_data_pointer == IntPtr.Zero)
{
Debug.Log(CLASS_NAME + "metadata pointer is null, cannot save metadata to this ADF!");
return(Common.ErrorType.TANGO_ERROR);
}
Debug.Log("UUID being saved is: " + adfUnityHolder.GetStringDataUUID());
int returnValue = PoseProviderAPI.TangoService_saveAreaDescriptionMetadata(adfUnityHolder.GetStringDataUUID(), adfUnityHolder.uuidMetaData.meta_data_pointer);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + "Could not save metadata to the ADF!");
}
return(returnValue);
}
/// <summary>
/// Sets the listener coordinate frame pairs.
/// </summary>
/// <param name="count">Count.</param>
/// <param name="frames">Frames.</param>
public static void SetListenerCoordinateFramePairs(int count,
ref TangoCoordinateFramePair frames)
{
int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames(count, ref frames);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs");
}
}
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
#if UNITY_EDITOR
poseData.framePair = framePair;
bool pairIsValid = true;
Matrix4x4 baseToDevice;
Matrix4x4 targetToDevice;
double adjustedTimeStamp1;
double adjustedTimeStamp2;
bool a = !GetFrameToDeviceTransformation(framePair.baseFrame, timeStamp, out adjustedTimeStamp1, out baseToDevice);
bool b = !GetFrameToDeviceTransformation(framePair.targetFrame, timeStamp, out adjustedTimeStamp2, out targetToDevice);
if (a || b)
{
pairIsValid = false;
}
Matrix4x4 baseToTarget = baseToDevice * targetToDevice.inverse;
Quaternion rotation = Quaternion.LookRotation(baseToTarget.GetColumn(2), baseToTarget.GetColumn(1));
poseData.translation[0] = baseToTarget.m03;
poseData.translation[1] = baseToTarget.m13;
poseData.translation[2] = baseToTarget.m23;
poseData.orientation[0] = rotation.x;
poseData.orientation[1] = rotation.y;
poseData.orientation[2] = rotation.z;
poseData.orientation[3] = rotation.w;
if (pairIsValid)
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
}
else
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
Debug.Log(string.Format(
CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}",
timeStamp, framePair.baseFrame, framePair.targetFrame));
}
// Let most recent timestamp involved in the transformation be the timestamp
// (relevant when using GetPoseAtTime(0)).
// Behaviour may need to be updated after implmenting Area Description emulation.
poseData.timestamp = System.Math.Max(adjustedTimeStamp1, adjustedTimeStamp2);
#else
int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
#endif
}
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
}
/// <summary>
/// Sets the listener coordinate frame pairs.
/// </summary>
/// <param name="count">Count.</param>
/// <param name="frames">Frames.</param>
public static void SetListenerCoordinateFramePairs(int count,
ref TangoCoordinateFramePair frames)
{
int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames(count, ref frames);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR,
CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs");
}
}
/// <summary>
/// Set the C callback for the Tango pose interface.
/// </summary>
/// <param name="framePairs">Passed in to the C API.</param>
/// <param name="callback">Callback method.</param>
internal static void SetCallback(TangoCoordinateFramePair[] framePairs, TangoService_onPoseAvailable callback)
{
int returnValue = PoseProviderAPI.TangoService_connectOnPoseAvailable(framePairs.Length, framePairs, callback);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".SetCallback() Callback was not set!");
}
else
{
Debug.Log(CLASS_NAME + ".SetCallback() OnPose callback was set!");
}
}
/// <summary>
/// Deletes an area description with the specified unique ID.
/// </summary>
/// <returns>
/// Returns TANGO_SUCCESS if area description file with specified unique ID is found and can be removed.
/// </returns>
/// <param name="toDeleteUUID">The area description to delete.</param>
public static int DeleteAreaDescription(string toDeleteUUID)
{
if (string.IsNullOrEmpty(toDeleteUUID))
{
Debug.Log(CLASS_NAME + ".DeleteAreaDescription() Could not delete area description, UUID was empty or null.");
return(Common.ErrorType.TANGO_ERROR);
}
int returnValue = PoseProviderAPI.TangoService_deleteAreaDescription(toDeleteUUID);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".DeleteAreaDescription() Could not delete area description, API returned invalid.");
}
return(returnValue);
}
/// <summary>
/// Sets the callback to be used when a new Pose is
/// presented by the Tango Service.
/// </summary>
/// <param name="callback">Callback.</param>
public static void SetCallback(TangoCoordinateFramePair[] framePairs, TangoService_onPoseAvailable callback)
{
int returnValue = PoseProviderAPI.TangoService_connectOnPoseAvailable(framePairs.Length, framePairs, callback);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR,
CLASS_NAME + ".SetCallback() Callback was not set!");
}
else
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_INFO,
CLASS_NAME + ".SetCallback() OnPose callback was set!");
}
}
/// <summary>
/// Gets the metadata handle associated with a single area description unique ID.
/// </summary>
/// <returns>
/// Returns TANGO_SUCCESS on successful load of metadata, or TANGO_ERROR if the service needs to be initialized
/// or if the metadata could not be loaded.
/// </returns>
/// <param name="adfUnityHolder">
/// The TangoUUID for which to load the metadata. On success, this function sets the pointer to raw UUID
/// metadata which can then be extracted using AreaDescriptionMetaData_get, AreaDescriptionMetaData_get, or
/// PopulateAreaDescriptionMetaDataKeyValues.
/// </param>
public static int GetAreaDescriptionMetaData(UUIDUnityHolder adfUnityHolder)
{
if (string.IsNullOrEmpty(adfUnityHolder.GetStringDataUUID()))
{
Debug.Log(CLASS_NAME + ".MetaData cannot be retrived for the area description as UUIDUnityHolder object was empty or null.");
return(Common.ErrorType.TANGO_ERROR);
}
int returnValue = PoseProviderAPI.TangoService_getAreaDescriptionMetadata(adfUnityHolder.GetStringDataUUID(), ref adfUnityHolder.uuidMetaData.meta_data_pointer);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + "Meta Data could not be loaded");
}
Debug.Log("GetAreaDescription return value is: " + returnValue.ToString());
return(returnValue);
}
/// <summary>
/// Set the value of a key in a metadata.
/// </summary>
/// <returns>TANGO_SUCCESS if successful, else TANGO_INVALID or TANGO_ERROR.</returns>
/// <param name="key">Key to set the value of.</param>
/// <param name="value">Value to set.</param>
/// <param name="adfUnityHolder">Area description + metadata holder.</param>
public static int AreaDescriptionMetaData_set(String key, String value, UUIDUnityHolder adfUnityHolder)
{
if (string.IsNullOrEmpty(adfUnityHolder.GetStringDataUUID()))
{
Debug.Log(CLASS_NAME + ".MetaData cannot be retrived for the area description as UUIDUnityHolder object was empty or null.");
return(Common.ErrorType.TANGO_ERROR);
}
int returnValue = PoseProviderAPI.TangoAreaDescriptionMetadata_set(adfUnityHolder.uuidMetaData.meta_data_pointer, key, (uint)value.Length, value);
if (returnValue != Tango.Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log("Could not set Metadata Key, Error return value is: " + returnValue);
return(returnValue);
}
else
{
Debug.Log("Metadata Set succesful, Key set is: " + key + " Value set is: " + value);
return(returnValue);
}
}
/// <summary>
/// Export an area with the UUID from the default area storage location to the destination file directory with
/// the UUID as its name.
/// </summary>
/// <returns>Returns TANGO_SUCCESS if the file was exported, or TANGO_ERROR if the export failed.</returns>
/// <param name="uuid">The UUID of the area.</param>
/// <param name="filePath">The destination file directory.</param>
public static int ExportAreaDescriptionToFile(string uuid, string filePath)
{
if (string.IsNullOrEmpty(uuid))
{
Debug.Log("Can't export an empty UUID. Please define one.");
return(Common.ErrorType.TANGO_ERROR);
}
if (string.IsNullOrEmpty(filePath))
{
Debug.Log("Missing file path for exporting area description. Please define one.");
return(Common.ErrorType.TANGO_ERROR);
}
int returnValue = PoseProviderAPI.TangoService_exportAreaDescription(uuid, filePath);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".ExportAreaDescription() Could not export area description: " + uuid +
" with path: " + filePath);
}
return(returnValue);
}
/// <summary>
/// Takes care of saving a ADF file to the specified folder.
/// </summary>
/// <returns><c>Common.ErrorType.TANGO_SUCCESS</c> if the ADF file was exported successfully.</returns>
/// <param name="UUID">The UUID of the ADF file we want to export.</param>
/// <param name="filePath">File path where we want to export the ADF.</param>
public static int ExportAreaDescriptionToFile(string UUID, string filePath)
{
if (string.IsNullOrEmpty(UUID))
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR, "Can't export an empty UUID. Please define one.");
return(Common.ErrorType.TANGO_ERROR);
}
if (string.IsNullOrEmpty(filePath))
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR, "Missing file path for exporting area description. Please define one.");
return(Common.ErrorType.TANGO_ERROR);
}
int returnValue = PoseProviderAPI.TangoService_exportAreaDescription(UUID, filePath);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR,
CLASS_NAME + ".ExportAreaDescription() Could not export area description: " + UUID +
" with path: " + filePath);
}
return(returnValue);
}
/// <summary>
/// Get the value of a key from a metadata.
/// </summary>
/// <returns>TANGO_SUCCESS if successful, else TANGO_INVALID or TANGO_ERROR.</returns>
/// <param name="key">Key to lookup.</param>
/// <param name="value">On success, the value for that key.</param>
/// <param name="adfUnityHolder">Area description + metadata holder.</param>
public static int AreaDescriptionMetaData_get(String key, ref String value, UUIDUnityHolder adfUnityHolder)
{
if (string.IsNullOrEmpty(adfUnityHolder.GetStringDataUUID()))
{
Debug.Log(CLASS_NAME + ".MetaData cannot be retrived for the area description as UUIDUnityHolder object was empty or null.");
return(Common.ErrorType.TANGO_ERROR);
}
uint valuesize = 0;
IntPtr valuePointer = IntPtr.Zero;
int returnValue = PoseProviderAPI.TangoAreaDescriptionMetadata_get(adfUnityHolder.uuidMetaData.meta_data_pointer, key, ref valuesize, ref valuePointer);
if (returnValue != Tango.Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log("Could not read metadata key, Error return value is: " + returnValue);
return(returnValue);
}
else
{
byte[] valueByteArray = new byte[valuesize];
Marshal.Copy(valuePointer, valueByteArray, 0, (int)valuesize);
value = System.Text.Encoding.UTF8.GetString(valueByteArray);
return(returnValue);
}
}
/// <summary>
/// Resets the motion tracking system.
///
/// This reinitializes the <c>TANGO_COORDINATE_FRAME_START_OF_SERVICE</c> coordinate frame to where the
/// device is when you call this function; afterwards, if you ask for the pose with relation to start of
/// service, it uses this as the new origin. You can call this function at any time.
///
/// If you are using Area Learning, the <c>TANGO_COORDINATE_FRAME_AREA_DESCRIPTION</c> coordinate frame
/// is not affected by calling this function; however, the device needs to localize again before you can use
/// the area description.
/// </summary>
public static void ResetMotionTracking()
{
PoseProviderAPI.TangoService_resetMotionTracking();
}
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
#if UNITY_EDITOR
bool baseFrameIsWorld =
framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
bool targetFrameIsWorld =
framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
// Area Descriptions are explicitly not supported yet.
if (framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION ||
framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() emulation does not support Area Descriptions.");
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
poseData.timestamp = 0;
poseData.translation[0] = Vector3.zero.x;
poseData.translation[1] = Vector3.zero.y;
poseData.translation[2] = Vector3.zero.z;
poseData.orientation[0] = Quaternion.identity.x;
poseData.orientation[1] = Quaternion.identity.y;
poseData.orientation[2] = Quaternion.identity.z;
poseData.orientation[3] = Quaternion.identity.w;
return;
}
if (baseFrameIsWorld && !targetFrameIsWorld)
{
if (timeStamp == 0)
{
float poseTimestamp;
Vector3 posePosition;
Quaternion poseRotation;
if (GetTangoEmulationCurrent(out poseTimestamp, out posePosition, out poseRotation))
{
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
poseData.timestamp = poseTimestamp;
poseData.translation[0] = posePosition.x;
poseData.translation[1] = posePosition.y;
poseData.translation[2] = posePosition.z;
poseData.orientation[0] = poseRotation.x;
poseData.orientation[1] = poseRotation.y;
poseData.orientation[2] = poseRotation.z;
poseData.orientation[3] = poseRotation.w;
return;
}
}
else
{
Vector3 posePosition;
Quaternion poseRotation;
if (GetTangoEmulationAtTime((float)timeStamp, out posePosition, out poseRotation))
{
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
poseData.timestamp = timeStamp;
poseData.translation[0] = posePosition.x;
poseData.translation[1] = posePosition.y;
poseData.translation[2] = posePosition.z;
poseData.orientation[0] = poseRotation.x;
poseData.orientation[1] = poseRotation.y;
poseData.orientation[2] = poseRotation.z;
poseData.orientation[3] = poseRotation.w;
return;
}
}
}
else if (baseFrameIsWorld == targetFrameIsWorld)
{
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
poseData.timestamp = timeStamp;
poseData.translation[0] = Vector3.zero.x;
poseData.translation[1] = Vector3.zero.y;
poseData.translation[2] = Vector3.zero.z;
poseData.orientation[0] = Quaternion.identity.x;
poseData.orientation[1] = Quaternion.identity.y;
poseData.orientation[2] = Quaternion.identity.z;
poseData.orientation[3] = Quaternion.identity.w;
return;
}
Debug.Log(string.Format(
CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}",
timeStamp, framePair.baseFrame, framePair.targetFrame));
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
poseData.timestamp = 0;
poseData.translation[0] = Vector3.zero.x;
poseData.translation[1] = Vector3.zero.y;
poseData.translation[2] = Vector3.zero.z;
poseData.orientation[0] = Quaternion.identity.x;
poseData.orientation[1] = Quaternion.identity.y;
poseData.orientation[2] = Quaternion.identity.z;
poseData.orientation[3] = Quaternion.identity.w;
#else
int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
#endif
}
