public void OnReplace(depth.Replace replace)
{
// Can't actually send a replace to this service, but it's here
// for symmetry with replace events that are raised as the device
// signals changes to it.
replace.ResponsePort.Post(
Fault.FromCodeSubcode(
FaultCodes.Receiver,
DsspFaultCodes.MessageNotSupported));
}
public IEnumerator <ITask> ReplaceHandler(depthcam.Replace replace)
{
_state = replace.Body;
if (replace.ResponsePort != null)
{
replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
}
// issue notification
_subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
yield break;
}
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));
}
}
