private void CloudSpatialAnchorSession_AnchorLocated(object sender, AnchorLocatedEventArgs args)
{
switch (args.Status)
{
case LocateAnchorStatus.Located:
DispatcherQueue.Enqueue(async() =>
{
// Instantiate the Prefab
_prefabInstance = GameObject.Instantiate(Prefab, Vector3.zero, Quaternion.identity) as GameObject;
var localAnchor = _prefabInstance.AddComponent <WorldAnchor>();
// Get the WorldAnchor from the CloudSpatialAnchor and assign it to the local anchor
localAnchor.SetNativeSpatialAnchorPtr(args.Anchor.LocalAnchor);
// Delete the ASA anchor Clean up state so that we can start over and create a new anchor.
await _cloudSpatialAnchorSession.DeleteAnchorAsync(args.Anchor);
_currentCloudAnchorId = String.Empty;
_wasTapped = false;
Log($"Anchor located: {args.Identifier}\r\nGaze and tap to place a new anchor.", Color.green);
});
break;
case LocateAnchorStatus.AlreadyTracked:
Log($"ASA Anchor already tracked: {args.Identifier}", Color.magenta);
break;
case LocateAnchorStatus.NotLocated:
Log($"ASA Anchor not located : {args.Identifier}", Color.magenta);
break;
case LocateAnchorStatus.NotLocatedAnchorDoesNotExist:
Log($"ASA Anchor not located -> Does not exist: {args.Identifier}", Color.red);
break;
}
}
public void LocalizeAnchor()
{
DispatcherQueue.Enqueue(() =>
{
if (_cloudSpatialAnchorSession == null)
{
Log("CloudSpatialAnchorSession was null. Weird.", Color.red);
return;
}
else
{
// Initialize session fresh & clean
CleanupObjects();
_cloudSpatialAnchorSession.Stop();
_cloudSpatialAnchorSession.Dispose();
_cloudSpatialAnchorSession = null;
InitializeSession();
// Create a Watcher with anchor ID to locate the anchor that was created before
AnchorLocateCriteria criteria = new AnchorLocateCriteria
{
Identifiers = new string[] { _currentCloudAnchorId }
};
_cloudSpatialAnchorSession.CreateWatcher(criteria);
Log($"Localizing anchor with {_cloudSpatialAnchorSession.GetActiveWatchers().Count} watchers.\r\nLook around to gather spatial data...");
}
});
}
public async Task Execute(EventMetadata m, TEvent ev)
{
_dispatcherQueue.Enqueue(async() =>
{
await _projectionHandler.Execute(m, ev);
foreach (var i in _queries)
{
i.Load(_model);
}
});
}
private void Log(string text, Color?color = null)
{
if (LogText != null)
{
DispatcherQueue.Enqueue(() =>
{
LogText.text = text;
LogText.color = color ?? Color.gray;
});
}
else
{
Debug.LogError("Log text control is not assigned.");
}
Debug.Log(text);
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.DepthCameraEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
try
{
_entity.Register(_raycastResults);
}
catch (Exception ex)
{
LogError(ex);
}
if (_rayCastQueue == null)
{
_rayCastQueue = new DispatcherQueue(_entity.EntityState.Name + "depthNotifications",
TaskQueue.Dispatcher,
TaskExecutionPolicy.ConstrainQueueDepthDiscardTasks,
1);
// attach handler to raycast results port
_rayCastQueue.Enqueue(Arbiter.ReceiveWithIterator(false, _raycastResults, RaycastResultsHandler));
}
}
private IEnumerator <ITask> RaycastResultsHandler(simengine.DepthCameraEntity.DepthCameraResult result)
{
try
{
var now = Microsoft.Robotics.Common.Utilities.ElapsedSecondsSinceStart;
if (now - _lastRaycastUpdate < ((double)_entity.UpdateInterval / 1000.0))
{
// dont update more than 20 times a second
yield break;
}
_lastRaycastUpdate = now;
if (result.Data == null)
{
yield break;
}
var latestResults = new depthcam.DepthCamSensorState();
latestResults.MaximumRange = this._state.MaximumRange;
latestResults.MinimumRange = this._state.MinimumRange;
latestResults.FurtherThanMaxDepthValue = this._state.FurtherThanMaxDepthValue;
latestResults.NearerThanMinDepthValue = this._state.NearerThanMinDepthValue;
var depthImage = new short[result.Data.Length];
latestResults.DepthImage = depthImage;
latestResults.DepthImageSize = new Size(result.Width, result.Height);
if (_entity != null)
{
latestResults.FieldOfView = (float)(_entity.FieldOfView * Math.PI / 180.0f);
}
short maxDepthMM = (short)(this._state.MaximumRange * 1000);
short minDepthMM = (short)(this._state.MinimumRange * 1000);
for (int i = 0; i < result.Data.Length; i++)
{
var s = (short)(result.Data[i] & 0xFF);
var depth = (short)((s * (short)maxDepthMM) / byte.MaxValue);
// The camera's far plane is already set at MaxDepth so no pixels will be further than
// that. To compensate for that, we relax the test from '>' to '>=' so that the
// 'further-than' pixel value can be set. This enables similar behavior to Kinect where
// too-near and too-far pixels are both set to zero.
if (depth >= maxDepthMM)
{
// this if branch is redundant if the shader sets the depth limit but its defense in depth.
depthImage[i] = this._state.FurtherThanMaxDepthValue;
}
else if (depth < minDepthMM)
{
depthImage[i] = this._state.NearerThanMinDepthValue;
}
else
{
depthImage[i] = depth;
}
}
byte[] rgbImage = null;
if (webcamPort != null)
{
var stateOrFault = webcamPort.QueryFrame();
yield return(stateOrFault.Choice(
response =>
{
rgbImage = response.Frame;
},
fault => LogError(fault)));
}
if (rgbImage != null)
{
latestResults.VisibleImage = rgbImage;
}
latestResults.Pose = _state.Pose;
latestResults.TimeStamp = DateTime.Now;
// send replace message to self
var replace = new depthcam.Replace();
// for perf reasons dont set response port, we are just talking to ourself anyway
replace.ResponsePort = null;
replace.Body = latestResults;
_mainPort.Post(replace);
}
finally
{
_raycastResults.Clear();
_rayCastQueue.Enqueue(Arbiter.ReceiveWithIterator(false, _raycastResults, RaycastResultsHandler));
}
}