/// <summary>
/// Convert raw skeletal structure into one we can visualize (with window coordinates) and process in Behavior.
/// The result is a list of seven VisualizableSkeletonInformation objects, some having IsSkeletonActive=true and other properties filled.
/// </summary>
/// <returns>CCR Iterator</returns>
public IEnumerator <ITask> ProcessSkeletons()
{
// while calculating the frame, we operate on a freshly allocated list of seven skeletons:
AllocateSkeletonsTemp();
int skeletonIndex = 0;
int mainSkeletonIndex = -1;
double minSkelDistance = double.MaxValue;
foreach (Skeleton skel in this.RawFrames.RawSkeletonFrameData.SkeletonData)
{
VisualizableSkeletonInformation vsi = this.SkeletonsTemp[skeletonIndex];
vsi.IsSkeletonActive = false;
vsi.SkeletonQuality = string.Empty;
vsi.SkeletonPose = SkeletonPose.None;
// skeleton is tracked, head and jointTypeOfInterest clearly visible:
if (SkeletonTrackingState.Tracked == skel.TrackingState &&
skel.Joints[JointType.Head].TrackingState == JointTrackingState.Tracked &&
skel.Joints[jointTypeOfInterest].TrackingState == JointTrackingState.Tracked)
{
vsi.IsSkeletonActive = true;
vsi.SkeletonQuality = skel.ClippedEdges.ToString();
vsi.TrackingId = skel.TrackingId; // see http://msdn.microsoft.com/en-us/library/jj131025.aspx#Active_User_Tracking
if (skel.Joints[JointType.FootLeft].TrackingState == JointTrackingState.Tracked && skel.Joints[JointType.FootRight].TrackingState == JointTrackingState.Tracked)
{
vsi.SkeletonSizeMeters = skel.Joints[JointType.Head].Position.Y - (skel.Joints[JointType.FootLeft].Position.Y + skel.Joints[JointType.FootRight].Position.Y) / 2.0d + 0.08d; // plus head diameter
}
vsi.detectSkeletonPose(skel); // fills vsi.SkeletonPose
// Populate joint poitns and compute Pan Tilt and DistanceMeters:
foreach (Joint joint in skel.Joints)
{
yield return(new IterativeTask <Joint>(joint, this.JointToPointCoordinates));
vsi.JointPoints[joint.JointType].JointCoordinates = this.cachedJointPoint;
vsi.JointPoints[joint.JointType].TrackingState = joint.TrackingState;
VisualizableJoint vj = vsi.JointPoints[joint.JointType];
vj.JointCoordinates = this.cachedJointPoint;
vj.TrackingState = joint.TrackingState;
vj.X = skel.Joints[joint.JointType].Position.X / KinectNykoGlassesFactor;
vj.Y = skel.Joints[joint.JointType].Position.Y / KinectNykoGlassesFactor;
vj.Z = skel.Joints[joint.JointType].Position.Z / KinectNykoGlassesFactor;
vj.ComputePanTilt();
if (joint.JointType == jointTypeOfInterest)
{
vj.IsJointOfInterest = true;
vsi.DistanceMeters = vj.Z;
if (vsi.DistanceMeters < minSkelDistance)
{
minSkelDistance = vsi.DistanceMeters;
mainSkeletonIndex = skeletonIndex;
}
}
}
if (skeletonIndex < _state.HumanInteractionStates.Length)
{
HumanInteractionState his = _state.HumanInteractionStates[skeletonIndex];
VisualizableJoint jointOfInterest = vsi.JointPoints[jointTypeOfInterest];
his.IsTracked = true;
his.TrackingId = skel.TrackingId;
his.IsMain = false;
his.TimeStamp = DateTime.Now;
his.DirectionPan = jointOfInterest.Pan;
his.DirectionTilt = jointOfInterest.Tilt;
his.DistanceMeters = vsi.DistanceMeters;
}
}
else
{
if (skeletonIndex < _state.HumanInteractionStates.Length)
{
HumanInteractionState his = _state.HumanInteractionStates[skeletonIndex];
his.IsTracked = false;
his.IsMain = false;
}
}
skeletonIndex++;
}
if (mainSkeletonIndex >= 0)
{
VisualizableSkeletonInformation vsi = this.SkeletonsTemp[mainSkeletonIndex];
vsi.IsMainSkeleton = true;
if (mainSkeletonIndex < _state.HumanInteractionStates.Length)
{
HumanInteractionState his = _state.HumanInteractionStates[mainSkeletonIndex];
his.IsMain = true;
}
}
// make the result available for outside consumption:
AllSkeletons = SkeletonsTemp;
yield break;
}
public void Init()
{
Dropping = false;
IsTurning = false;
LastTurnStarted = DateTime.MinValue;
LastTurnCompleted = DateTime.MinValue;
if (WheelsEncoderState == null)
{
WheelsEncoderState = new WheelsEncoderState();
}
if (collisionState == null)
{
collisionState = new CollisionState();
}
if (gpsState == null)
{
gpsState = new GpsState();
}
if (MostRecentAnalogValues == null)
{
MostRecentAnalogValues = new proxibrick.AnalogDataDssSerializable()
{
TimeStamp = DateTime.MinValue
};
}
MostRecentLaserTimeStamp = DateTime.Now;
if (VoiceCommandState == null)
{
VoiceCommandState = new VoiceCommandState();
}
int MAX_HUMANS_TO_TRACK = 7; // FrameProcessor preallocates 7
HumanInteractionStates = new HumanInteractionState[MAX_HUMANS_TO_TRACK];
for (int i = 0; i < HumanInteractionStates.Length; i++)
{
HumanInteractionStates[i] = new HumanInteractionState();
}
if (followDirectionPidControllerAngularSpeed == null)
{
followDirectionPidControllerAngularSpeed = new PIDController()
{
Name = "AngularSpeed",
MaxIntegralError = 180.0d, // degrees; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 100.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (followDirectionPidControllerLinearSpeed == null)
{
followDirectionPidControllerLinearSpeed = new PIDController()
{
Name = "LinearSpeed",
MaxIntegralError = 2000.0d, // mm/sec; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 1000.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (PowerScale == 0.0d)
{
PowerScale = 0.5d;
}
}
public void Init()
{
Dropping = false;
IsTurning = false;
LastTurnStarted = DateTime.MinValue;
LastTurnCompleted = DateTime.MinValue;
if (WheelsEncoderState == null)
{
WheelsEncoderState = new WheelsEncoderState();
}
if (collisionState == null)
{
collisionState = new CollisionState();
}
if (gpsState == null)
{
gpsState = new GpsState();
}
if (MostRecentAnalogValues == null)
{
MostRecentAnalogValues = new proxibrick.AnalogDataDssSerializable() { TimeStamp = DateTime.MinValue };
}
MostRecentLaserTimeStamp = DateTime.Now;
if (VoiceCommandState == null)
{
VoiceCommandState = new VoiceCommandState();
}
int MAX_HUMANS_TO_TRACK = 7; // FrameProcessor preallocates 7
HumanInteractionStates = new HumanInteractionState[MAX_HUMANS_TO_TRACK];
for (int i = 0; i < HumanInteractionStates.Length; i++)
{
HumanInteractionStates[i] = new HumanInteractionState();
}
if (followDirectionPidControllerAngularSpeed == null)
{
followDirectionPidControllerAngularSpeed = new PIDController()
{
Name = "AngularSpeed",
MaxIntegralError = 180.0d, // degrees; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 100.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (followDirectionPidControllerLinearSpeed == null)
{
followDirectionPidControllerLinearSpeed = new PIDController()
{
Name = "LinearSpeed",
MaxIntegralError = 2000.0d, // mm/sec; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 1000.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (PowerScale == 0.0d)
{
PowerScale = 0.5d;
}
}
