WorldAnalysisARFoundationModuleARKitWorldMap.cs

#if UNITY_IOS
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.XR.ARFoundation;
using UnityEngine.XR.ARSubsystems;
using UnityEngine.XR.ARKit;
using System.IO;
using Unity.XR.CoreUtils;
using static WorldAnalysisARFoundationModule;
using ETSI.ARF.OpenAPI.WorldStorage;
using Unity.Collections;


public class WorldAnalysisARFoundationModuleARKitWorldMap : WorldAnalysisARFoundationModule
{
    /// <summary>
    ///  Anchor manager
    /// </summary>
    private ARAnchorManager m_anchorManager;
    ///Has loaded a worl map
    private bool m_hasAddedMap ;
    /// Possible trackable id (arfoundation) of an anchor contained in the world map. Take the pose of this anchor if it exists or zero
    private string m_arfoundationAnchorTrackableId ;
    /// uuId of the map trackabled 
    private  Guid   m_trackedUUID ;
    /// computed and updated pose 
    private TrackableInfo m_trackedPose ;

    /// <summary>
    /// Initialize image tracking module
    /// </summary>
    public void Initialize()
    {
        XROrigin origin = UnityEngine.Object.FindAnyObjectByType<XROrigin>();
        m_anchorManager = origin.gameObject.AddComponent<ARAnchorManager>();
        GameObject anchorPrefab = (GameObject)Resources.Load("ARFAnchorTrackingPrefab");
        m_anchorManager.anchorPrefab = anchorPrefab;
        m_anchorManager.anchorsChanged += OnTrackedAnchorChanged;
        m_trackedPose = null ;
        m_hasAddedMap= false ;
    }

    /// <summary>
    ///  found  
    /// </summary>
    /// <param name="uuid">name of the mesh trackable</param>
    public TrackableInfo GetPoseTrackable(Guid uuid)
    {
        if (m_trackedPose != null)
        {
            if (m_trackedUUID == uuid)
            {
                return m_trackedPose ;
            }
        }
        return null; 
    }

    /// <summary>
    /// Add a trackable : need to be a map
    /// </summary>
    /// <param name="trackable">Image trackable</param>
    /// <returns>Supported or not</returns>
    public bool AddTrackable(Trackable trackable)
    {
        if (trackable.TrackableType != ETSI.ARF.OpenAPI.WorldStorage.TrackableType.MAP)
        {
            return false;
        }
        if (m_hasAddedMap) 
        {
            Debug.Log("Only one ARKit map can be loaded");
            return false ;
        }

        // Check if a map url is provided
        string url = "" ;
        if (trackable.KeyvalueTags.ContainsKey("url"))
        {
            foreach(string s in trackable.KeyvalueTags["url"])
            {
                // first one
                url = s ;
                break ;
            }
        }
        bool resul = AddWorldMapToARKit(url); 
        m_trackedUUID = trackable.UUID ;
        m_arfoundationAnchorTrackableId = trackable.Name ;
        m_hasAddedMap = resul ;
        return resul;
    }

    /// <summary>
    ///  Initialize capability object with Mesh tracking
    /// </summary>
    public ETSI.ARF.OpenAPI.WorldAnalysis.Capability  GetSupportedCapability()
    {
        ETSI.ARF.OpenAPI.WorldAnalysis.Capability capabilityMesh = new ETSI.ARF.OpenAPI.WorldAnalysis.Capability();
        capabilityMesh.TrackableType = ETSI.ARF.OpenAPI.WorldAnalysis.TrackableType.MAP;
        ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure encodingInformation = new ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure();
        encodingInformation.DataFormat = ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructureDataFormat.ARKIT;
        encodingInformation.Version = "1.01";
        capabilityMesh.EncodingInformation = encodingInformation;
        capabilityMesh.Framerate = 30; // Not particularly specified on ARKit 
        capabilityMesh.Latency = 0; // Not particularly specified on ARKit
        capabilityMesh.Accuracy = 1; // Not particularly specified on ARKit
        return capabilityMesh ;
    }

    /// <summary>
    /// Callback when Unity anchors are updated
    /// </summary>
    /// <param name="eventArgs">update values</param>
    private void OnTrackedAnchorChanged(ARAnchorsChangedEventArgs eventArgs)
    {
        foreach (var trackedAnchor in eventArgs.updated)
        {
            Debug.Log("Anchor found  " +trackedAnchor.trackableId.ToString());

            if (trackedAnchor.trackableId.ToString() == m_arfoundationAnchorTrackableId)
            {
                /// look for an anchor with the trackable Id correspond to the ETSI ARF trackable name
                UpdateTrackableInfoWithPose(trackedAnchor.transform.position,trackedAnchor.transform.rotation);
                break ;
            }
        }
    }

