/// <summary>
/// Main read loop
/// Read raw frame from Kinect service, then process it asynchronously, then request UI update
/// </summary>
/// <returns>A standard CCR iterator.</returns>
private IEnumerator<ITask> ReadKinectLoop()
{
// note: see frame rate at C:\Microsoft Robotics Dev Studio 4\projects\TrackRoamer\TrackRoamerServices\Config\TrackRoamer.TrackRoamerBot.Kinect.Config.xml
while (true)
{
try
{
kinectProxy.QueryRawFrameRequest frameRequest = new kinectProxy.QueryRawFrameRequest();
frameRequest.IncludeDepth = this.IncludeDepth;
frameRequest.IncludeVideo = this.IncludeVideo;
frameRequest.IncludeSkeletons = this.IncludeSkeletons;
if (!this.IncludeDepth && !this.IncludeVideo && !this.IncludeSkeletons)
{
// poll 2 times a sec if user for some reason deselected all image options (this would turn into a busy loop then)
yield return TimeoutPort(KinectLoopWaitIntervalMs).Receive();
}
kinect.RawKinectFrames rawFrames = null;
// poll depth camera
yield return this.kinectPort.QueryRawFrame(frameRequest).Choice(
rawFrameResponse =>
{
rawFrames = rawFrameResponse.RawFrames;
},
failure =>
{
if (!this.atLeastOneFrameQueryFailed)
{
this.atLeastOneFrameQueryFailed = true;
LogError(failure);
}
});
this.frameProcessor.currentPanKinect = _state.currentPanKinect;
this.frameProcessor.currentTiltKinect = _state.currentTiltKinect;
this.frameProcessor.SetRawFrame(rawFrames);
if (null != rawFrames.RawSkeletonFrameData)
{
yield return new IterativeTask(this.frameProcessor.ProcessSkeletons);
var tmpAllSkeletons = frameProcessor.AllSkeletons; // get a snapshot of the pointer to allocated array, and then take sweet time processing it knowing it will not change
if (tmpAllSkeletons != null)
{
// tmpAllSkeletons is a list of seven skeletons, those good enough for processing have IsSkeletonActive true
var skels = from s in tmpAllSkeletons
where s.IsSkeletonActive
select s;
foreach (VisualizableSkeletonInformation skel in skels)
{
// Kinect Z goes straight forward, X - to the left side, Y - up
double kZ = skel.JointPoints[nui.JointType.Spine].Z; // meters, relative to Kinect camera
double kX = skel.JointPoints[nui.JointType.Spine].X;
GeoPosition pos1 = (GeoPosition)_mapperVicinity.robotPosition.Clone();
double relBearing = Direction.to180fromRad(Math.Atan2(-kX, kZ));
double rangeMeters = Math.Sqrt(kZ * kZ + kX * kX);
pos1.translate(new Direction() { heading = _mapperVicinity.robotDirection.heading, bearingRelative = relBearing }, new Distance(rangeMeters));
DetectedHuman dhum1 = new DetectedHuman()
{
geoPosition = pos1,
firstSeen = DateTime.Now.Ticks,
lastSeen = DateTime.Now.Ticks,
detectorType = DetectorType.KINECT_SKELETON,
};
lock (_mapperVicinity)
{
_mapperVicinity.Add(dhum1);
}
}
}
}
if (null != rawFrames.RawColorFrameData)
{
yield return new IterativeTask(this.frameProcessor.ProcessImageFrame); // RGB Video
}
if (null != rawFrames.RawDepthFrameData)
{
yield return new IterativeTask(this.frameProcessor.ProcessDepthFrame); // Depth information frame
}
this.UpdateUI(this.frameProcessor);
Decide(SensorEventSource.Kinect);
// poll state at low frequency to see if tilt has shifted (may happen on an actual robot due to shaking)
if (common.Utilities.ElapsedSecondsSinceStart - this.lastStateReadTime > 1)
{
yield return this.kinectPort.Get().Choice(
kinectState =>
{
this.UpdateState(kinectState); // update value displayed in WPF window
_state.currentTiltKinect = kinectState.TiltDegrees;
},
failure =>
{
if (!this.atLeastOneTiltPollFailed)
{
this.atLeastOneTiltPollFailed = true;
LogError(failure);
}
});
this.lastStateReadTime = common.Utilities.ElapsedSecondsSinceStart;
}
}
finally
{
}
}
}
/// <summary>
/// Main read loop
/// Read raw frame from Kinect service, then process it asynchronously, then request UI update
/// </summary>
/// <returns>A standard CCR iterator.</returns>
private IEnumerator <ITask> ReadKinectLoop()
{
while (true)
{
kinectProxy.QueryRawFrameRequest frameRequest = new kinectProxy.QueryRawFrameRequest();
frameRequest.IncludeDepth = this.IncludeDepth;
frameRequest.IncludeVideo = this.IncludeVideo;
frameRequest.IncludeSkeletons = this.IncludeSkeletons;
if (!this.IncludeDepth && !this.IncludeVideo && !this.IncludeSkeletons)
{
// poll 5 times a sec if user for some reason deselected all image options (this would turn
// into a busy loop then)
yield return(TimeoutPort(200).Receive());
}
kinect.RawKinectFrames rawFrames = null;
// poll depth camera
yield return(this.kinectPort.QueryRawFrame(frameRequest).Choice(
rawFrameResponse =>
{
rawFrames = rawFrameResponse.RawFrames;
},
failure =>
{
if (!this.frameQueryFailed)
{
this.frameQueryFailed = true;
LogError(failure);
}
}));
this.frameProcessor.SetRawFrame(rawFrames);
if (null != rawFrames.RawSkeletonFrameData)
{
yield return(new IterativeTask(this.frameProcessor.ProcessSkeletons));
}
this.UpdateUI(this.frameProcessor);
// poll state at low frequency to see if tilt has shifted (may happen on an actual robot due to shaking)
if (common.Utilities.ElapsedSecondsSinceStart - this.lastStateReadTime > 1)
{
yield return(this.kinectPort.Get().Choice(
kinectState =>
{
this.UpdateState(kinectState);
},
failure =>
{
if (!this.tiltPollFailed)
{
this.tiltPollFailed = true;
LogError(failure);
}
}));
this.lastStateReadTime = common.Utilities.ElapsedSecondsSinceStart;
}
}
}
/// <summary>
/// Main read loop
/// Read raw frame from Kinect service, then process it asynchronously, then request UI update
/// </summary>
/// <returns>A standard CCR iterator.</returns>
private IEnumerator <ITask> ReadKinectLoop()
{
// note: see frame rate at C:\Microsoft Robotics Dev Studio 4\projects\TrackRoamer\TrackRoamerServices\Config\TrackRoamer.TrackRoamerBot.Kinect.Config.xml
while (true)
{
try
{
kinectProxy.QueryRawFrameRequest frameRequest = new kinectProxy.QueryRawFrameRequest();
frameRequest.IncludeDepth = this.IncludeDepth;
frameRequest.IncludeVideo = this.IncludeVideo;
frameRequest.IncludeSkeletons = this.IncludeSkeletons;
if (!this.IncludeDepth && !this.IncludeVideo && !this.IncludeSkeletons)
{
// poll 2 times a sec if user for some reason deselected all image options (this would turn into a busy loop then)
yield return(TimeoutPort(KinectLoopWaitIntervalMs).Receive());
}
kinect.RawKinectFrames rawFrames = null;
// poll depth camera
yield return(this.kinectPort.QueryRawFrame(frameRequest).Choice(
rawFrameResponse =>
{
rawFrames = rawFrameResponse.RawFrames;
},
failure =>
{
if (!this.atLeastOneFrameQueryFailed)
{
this.atLeastOneFrameQueryFailed = true;
LogError(failure);
}
}));
this.frameProcessor.currentPanKinect = _state.currentPanKinect;
this.frameProcessor.currentTiltKinect = _state.currentTiltKinect;
this.frameProcessor.SetRawFrame(rawFrames);
if (null != rawFrames.RawSkeletonFrameData)
{
yield return(new IterativeTask(this.frameProcessor.ProcessSkeletons));
var tmpAllSkeletons = frameProcessor.AllSkeletons; // get a snapshot of the pointer to allocated array, and then take sweet time processing it knowing it will not change
if (tmpAllSkeletons != null)
{
// tmpAllSkeletons is a list of seven skeletons, those good enough for processing have IsSkeletonActive true
var skels = from s in tmpAllSkeletons
where s.IsSkeletonActive
select s;
foreach (VisualizableSkeletonInformation skel in skels)
{
// Kinect Z goes straight forward, X - to the left side, Y - up
double kZ = skel.JointPoints[nui.JointType.Spine].Z; // meters, relative to Kinect camera
double kX = skel.JointPoints[nui.JointType.Spine].X;
GeoPosition pos1 = (GeoPosition)_mapperVicinity.robotPosition.Clone();
double relBearing = Direction.to180fromRad(Math.Atan2(-kX, kZ));
double rangeMeters = Math.Sqrt(kZ * kZ + kX * kX);
pos1.translate(new Direction()
{
heading = _mapperVicinity.robotDirection.heading, bearingRelative = relBearing
}, new Distance(rangeMeters));
DetectedHuman dhum1 = new DetectedHuman()
{
geoPosition = pos1,
firstSeen = DateTime.Now.Ticks,
lastSeen = DateTime.Now.Ticks,
detectorType = DetectorType.KINECT_SKELETON,
};
lock (_mapperVicinity)
{
_mapperVicinity.Add(dhum1);
}
}
}
}
if (null != rawFrames.RawColorFrameData)
{
yield return(new IterativeTask(this.frameProcessor.ProcessImageFrame)); // RGB Video
}
if (null != rawFrames.RawDepthFrameData)
{
yield return(new IterativeTask(this.frameProcessor.ProcessDepthFrame)); // Depth information frame
}
this.UpdateUI(this.frameProcessor);
Decide(SensorEventSource.Kinect);
// poll state at low frequency to see if tilt has shifted (may happen on an actual robot due to shaking)
if (common.Utilities.ElapsedSecondsSinceStart - this.lastStateReadTime > 1)
{
yield return(this.kinectPort.Get().Choice(
kinectState =>
{
this.UpdateState(kinectState); // update value displayed in WPF window
_state.currentTiltKinect = kinectState.TiltDegrees;
},
failure =>
{
if (!this.atLeastOneTiltPollFailed)
{
this.atLeastOneTiltPollFailed = true;
LogError(failure);
}
}));
this.lastStateReadTime = common.Utilities.ElapsedSecondsSinceStart;
}
}
finally
{
}
}
}
