/// <summary>
/// Called when the heart beat timer expires. Sends a "blank" packet with player
/// count and Joystick data set to 0.
/// </summary>
/// <param name="stateInfo"></param>
private void HeartBeatExpired(Object stateInfo)
{
sbyte[] nullAxis = new sbyte[6];
m_version1Packet.PlayerCount.Set(0);
m_version1Packet.Joystick1.Set(nullAxis, 0);
m_version1Packet.Joystick2.Set(nullAxis, 0);
Send();
NTKinect.UpdateJoysticks(0.0, 0.0, 0.0, 0.0);
}
/// <summary>
/// Manages sending Kinect information as UDP to the specified hostname and port
/// at least once a second.
///
/// Uses the FIRST gesture processor to process gestures into joysticks.
/// </summary>
/// <param name="kinectVersion">The version string to report to the Driver Station.</param>
/// <param name="hostname">The destination hostname.</param>
/// <param name="port">The destination port number.</param>
public KinectProtocol_v1Manager(String kinectVersion, String hostname, int port)
{
m_kinectVersion = kinectVersion;
m_kinectStatus = "No Kinect";
m_hostname = hostname;
m_port = port;
m_udpClient = new UdpClient();
m_version1Packet = new KinectProtocol_v1();
m_heartbeatTimer = new Timer(this.HeartBeatExpired);
m_heartbeatTimer.Change(HEARTBEAT_PERIOD_MS, HEARTBEAT_PERIOD_MS);
m_gestureProcessor = new FIRSTGestureProcessor();
NTKinect.Init();
}
/// <summary>
/// Cuts a new UDP packet and sends it to the Driver Station.
/// </summary>
private void Send()
{
using (MemoryStream udpbuffer = new MemoryStream())
{
NetworkOrderBinaryWriter writer = new NetworkOrderBinaryWriter(udpbuffer);
lock (m_version1Packet)
{
m_version1Packet.VersionNumber.Set(m_kinectStatus);
m_version1Packet.Serialize(writer);
NTKinect.UpdateFromPacket(m_version1Packet);
}
m_udpClient.Send(udpbuffer.GetBuffer(), (int)udpbuffer.Length, m_hostname, m_port);
}
}
/// <summary>
/// Processes the given skeleton and updates the network packet data structure, then
/// sends the resulting packet.
/// </summary>
/// <param name="frame">The Kinect Skeleton frame to process.</param>
public void ProcessSkeletonData(SkeletonFrame frame)
{
if ((m_skeletonArray == null) || (m_skeletonArray.Length != frame.SkeletonArrayLength))
{
m_skeletonArray = new Skeleton[frame.SkeletonArrayLength];
}
frame.CopySkeletonDataTo(m_skeletonArray);
// Do stuff here
lock (m_version1Packet)
{
UpdatePacketGlobalData(frame);
// Get the best skeleton
Skeleton skeleton = KinectUtils.SelectBestSkeleton(m_skeletonArray);
UpdatePacketSkeletonData(skeleton);
UpdatePacketJoystickData(skeleton);
}
Send();
m_heartbeatTimer.Change(HEARTBEAT_PERIOD_MS, HEARTBEAT_PERIOD_MS);
NTKinect.UpdateHeartBeat();
}
/// <summary>
/// Processes a skeleton into joystick data using the default FIRST gestures.
/// </summary>
/// <param name="joy">Vector of Joysticks to put the result in</param>
/// <param name="skeleton">The skeleton to process</param>
public void ProcessGestures(Networking.WritableElements.WritableJoystick[] joy, Microsoft.Kinect.Skeleton skeleton)
{
// Check edge cases
if (joy == null || joy.Length < 2 || joy[0] == null || joy[1] == null)
{
return;
}
sbyte[] leftAxis = new sbyte[6];
sbyte[] rightAxis = new sbyte[6];
sbyte[] nullAxis = new sbyte[6];
bool dataWithinExpectedRange;
ushort buttons = 0;
double leftAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderLeft].Position,
skeleton.Joints[JointType.WristLeft].Position,
true));
double rightAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderRight].Position,
skeleton.Joints[JointType.WristRight].Position));
dataWithinExpectedRange = leftAngle <ARM_MAX_ANGLE && leftAngle> ARM_MIN_ANGLE &&
rightAngle <ARM_MAX_ANGLE && rightAngle> ARM_MIN_ANGLE;
double leftYAxis = CoerceToRange(leftAngle,
-70,
70,
-127,
128);
double rightYAxis = CoerceToRange(rightAngle,
-70,
70,
-127,
128);
dataWithinExpectedRange = dataWithinExpectedRange &&
InSameZPlane(skeleton.Joints[JointType.ShoulderLeft].Position,
skeleton.Joints[JointType.WristLeft].Position,
Z_PLANE_TOLERANCE) &&
InSameZPlane(skeleton.Joints[JointType.ShoulderRight].Position,
skeleton.Joints[JointType.WristRight].Position,
Z_PLANE_TOLERANCE);
// Head buttons
double headAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderCenter].Position,
skeleton.Joints[JointType.Head].Position));
if (IsHeadRight(headAngle))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn1;
}
if (IsHeadLeft(headAngle))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn2;
}
// Right Leg XY Button
double rightLegAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.HipRight].Position,
skeleton.Joints[JointType.AnkleRight].Position));
if (IsLegOut(rightLegAngle))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn3;
}
// Left Leg XY Button
double leftLegAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.HipLeft].Position,
skeleton.Joints[JointType.AnkleLeft].Position,
true));
if (IsLegOut(leftLegAngle))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn4;
}
// Right Leg YZ Buttons
double rightLegYZ = RadToDeg(AngleYZ(skeleton.Joints[JointType.HipRight].Position,
skeleton.Joints[JointType.AnkleRight].Position));
if (IsLegForward(rightLegYZ))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn5;
}
if (IsLegBackward(rightLegYZ))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn6;
}
// Left Leg YZ Buttons
double leftLegYZ = RadToDeg(AngleYZ(skeleton.Joints[JointType.HipLeft].Position,
skeleton.Joints[JointType.AnkleLeft].Position));
if (IsLegForward(leftLegYZ))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn7;
}
if (IsLegBackward(leftLegYZ))
{
buttons |= (ushort)WritableJoystick.Buttons.Btn8;
}
if (dataWithinExpectedRange)
{
// Invert joystick axis to match a real joystick
// (pushing away creates negative values)
leftAxis[(uint)WritableJoystick.Axis.Y] = (sbyte)-leftYAxis;
rightAxis[(uint)WritableJoystick.Axis.Y] = (sbyte)-rightYAxis;
//Use "Button 9" as Kinect control enabled signal
buttons |= (ushort)WritableJoystick.Buttons.Btn9;
joy[0].Set(leftAxis, buttons);
joy[1].Set(rightAxis, buttons);
NTKinect.UpdateJoysticks(0.0, leftYAxis, 0.0, rightYAxis);
}
else
{
joy[0].Set(nullAxis, 0);
joy[1].Set(nullAxis, 0);
NTKinect.UpdateJoysticks(0.0, 0.0, 0.0, 0.0);
}
}
