IEnumerator <ITask> UpdateImage(webcam.WebCamOperations webcamPort, ImageProcessingResultForm webcamForm)
{
long timestamp = _stopWatch.ElapsedMilliseconds;
byte[] rgbData = null;
Size size = new Size(0, 0);
yield return(Arbiter.Choice(webcamPort.QueryFrame(),
success =>
{
rgbData = success.Frame;
size = success.Size;
},
failure =>
{
LogError(failure.ToException());
}));
TaskQueue.Enqueue(new Task(() =>
{
if (rgbData != null)
{
ComputeGradient(ref rgbData, size);
UpdateBitmap(rgbData, size, webcamForm, timestamp);
}
}));
}
IEnumerator <ITask> UpdateImage(DateTime dateTime)
{
byte[] rgbData = null;
Size size = new Size(0, 0);
yield return(Arbiter.Choice(_simulatedWebcamServicePort.QueryFrame(),
success =>
{
rgbData = success.Frame;
size = success.Size;
},
failure =>
{
LogError(failure.ToException());
}));
if (rgbData != null)
{
ComputeGradient(ref rgbData, size);
UpdateBitmap(rgbData, size);
}
Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateImage));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(60), _dateTimePort);
}
/// <summary>
/// See adapter documentation in the Myro 3 developer manual.
/// http://wiki.roboteducation.org/Myro_3.0_Developer_Manual
/// </summary>
public void QueryFrame(MyroImageType type, out int Width, out int Height, out byte[] Image)
{
var r = RSUtils.ReceiveSync(queue,
opPort.QueryFrame(new webcam.QueryFrameRequest()
{
Format = type.Guid
}),
Params.DefaultRecieveTimeout);
Width = r.Size.Width;
Height = r.Size.Height;
Image = r.Frame;
}
IEnumerator <ITask> UpdateWebCamImage()
{
byte[] rgbData = null;
Size size = new Size(0, 0);
yield return(Arbiter.Choice(_simulatedWebcamServicePort.QueryFrame(),
success =>
{
rgbData = success.Frame;
size = success.Size;
},
failure =>
{
LogError(failure.ToException());
}));
if (rgbData != null)
{
ComputeGradient(ref rgbData, size);
UpdateBitmap(rgbData, size);
}
}
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));
}
}
IEnumerator <ITask> DoVisionProcessing(VisionProcessingParameters parameters)
{
try
{
List <ColorSet> colors = parameters.Colors;
Settings settings = parameters.Settings;
int[] colorSetMap = parameters.ColorSetMap;
webcam.QueryFrameResponse response = null;
yield return(Arbiter.Choice(
_webcamPort.QueryFrame(),
delegate(webcam.QueryFrameResponse success)
{
response = success;
},
LogError
));
if (response == null)
{
yield break;
}
double threshold = settings.Threshold;
int height = response.Size.Height;
int width = response.Size.Width;
int offset;
int stride = response.Frame.Length / height;
byte[] bytes = response.Frame;
byte[,] indexed = new byte[width, height];
int segOffset;
int segStride = width;
byte[] segmented = new byte[width * height];
for (int y = 0; y < height; y++)
{
offset = stride * y;
segOffset = segStride * y;
for (int x = 0; x < width; x++)
{
int blu = bytes[offset++];
int grn = bytes[offset++];
int red = bytes[offset++];
int col = (((red + 7) & 0xF0) << 4) |
((grn + 7) & 0xF0) |
(((blu + 7) & 0xF0) >> 4);
int bestIndex = colorSetMap[col];
indexed[x, y] = (byte)bestIndex;
segmented[segOffset++] = (byte)bestIndex;
}
}
if (settings.Despeckle)
{
int[] votes;
byte[,] despeckled = new byte[width, height];
offset = 0;
for (int y = 0; y < height; y++)
{
offset = y * stride;
for (int x = 0; x < width; x++, offset++)
{
votes = new int[colors.Count + 1];
int color;
int leader = 0;
if (x == 0 || y == 0 ||
x == width - 1 || y == height - 1)
{
leader = indexed[x, y];
votes[leader]++;
}
else
{
for (int iy = -1; iy < 2; iy++)
{
for (int ix = -1; ix < 2; ix++)
{
color = indexed[x + ix, y + iy];
votes[color]++;
if (votes[color] > votes[leader])
{
leader = color;
}
}
}
//
// if there is a tie, then use the original color.
//
int bestVote = votes[leader];
for (int i = 0; i < votes.Length; i++)
{
if (i != leader &&
votes[i] == bestVote)
{
leader = indexed[x, y];
break;
}
}
}
despeckled[x, y] = (byte)leader;
}
}
indexed = despeckled;
}
List <ColorArea> areas = new List <ColorArea>();
byte[,] copy = (byte[, ])indexed.Clone();
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
int curr = copy[x, y];
if (curr != 0)
{
ColorArea area = new ColorArea(colors[curr - 1].Name);
area.Flood(copy, x, y);
areas.Add(area);
}
}
}
areas = areas.FindAll(
delegate(ColorArea test)
{
return(test.Area >= settings.MinBlobSize);
}
);
areas.Sort(
delegate(ColorArea left, ColorArea right)
{
return(-left.Area.CompareTo(right.Area));
}
);
Color[] outputColors = new Color[colors.Count + 1];
for (int i = 0; i < outputColors.Length; i++)
{
if (i == 0)
{
outputColors[i] = Color.Black;
}
else
{
ColorSet set = colors[i - 1];
ColorDefinition def = set.Colors[0];
outputColors[i] = Color.FromArgb(def.R, def.G, def.B);
}
}
for (int y = 0; y < height; y++)
{
offset = y * stride;
segOffset = y * segStride;
for (int x = 0; x < width; x++)
{
int palIndex = segmented[segOffset++];
Color pixel = outputColors[palIndex];
bytes[offset++] = (byte)pixel.B;
bytes[offset++] = (byte)pixel.G;
bytes[offset++] = (byte)pixel.R;
}
}
segmented = bytes;
SegmentedImage image = new SegmentedImage(response.TimeStamp, width, height, segmented, indexed);
_mainPort.Post(new UpdateSegmentedImage(image));
FoundColorAreas foundColors = new FoundColorAreas();
foundColors.TimeStamp = response.TimeStamp;
foreach (ColorArea area in areas)
{
if (area.Area > 0)
{
area.Complete();
foundColors.Areas.Add(area);
}
}
_fwdPort.Post(new UpdateColorAreas(foundColors));
}
finally
{
_fwdPort.Post(new ProcessFrame(false));
}
}
IEnumerator <ITask> ProcessImage(List <ColorBin> bins)
{
Fault fault = null;
cam.QueryFrameRequest request = new cam.QueryFrameRequest();
request.Format = Guid.Empty;// new Guid("b96b3cae-0728-11d3-9d7b-0000f81ef32e");
byte[] frame = null;
DateTime timestamp = DateTime.MinValue;
int height = 0;
int width = 0;
yield return(Arbiter.Choice(
_camPort.QueryFrame(request),
delegate(cam.QueryFrameResponse response)
{
timestamp = response.TimeStamp;
frame = response.Frame;
width = response.Size.Width;
height = response.Size.Height;
},
delegate(Fault f)
{
fault = f;
}
));
ImageProcessedRequest processed = new ImageProcessedRequest();
if (fault != null)
{
_mainPort.Post(new ImageProcessed(processed));
yield break;
}
int size = width * height * 3;
processed.TimeStamp = timestamp;
List <FoundBlob> results = processed.Results;
foreach (ColorBin bin in bins)
{
FoundBlob blob = new FoundBlob();
blob.Name = bin.Name;
blob.XProjection = new int[width];
blob.YProjection = new int[height];
results.Add(blob);
}
int offset;
for (int y = 0; y < height; y++)
{
offset = y * width * 3;
for (int x = 0; x < width; x++, offset += 3)
{
int r, g, b;
b = frame[offset];
g = frame[offset + 1];
r = frame[offset + 2];
for (int i = 0; i < bins.Count; i++)
{
ColorBin bin = bins[i];
if (bin.Test(r, g, b))
{
results[i].AddPixel(x, y);
}
}
}
}
foreach (FoundBlob blob in results)
{
if (blob.Area > 0)
{
blob.MeanX = blob.MeanX / blob.Area;
blob.MeanY = blob.MeanY / blob.Area;
blob.CalculateMoments();
}
}
_mainPort.Post(new ImageProcessed(processed));
}
IEnumerator <ITask> DoCameraUpdateFrameHandler(cam.UpdateFrame update)
{
try
{
cam.QueryFrameResponse frame = null;
Fault fault = null;
yield return(Arbiter.Choice(
_webCamPort.QueryFrame(new cam.QueryFrameRequest()),
delegate(cam.QueryFrameResponse success)
{
frame = success;
},
delegate(Fault f)
{
fault = f;
}
));
if (fault != null)
{
LogError(null, "Failed to get frame from camera", fault);
yield break;
}
Bitmap bmp = new Bitmap(
frame.Size.Width,
frame.Size.Height,
PixelFormat.Format24bppRgb
);
BitmapData data = bmp.LockBits(
new Rectangle(0, 0, bmp.Width, bmp.Height),
ImageLockMode.WriteOnly,
PixelFormat.Format24bppRgb
);
Marshal.Copy(frame.Frame, 0, data.Scan0, frame.Frame.Length);
bmp.UnlockBits(data);
FormInvoke setImage = new FormInvoke(
delegate()
{
if (!_form.IsDisposed)
{
_form.CameraImage = bmp;
}
}
);
WinFormsServicePort.Post(setImage);
Arbiter.Choice(
setImage.ResultPort,
delegate(EmptyValue success) { },
delegate(Exception e)
{
fault = Fault.FromException(e);
}
);
if (fault != null)
{
LogError(null, "Unable to set camera image on form", fault);
yield break;
}
}
finally
{
_mainPort.Post(new UpdateProcessing(false));
}
}
