WorldAnalysisARFoundationModuleGeospatial.cs

#if UNITY_ANDROID && ETSIARF_ARCORE_EXTENSIONS
using System;
using System.Collections.Generic;
using Google.XR.ARCoreExtensions;
using Unity.XR.CoreUtils;
using UnityEngine;
using UnityEngine.XR.ARFoundation;
using UnityEngine.XR.ARSubsystems;
using static WorldAnalysisARFoundationModule;

public class WorldAnalysisARFoundationModuleGeospatial : WorldAnalysisARFoundationModule 
{
    /// <summary>
    ///  Anchor manager
    /// </summary>
    private ARAnchorManager m_anchorManager;

    /// <summary>
    /// Earth manager
    /// </summary>
    private AREarthManager m_arEarthManager;

    /// <summary>
    /// List of geospatial anchors with tracking infos
    /// </summary>
    private Dictionary<string, TrackableInfo> m_trackedGeospatialAnchors = new Dictionary<string, TrackableInfo>();

    /// <summary>
    /// Correspondance between local trackable and etsi uuid
    /// </summary>
    private Dictionary<string, string> m_localIdToEtsiId = new Dictionary<string, string>();

    /// <summary>
    /// Check if ARCore Geospatial is supported on the device
    /// </summary>
    private bool geospatialSupported = true;

    /// <summary>
    /// Initialize ARCore geospatial anchors tracking module
    /// </summary>
    public void Initialize()
    {
        XROrigin origin = UnityEngine.Object.FindAnyObjectByType<XROrigin>();
        m_anchorManager = origin.gameObject.GetComponent<ARAnchorManager>();
        if (m_anchorManager == null)
        {
            m_anchorManager = origin.gameObject.AddComponent<ARAnchorManager>();
            GameObject anchorPrefab = (GameObject)Resources.Load("ARFAnchorTrackingPrefab");
            m_anchorManager.anchorPrefab = anchorPrefab;
        }
        m_arEarthManager = origin.gameObject.AddComponent<AREarthManager>();
        m_anchorManager.anchorsChanged += OnTrackedGeospatialAnchorChanged;   
    }

    /// <summary>
    /// Get the pose of the trackable from its uuid
    /// </summary>
    /// <param name="uuid">id of the trackable</param>
    /// <returns>null or trackableInfo with last updated values</returns>
    public TrackableInfo GetPoseTrackable(Guid uuid)
    {
        if (m_trackedGeospatialAnchors.ContainsKey(uuid.ToString()))
        {
            return m_trackedGeospatialAnchors[uuid.ToString()];
        }
        else
        {
            return null;
        }
    }

    /// <summary>
    /// Need to be a geopose
    /// </summary>
    /// <param name="trackable"></param>
    /// <returns>geopose or not (does not check is solved)</returns>
    public bool AddTrackable(ETSI.ARF.OpenAPI.WorldStorage.Trackable trackable)
    {
        Debug.Log("GEO : Add Geosptial Trackable");
        if (!geospatialSupported) return false;

        if (trackable.TrackableType != ETSI.ARF.OpenAPI.WorldStorage.TrackableType.GEOPOSE)
        {
            return false;
        }
        CreateGeosptialAnchor(trackable);
  
        return true;
    }

    /// <summary>
    /// Initialize capability object with the features of the geospatial module
    /// </summary>
    /// <returns>null if ARCore Geospatial is not supported on the device, or a Capability object</returns>
    public ETSI.ARF.OpenAPI.WorldAnalysis.Capability GetSupportedCapability()
    {
        if (!geospatialSupported) return null;

        ETSI.ARF.OpenAPI.WorldAnalysis.Capability capabilityGeospatialAnchor = new ETSI.ARF.OpenAPI.WorldAnalysis.Capability();
        capabilityGeospatialAnchor.TrackableType = ETSI.ARF.OpenAPI.WorldAnalysis.TrackableType.GEOPOSE;
        ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure encodingInformation = new ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure();
        encodingInformation.DataFormat = ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructureDataFormat.ARCORE;
        encodingInformation.Version = "1.01";
        capabilityGeospatialAnchor.EncodingInformation = encodingInformation;
        capabilityGeospatialAnchor.Framerate = 30; // Not particularly specified on ARKit and ARCore
        capabilityGeospatialAnchor.Latency = 0; // Not particularly specified on ARKit and ARCore
        capabilityGeospatialAnchor.Accuracy = 1; // Not particularly specified on ARKit and ARCore
        return capabilityGeospatialAnchor;
    }