    /// <summary>
    /// Add a map to arkitsubsystem : check local file exist or download from given url
    /// </summary>
    /// <param name="url">url of a map to download</param>
    /// <returns>found or not</returns>
    protected bool AddWorldMapToARKit(string url)
    {
        if (url.Length > 0)
        {
             Debug.Log("Load AR Map from URL");
            LoadWorldMapFromURL(url);
            // don't check if url is valid
            return true ;
        }
        else 
        {
             Debug.Log("Load AR Map locally");
            string localMap = Application.persistentDataPath + "/ARkitWorlMap.map";
            if (File.Exists(localMap))
            {
                var coroutine = CoroutineLoadWorldMap(localMap);
                WorldAnalysisARFoundationCoroutineHelper.Instance.StartACoroutine(coroutine);
                return true ;
            }
        }
        // no url and no local map
        return false ;
    }

    /// <summary>
    /// Update pose of the map trackable
    /// </summary>
    /// </param name="position"> evaluated position of trackable/param>
    /// </param name="rotation"> evaluated rotation of the trackable</param>
    private void UpdateTrackableInfoWithPose(Vector3 position, Quaternion rotation)
    {
        m_trackedPose = new TrackableInfo();
        m_trackedPose.name = m_arfoundationAnchorTrackableId;
        m_trackedPose.state = ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState.OK; // here : could check the state of the ARKit slam
        m_trackedPose.confidence = 100;
        m_trackedPose.timeStamp = unchecked((int)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds());
        m_trackedPose.position = position;
        m_trackedPose.rotation = rotation;
        m_trackedPose.trackableType = ETSI.ARF.OpenAPI.WorldAnalysis.TrackableType.MAP;
    }

    /// <summary>
    /// Load a world map from a url
    /// </summary>
    /// </param name="url"> url to the worlmap to download</param>
    private async void LoadWorldMapFromURL(string url)
    {
        Debug.Log("Download WorlMap from url "+ url);
        KeyValuePair<string , string> downloaded = await WorldAnalysisARFoundationHelper.DownloadFileHTTP(url);
        var coroutine = CoroutineLoadWorldMap(downloaded.Key);
        WorldAnalysisARFoundationCoroutineHelper.Instance.StartACoroutine(coroutine);
    }

    /// <summary>
    /// Load current map from path
    /// </summary>
    /// <param name="mapPath">path of the map</param>
    /// <returns>coroutine</returns>
    public IEnumerator CoroutineLoadWorldMap(string mapPath)
    {
        while (ARSession.state == ARSessionState.CheckingAvailability || ARSession.state == ARSessionState.None ||ARSession.state == ARSessionState.SessionInitializing)
        {
            // wait for ar session to be ready
            yield return null ;
        }

        ARSession session = Component.FindAnyObjectByType<ARSession>() ;
        ARKitSessionSubsystem sessionSubsystem = (ARKitSessionSubsystem)session.subsystem;
        if (sessionSubsystem == null)
        {
            Debug.Log("Cannot load map: no ARKitSessionSubsystem");
        }
        else 
        {
            FileStream file;
            file = File.Open(mapPath, FileMode.Open);
            const int bytesPerFrame = 1024 * 10;
            var bytesRemaining = file.Length;
            var binaryReader = new BinaryReader(file);
            var allBytes = new List<byte>();
            while (bytesRemaining > 0)
            {
                var bytes = binaryReader.ReadBytes(bytesPerFrame);
                allBytes.AddRange(bytes);
                bytesRemaining -= bytesPerFrame;
                yield return null;
            }

            var data = new NativeArray<byte>(allBytes.Count, Allocator.Temp);
            data.CopyFrom(allBytes.ToArray());
            ARWorldMap worldMap;
            if (ARWorldMap.TryDeserialize(data, out worldMap))
            {
                data.Dispose();
            }
            sessionSubsystem.ApplyWorldMap(worldMap);
            UpdateTrackableInfoWithPose(Vector3.zero, Quaternion.identity); // before trying to find an anchor: default pause is origin of the map
        }
    }
}
#endif