/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
//
// Add service specific initialization here
//
base.Start();
#region Run WinForms and Activate Timers
_stopWatch.Start();
var webcamPorts = new[] { _firstWebcamPort, _secondWebcamPort };
for (int idx = 0; idx < _webcamForms.Length; ++idx)
{
int i = idx;
WinFormsServicePort.Post(new RunForm(() =>
{
_webcamForms[i] = new ImageProcessingResultForm();
_webcamForms[i].Show();
Activate(Arbiter.Receive(false, _timers[i], dateTime => UpdateWebcam(webcamPorts[i],
_webcamForms[i], _timers[i])));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(60), _timers[i]);
return(_webcamForms[i]);
}));
}
#endregion
}
public void SetFrameRateHandler(kinect.SetFrameRate update)
{
var rate = update.Body.FrameRate;
if (rate < 1 || rate > kinect.KinectState.MaxFrameRate)
{
update.ResponsePort.Post(
Fault.FromCodeSubcodeReason(
FaultCodes.Receiver,
DsspFaultCodes.OperationFailed,
Resources.FrameRateOutOfRange));
}
else
{
this.pollInterval = TimeSpan.FromMilliseconds(1000.0 / rate);
TaskQueue.EnqueueTimer(this.pollInterval, this.pollPort);
this.kinectEntity.WebCam.UpdateInterval = (int)this.pollInterval.TotalMilliseconds;
this.kinectEntity.DepthCam.UpdateInterval = (int)this.pollInterval.TotalMilliseconds;
this.state.FrameRate = rate;
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
SendNotification(this.kinectSubmgrPort, update);
}
}
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);
}
IEnumerator <ITask> UpdateWebcamHelper(webcam.WebCamOperations webcamPort, ImageProcessingResultForm webcamForm,
Port <DateTime> timerPort)
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
yield return(new IterativeTask(() => UpdateImage(webcamPort, webcamForm)));
Activate(Arbiter.Receive(false, timerPort, dateTime => UpdateWebcam(webcamPort,
webcamForm, timerPort)));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(Math.Max(1, 60 - stopwatch.ElapsedMilliseconds)), timerPort);
yield break;
}
IEnumerator <ITask> UpdateSensorData(DateTime dateTime)
{
var resultPort = new CompletionPort();
PostOnTaskCompletion(resultPort, UpdateColorSensor);
PostOnTaskCompletion(resultPort, UpdateBrightnessSensor);
PostOnTaskCompletion(resultPort, UpdateCompass);
PostOnTaskCompletion(resultPort, UpdateLRF);
PostOnTaskCompletion(resultPort, UpdateSonar);
PostOnTaskCompletion(resultPort, UpdateInfrared);
PostOnTaskCompletion(resultPort, UpdateWebCamImage);
Activate(Arbiter.MultipleItemReceive(false, resultPort, 7, allComplete =>
{
Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateSensorData));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(60), _dateTimePort);
}));
yield break;
}
/// <summary>
/// Service start
/// </summary>
protected override void Start()
{
//
// Add service specific initialization here
//
base.Start();
#region Run WinForm and Activate Timer
WinFormsServicePort.Post(new RunForm(() =>
{
_imageProcessingForm = new ImageProcessingResultForm();
_imageProcessingForm.Show();
Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateSensorData));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(60), _dateTimePort);
return(_imageProcessingForm);
}));
#endregion
}
/// <summary>
/// Drains the pending requests.
/// </summary>
/// <param name="timestamp">The timestamp.</param>
/// <returns>Standard ccr iterator</returns>
private IEnumerator <ITask> DrainPendingRequests(DateTime timestamp)
{
var stopwatch = Stopwatch.StartNew();
try
{
// No pending request - exit early
if (this.pendingFrameRequests.Count == 0)
{
yield break;
}
// Fetch the depth and video in parallel as required. Do once each for any pending requests.
var taskCount = 0;
var donePort = new Port <EmptyValue>();
var rawFrames = new RawKinectFrames();
if (this.pendingFrameRequests.Any(p => p.Body.IncludeDepth))
{
Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask <DepthImageFormat, RawKinectFrames>(
this.state.DepthImageFormat,
rawFrames,
this.BuildRawDepthFrame),
donePort);
++taskCount;
}
if (this.pendingFrameRequests.Any(p => p.Body.IncludeVideo))
{
Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask <ColorImageFormat, RawKinectFrames>(
this.state.ColorImageFormat,
rawFrames,
this.BuildRawVideoFrame),
donePort);
++taskCount;
}
if (taskCount > 0)
{
// Wait for the operations to complete
yield return(Arbiter.MultipleItemReceive(false, donePort, taskCount, EmptyHandler));
}
// Stamp the returned frames
var currentFrame = ++this.frameNumber;
if (rawFrames.RawDepthFrameData != null)
{
rawFrames.RawDepthFrameInfo.FrameNumber = currentFrame;
rawFrames.RawDepthFrameInfo.Timestamp = timestamp.Ticks;
}
if (rawFrames.RawColorFrameData != null)
{
rawFrames.RawColorFrameInfo.FrameNumber = currentFrame;
rawFrames.RawColorFrameInfo.Timestamp = timestamp.Ticks;
}
// Return the requested frames to each pending request.
foreach (var request in this.pendingFrameRequests)
{
var response = new RawKinectFrames
{
RawDepthFrameInfo = request.Body.IncludeDepth ? rawFrames.RawDepthFrameInfo : null,
RawDepthFrameData = request.Body.IncludeDepth ? rawFrames.RawDepthFrameData : null,
RawColorFrameInfo = request.Body.IncludeVideo ? rawFrames.RawColorFrameInfo : null,
RawColorFrameData = request.Body.IncludeVideo ? rawFrames.RawColorFrameData : null,
};
request.ResponsePort.Post(new kinect.GetRawFrameResponse {
RawFrames = response
});
}
}
finally
{
this.pendingFrameRequests.Clear();
var timeToNextFrame = this.pollInterval - stopwatch.Elapsed;
if (timeToNextFrame <= TimeSpan.Zero)
{
this.pollPort.Post(DateTime.Now);
}
else
{
TaskQueue.EnqueueTimer(timeToNextFrame, this.pollPort);
}
}
}
/// <summary>
/// Tries to subscribe to an auto partner single contract, or throws an exception
/// if we don't support the contract. Also starts a 1-second poll that continues
/// until publishers send non-zero-length arrays.
/// </summary>
/// <param name="def"></param>
/// <param name="serviceInfo"></param>
private void subscribeAutoSingle(AutoDefinition def, ServiceInfoType serviceInfo)
{
// Check for each contract we know about and subscribe.
////////////////// Analog Sensor
if (serviceInfo.Contract.Equals(analog.Contract.Identifier))
{
var partnerPort = ServiceForwarder <analog.AnalogSensorOperations>(new Uri(serviceInfo.Service));
var notifyPort = new Port <analog.Replace>();
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analog.Replace)),
delegate(SubscribeResponseType success)
{
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive(true, notifyPort,
delegate(analog.Replace replace)
{ autoSubscribeNotificationHelper(def, new List <double>(1)
{
replace.Body.RawMeasurement
}, replace.Body.TimeStamp, false); }
)),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
////////////////// Analog Sensor Array
else if (serviceInfo.Contract.Equals(analogArray.Contract.Identifier))
{
var partnerPort = ServiceForwarder <analogArray.AnalogSensorOperations>(new Uri(serviceInfo.Service));
var notifyPort = new Port <analogArray.Replace>();
var pollPort = base.TimeoutPort(1000);
bool gotNonzeroState = false;
Activate(Arbiter.Receive(true, pollPort,
delegate(DateTime time)
{
if (gotNonzeroState == false)
{
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(analogArray.AnalogSensorArrayState state) { notifyPort.Post(new analogArray.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
}
}));
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analogArray.Replace)),
delegate(SubscribeResponseType success)
{
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive(true, notifyPort,
delegate(analogArray.Replace replace)
{
if (replace.Body.Sensors.Count > 0)
{
gotNonzeroState = true;
}
autoSubscribeNotificationHelper(def,
new List <double>(from s in replace.Body.Sensors select s.RawMeasurement),
(replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), false);
})),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
////////////////// Contact Sensor
else if (serviceInfo.Contract.Equals(contact.Contract.Identifier))
{
var partnerPort = ServiceForwarder <contact.ContactSensorArrayOperations>(new Uri(serviceInfo.Service));
var notifyPort = new PortSet <contact.Replace, contact.Update>();
// Post an initial Replace notification to update the HW ID map (no, this is now done automatically)
//notifyPort.Post(new contact.Replace(RSUtils.ReceiveSync(TaskQueue, partnerPort.Get(), Params.defaultRecieveTimeout)));
var pollPort = base.TimeoutPort(1000);
bool gotNonzeroState = false;
Activate(Arbiter.Receive(true, pollPort,
delegate(DateTime time)
{
if (gotNonzeroState == false)
{
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
}
}));
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(contact.Replace), typeof(contact.Update)),
delegate(SubscribeResponseType success)
{
var contactHWIDMap = new Dictionary <int, int>();
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <contact.Replace>(true, notifyPort,
delegate(contact.Replace replace)
{
if (replace.Body.Sensors.Count > 0)
{
gotNonzeroState = true;
}
if (def.hwKeys.Count < replace.Body.Sensors.Count)
{
def.hwKeys.Capacity = replace.Body.Sensors.Count;
while (def.hwKeys.Count < replace.Body.Sensors.Count)
{
def.hwKeys.Add("");
}
}
// Refresh the HW ID map
contactHWIDMap.Clear();
for (int i = 0; i < replace.Body.Sensors.Count; i++)
{
contactHWIDMap.Add(replace.Body.Sensors[i].HardwareIdentifier, i);
//if (def.hwKeys[i] == null || def.hwKeys[i].Length <= 0)
def.hwKeys[i] = replace.Body.Sensors[i].Name;
}
autoSubscribeNotificationHelper(def,
new List <double>(from s in replace.Body.Sensors select(s.Pressed ? 1.0 : 0.0)),
(replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), true);
}),
Arbiter.Receive <contact.Update>(true, notifyPort,
delegate(contact.Update update)
{
gotNonzeroState = true;
try
{
int i = contactHWIDMap[update.Body.HardwareIdentifier];
autoSubscribeNotificationUpdateHelper(def, i, (update.Body.Pressed ? 1.0 : 0.0), update.Body.TimeStamp);
}
catch (KeyNotFoundException)
{
LogError("Vector got update for contact sensor hardware identifier " + update.Body.HardwareIdentifier + " and doesn't know about this hardware ID. Trying to fetch entire state.");
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get contact state", failure); }));
}
})),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
else
{
throw new UnrecognizedContractException();
}
}
