/// <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; } }