    /// <summary>
    /// Creates a new ARCore Geospatial anchor using informations from a geopose trackable object
    /// </summary>
    /// <param name="trackable"></param>
    public async void CreateGeosptialAnchor(ETSI.ARF.OpenAPI.WorldStorage.Trackable trackable)
    {

        double[] values = new double[trackable.TrackablePayload.Length / sizeof(double)];

        for (int i = 0; i < values.Length; i++)
        {
            values[i] = BitConverter.ToDouble(trackable.TrackablePayload, i * sizeof(double));
        }

        if (geospatialSupported)
        {
            while (ARSession.state == ARSessionState.CheckingAvailability || ARSession.state == ARSessionState.None || ARSession.state == ARSessionState.SessionInitializing || m_arEarthManager.EarthState != EarthState.Enabled || m_arEarthManager.EarthTrackingState != TrackingState.Tracking)
            {
                Debug.Log("Geo : checking " + ARSession.state + "  " + m_arEarthManager.EarthState + "   " + m_arEarthManager.EarthTrackingState);
                await System.Threading.Tasks.Task.Delay(100);
            }
            if (m_arEarthManager.IsGeospatialModeSupported(GeospatialMode.Enabled) != FeatureSupported.Supported)
            {
                geospatialSupported = false;
                Debug.Log("Geo : not supported");
            }
            else
            {
                values = new double[trackable.TrackablePayload.Length / sizeof(double)];

                for (int i = 0; i < values.Length; i++)
                {
                    values[i] = BitConverter.ToDouble(trackable.TrackablePayload, i * sizeof(double));
                }

                Quaternion rotation = Quaternion.Euler((float)values[3], (float)values[4], (float)values[5]);

                if (m_arEarthManager.EarthTrackingState == TrackingState.Tracking)
                {
                    var anchor = m_anchorManager.AddAnchor(values[0], values[1], values[2], rotation);
                    m_localIdToEtsiId.Add(anchor.trackableId.subId2.ToString("X16"), trackable.UUID.ToString());
                }
            }
        }
    }

    /// <summary>
    /// Update geospatial anchors informations
    /// </summary>
    /// <param name="eventArgs">Event arguments for the <see cref="ARAnchorManager.anchorsChanged"/> event</param>
    private void OnTrackedGeospatialAnchorChanged(ARAnchorsChangedEventArgs eventArgs)
    {
        foreach (var trackedGeospatialAnchor in eventArgs.updated)
        {
            Vector3 position = trackedGeospatialAnchor.transform.position;
            Quaternion rotation = trackedGeospatialAnchor.transform.rotation;

            TrackableInfo info = new TrackableInfo();
            info.name = trackedGeospatialAnchor.name;
            string localId = trackedGeospatialAnchor.trackableId.subId1.ToString("X16");// there must be a better way : does it work every time?

            if (m_localIdToEtsiId.ContainsKey(localId))
            {
                info.name = m_localIdToEtsiId[localId];
            }
            else
            {
                //Debug.Log("ARCore Geospatial Anchor No correspondance for Local Anchor " + localId);
                continue;
            }

            if (trackedGeospatialAnchor.trackingState == TrackingState.Tracking)
            {
                info.state = ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState.OK;
                info.confidence = 100;
            }
            else if (trackedGeospatialAnchor.trackingState == TrackingState.Limited)
            {
                info.state = ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState.OK;
                info.confidence = 50;
            }
            else
            {
                info.state = ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState.FAILURE; //ADD NOT_TRACKED ?
                info.confidence = 0;
            }
            info.timeStamp = unchecked((int)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds());
            info.position = position;
            info.rotation = rotation;
            info.trackableType = ETSI.ARF.OpenAPI.WorldAnalysis.TrackableType.GEOPOSE;
                       
            m_trackedGeospatialAnchors[info.name] = info;
        }
    }
}
#endif