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)); } }