/// <summary>
/// Shallow clone
/// </summary>
/// <returns>New instance</returns>
public object Clone()
{
var copy = new DepthCamSensorState();
this.CopyTo(copy);
return(copy);
}
/// <summary>
/// Initialize depthcam state.
/// </summary>
private void DoInitializeDepthCamAlternate()
{
this.cachedProcessedFrames.DepthImage =
new short[this.kinectSensor.DepthStream.FrameWidth * this.kinectSensor.DepthStream.FrameHeight];
this.utilitiesPort = DsspHttpUtilitiesService.Create(Environment);
this.depthCamState = new depth.DepthCamSensorState
{
FieldOfView = KinectCameraConstants.HorizontalFieldOfViewRadians,
ImageMode = depth.DepthCamSensorImageMode.Rgb,
DepthImageSize =
new Size(
this.kinectSensor.DepthStream.FrameWidth,
this.kinectSensor.DepthStream.FrameHeight),
MaximumRange = KinectCameraConstants.MaximumRangeMeters,
TimeStamp = DateTime.MinValue,
Pose = new Microsoft.Robotics.PhysicalModel.Pose(
new Microsoft.Robotics.PhysicalModel.Vector3(0, 0, 0),
new Microsoft.Robotics.PhysicalModel.Quaternion()),
DepthImage =
new short[this.kinectSensor.DepthStream.FrameWidth * this.kinectSensor.DepthStream.FrameHeight]
};
}
/// <summary>
/// Handlers http query request for raw binary format.
/// </summary>
/// <param name="query">The http query</param>
/// <param name="state">Depth camera state</param>
/// <param name="transform"> XSLT transform to be applied</param>
/// <param name="utilitiesPort">Utitilise port to post the response</param>
/// <returns>CCR Task Chunk</returns>
private static IEnumerator <ITask> HttpQueryHelperRawFormat(
HttpQuery query,
DepthCamSensorState state,
string transform,
DsspHttpUtilitiesPort utilitiesPort)
{
var type = query.Body.Query[QueryType];
byte[] imageByteArray = null;
switch (type.ToLowerInvariant())
{
case DepthPlusRgb:
imageByteArray =
ConvertVisibleAndDepthToByteArray(
state.VisibleImage,
state.DepthImage);
break;
case Rgb:
imageByteArray = state.VisibleImage;
break;
case Depth:
imageByteArray = new byte[state.DepthImage.Length * sizeof(short)];
Buffer.BlockCopy(state.DepthImage, 0, imageByteArray, 0, imageByteArray.Length);
break;
case Ir:
imageByteArray = state.VisibleImage;
break;
}
if (imageByteArray == null)
{
query.ResponsePort.Post(
new HttpResponseType(
HttpStatusCode.OK,
state,
transform));
}
else
{
using (var mem = new MemoryStream(imageByteArray))
{
var response = new WriteResponseFromStream(
query.Body.Context,
mem,
OctetStream);
utilitiesPort.Post(response);
yield return(response.ResultPort.Choice());
}
}
}
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;
}
/// <summary>
/// Provides a standard implementation of HttpGet
/// </summary>
/// <param name="get">The HttpGet message to process</param>
/// <param name="state">The depthcam state</param>
/// <param name="transform">The embedded resource to use as the XSLT transform</param>
public static void HttpGetHelper(HttpGet get, DepthCamSensorState state, string transform)
{
var copy = state.Clone() as DepthCamSensorState;
copy.DepthImage = null;
copy.VisibleImage = null;
get.ResponsePort.Post(
new HttpResponseType(
HttpStatusCode.OK,
copy,
transform));
}
/// <summary>
/// Handles http query request.
/// </summary>
/// <param name="query">The http query</param>
/// <param name="state">Depth camera state</param>
/// <param name="transform"> XSLT transform to be applied</param>
/// <param name="utilitiesPort">Utitilise port to post the response</param>
/// <param name="visibleWidth">Width of a visible image - needed to blend depth and rgb pictures for a visual represenation</param>
/// <param name="visibleHeight">Height of a visible image - needed to blend depth and rgb pictures for a visual represenation</param>
/// <returns>CCR Task Chunk</returns>
public static IEnumerator <ITask> HttpQueryHelper(
HttpQuery query,
DepthCamSensorState state,
string transform,
DsspHttpUtilitiesPort utilitiesPort,
int visibleWidth,
int visibleHeight)
{
var type = query.Body.Query[QueryType];
var format = query.Body.Query[QueryFormat];
// Default is Png
if (format == null)
{
format = Png;
}
switch (type.ToLowerInvariant())
{
case DepthPlusRgb:
// Intentional fall through.
case Rgb:
if (state.ImageMode == DepthCamSensor.DepthCamSensorImageMode.Infrared)
{
state.ImageMode = DepthCamSensor.DepthCamSensorImageMode.Rgb;
}
break;
case Depth:
// Do nothing.
break;
case Ir:
if (state.ImageMode == DepthCamSensor.DepthCamSensorImageMode.Rgb)
{
state.ImageMode = DepthCamSensor.DepthCamSensorImageMode.Infrared;
}
break;
}
if (format.ToLowerInvariant().Equals(Raw))
{
return(HttpQueryHelperRawFormat(query, state, transform, utilitiesPort));
}
else
{
return(HttpQueryHelperBitmapSource(query, state, transform, utilitiesPort, visibleWidth, visibleHeight));
}
}
/// <summary>
/// Start initializes SimulatedDepthcamService and listens for drop messages
/// </summary>
protected override void Start()
{
const double KinectMaxValidDepthMm = 4000.0;
const double KinectMinValidDepthMm = 800.0;
if (this._state == null)
{
// no config file found, initialize default state according to KinectReservedSampleValues
this._state = new DepthCamSensorState();
this._state.MaximumRange = KinectMaxValidDepthMm / 1000.0;
this._state.MinimumRange = KinectMinValidDepthMm / 1000.0;
this._state.FurtherThanMaxDepthValue = (short)4095.0;
this._state.NearerThanMinDepthValue = (short)0.0;
this.SaveState(this._state);
}
this._physicsEngine = physics.PhysicsEngine.GlobalInstance;
_notificationTarget = new simengine.SimulationEnginePort();
utilitiesPort = DsspHttpUtilitiesService.Create(Environment);
// PartnerType.Service is the entity instance name.
simengine.SimulationEngine.GlobalInstancePort.Subscribe(ServiceInfo.PartnerList, _notificationTarget);
// dont start listening to DSSP operations, other than drop, until notification of entity
Activate(new Interleave(
new TeardownReceiverGroup
(
Arbiter.Receive <simengine.InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
Arbiter.Receive <dssp.DsspDefaultDrop>(false, _mainPort, DefaultDropHandler)
),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup()
));
}
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));
}
}
/// <summary>
/// Handlers http query request for bitmap source format.
/// </summary>
/// <param name="query">The http query</param>
/// <param name="state">Depth camera state</param>
/// <param name="transform"> XSLT transform to be applied</param>
/// <param name="utilitiesPort">Utitilise port to post the response</param>
/// <param name="visibleWidth">Width of a visible image - needed to blend depth and rgb pictures for a visual represenation</param>
/// <param name="visibleHeight">Height of a visible image - needed to blend depth and rgb pictures for a visual represenation</param>
/// <returns>CCR Task Chunk</returns>
private static IEnumerator <ITask> HttpQueryHelperBitmapSource(
HttpQuery query,
DepthCamSensorState state,
string transform,
DsspHttpUtilitiesPort utilitiesPort,
int visibleWidth,
int visibleHeight)
{
var type = query.Body.Query[QueryType];
BitmapSource bitmapSource = null;
try
{
switch (type.ToLowerInvariant())
{
case DepthPlusRgb:
// we must downscale visible image to match the resolution of depth image - in order to merge the
// two pictures. Its an expensive call - but we do it only here, where it was explicitly requested
byte[] resizedVisibleImage = ResizeVisibleImageToMatchDepthImageDimentions(
state.VisibleImage,
visibleWidth,
visibleHeight,
state.DepthImageSize.Width,
state.DepthImageSize.Height);
bitmapSource = CreateBitmapSourceFromByteArray(
ConvertVisibleAndDepthTo32bppByteArray(resizedVisibleImage, state.DepthImage),
state.DepthImageSize.Width,
state.DepthImageSize.Height,
swm.PixelFormats.Bgra32);
break;
case Rgb:
bitmapSource = CreateBitmapSourceFromByteArray(
state.VisibleImage,
visibleWidth,
visibleHeight,
swm.PixelFormats.Bgr24);
break;
case Depth:
bitmapSource = CreateBitmapSourceFromShortPixelArray(
state.DepthImage,
state.DepthImageSize.Width,
state.DepthImageSize.Height,
(int)(state.MaximumRange * 1000),
state.DepthImage.Clone() as short[]);
break;
case Ir:
bitmapSource = CreateBitmapSourceFromInfraredArray(
state.VisibleImage,
state.DepthImageSize.Width,
state.DepthImageSize.Height,
swm.PixelFormats.Gray16);
break;
}
}
catch
{
// We do not attempt to synchronize the HTTP view with
// state switching, so we might switch to RGB but still
// have data from IR mode
var width = state.DepthImageSize.Width;
var height = state.DepthImageSize.Height;
var zeroData = new byte[3 * width * height];
bitmapSource = BitmapSource.Create(
width,
height,
96,
96,
swm.PixelFormats.Bgr24,
null,
zeroData,
state.DepthImageSize.Width * 3);
}
if (bitmapSource == null)
{
query.ResponsePort.Post(
new HttpResponseType(
HttpStatusCode.OK,
state,
transform));
}
else
{
using (var mem = new MemoryStream())
{
BitmapEncoder encoder = null;
PngBitmapEncoder pngEncoder = new PngBitmapEncoder();
pngEncoder.Interlace = PngInterlaceOption.Off;
encoder = pngEncoder;
encoder.Frames.Add(BitmapFrame.Create(bitmapSource));
encoder.Save(mem);
mem.Position = 0;
var response = new WriteResponseFromStream(
query.Body.Context,
mem,
MediaPng);
utilitiesPort.Post(response);
yield return(response.ResultPort.Choice());
}
}
}
