This is the latest WPILib src, VisionSample2013, cRIO image, ... pulled down from firstforge.wpi.edu.
There might be risks in using the top of tree rather than an official release, but the commit messages do mention fixes for some deadlocks and race conditions.
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4066 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Kinect.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Kinect.cpp
new file mode 100644
index 0000000..3f1c931
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Kinect.cpp
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Kinect.h"
+
+#include "DriverStation.h"
+#include "NetworkCommunication/FRCComm.h"
+#include "NetworkCommunication/UsageReporting.h"
+#include "Skeleton.h"
+#include "Synchronized.h"
+#include "WPIErrors.h"
+
+#define kHeaderBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Header
+#define kSkeletonExtraBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Extra1
+#define kSkeletonBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Vertices1
+
+Kinect *Kinect::_instance = NULL;
+
+Kinect::Kinect() :
+ m_recentPacketNumber(0),
+ m_numberOfPlayers(0)
+{
+ AddToSingletonList();
+ m_dataLock = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);
+
+ nUsageReporting::report(nUsageReporting::kResourceType_Kinect, 0);
+}
+
+Kinect::~Kinect()
+{
+ semTake(m_dataLock, WAIT_FOREVER);
+ semDelete(m_dataLock);
+}
+
+/**
+ * Get the one and only Kinect object
+ * @returns pointer to a Kinect
+ */
+Kinect *Kinect::GetInstance()
+{
+ if (_instance == NULL)
+ _instance = new Kinect();
+ return _instance;
+}
+
+/**
+ * Get the number of tracked players on the Kinect
+ * @return the number of players being actively tracked
+ */
+int Kinect::GetNumberOfPlayers()
+{
+ UpdateData();
+ return m_numberOfPlayers;
+}
+
+/**
+ * Get the floor clip plane as defined in the Kinect SDK
+ * @return The floor clip plane
+ */
+Kinect::Point4 Kinect::GetFloorClipPlane()
+{
+ UpdateData();
+ return m_floorClipPlane;
+}
+
+/**
+ * Get the gravity normal from the kinect as defined in the Kinect SDK
+ * @return The gravity normal (w is ignored)
+ */
+Kinect::Point4 Kinect::GetGravityNormal()
+{
+ UpdateData();
+ return m_gravityNormal;
+}
+
+/**
+ * Get the skeleton data
+ * Returns the detected skeleton data from the kinect as defined in the Kinect SDK
+ * @param skeletonIndex Which of (potentially 2) skeletons to return. This is ignored in this implementation and
+ * only a single skeleton is supported for the FRC release default gesture interpretation.
+ * @return The current version of the skeleton object.
+ */
+Skeleton Kinect::GetSkeleton(int skeletonIndex)
+{
+ if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
+ {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
+ return Skeleton();
+ }
+ UpdateData();
+ return m_skeletons[skeletonIndex-1];
+}
+
+/**
+ * Get the current position of the skeleton
+ * @param skeletonIndex the skeleton to read from
+ * @return the current position as defined in the Kinect SDK (w is ignored)
+ */
+Kinect::Point4 Kinect::GetPosition(int skeletonIndex)
+{
+ if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
+ {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
+ return Point4();
+ }
+ UpdateData();
+ return m_position[skeletonIndex-1];
+}
+
+/**
+ * Get the quality of the skeleton.
+ * Quality masks are defined in the SkeletonQuality enum
+ * @param skeletonIndex the skeleton to read from
+ * @return the quality value as defined in the Kinect SDK
+ */
+UINT32 Kinect::GetQuality(int skeletonIndex)
+{
+ if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
+ {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
+ return kClippedRight | kClippedLeft | kClippedTop | kClippedBottom;
+ }
+ UpdateData();
+ return m_quality[skeletonIndex-1];
+}
+
+/**
+ * Get the TrackingState of the skeleton.
+ * Tracking states are defined in the SkeletonTrackingState enum
+ * @param skeletonIndex the skeleton to read from
+ * @return the tracking state value as defined in the Kinect SDK
+ */
+Kinect::SkeletonTrackingState Kinect::GetTrackingState(int skeletonIndex)
+{
+ if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
+ {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
+ return kNotTracked;
+ }
+ UpdateData();
+ return m_trackingState[skeletonIndex-1];
+}
+
+/**
+ * Check for an update of new data from the Driver Station
+ * This will read the new values and update the data structures in this class.
+ */
+void Kinect::UpdateData()
+{
+ Synchronized sync(m_dataLock);
+ UINT32 packetNumber = DriverStation::GetInstance()->GetPacketNumber();
+ if (m_recentPacketNumber != packetNumber)
+ {
+ m_recentPacketNumber = packetNumber;
+ int retVal = getDynamicControlData(kHeaderBundleID, m_rawHeader, sizeof(m_rawHeader), 5);
+ if(retVal == 0)
+ {
+ m_numberOfPlayers = (int)m_rawHeader[13];
+ memcpy(&m_floorClipPlane.x, &m_rawHeader[18], 4);
+ memcpy(&m_floorClipPlane.y, &m_rawHeader[22], 4);
+ memcpy(&m_floorClipPlane.z, &m_rawHeader[26], 4);
+ memcpy(&m_floorClipPlane.w, &m_rawHeader[30], 4);
+ memcpy(&m_gravityNormal.x, &m_rawHeader[34], 4);
+ memcpy(&m_gravityNormal.y, &m_rawHeader[38], 4);
+ memcpy(&m_gravityNormal.z, &m_rawHeader[42], 4);
+ }
+
+ retVal = getDynamicControlData(kSkeletonExtraBundleID, m_rawSkeletonExtra, sizeof(m_rawSkeletonExtra), 5);
+ if(retVal == 0)
+ {
+ memcpy(&m_position[0].x, &m_rawSkeletonExtra[22], 4);
+ memcpy(&m_position[0].y, &m_rawSkeletonExtra[26], 4);
+ memcpy(&m_position[0].z, &m_rawSkeletonExtra[30], 4);
+ memcpy(&m_quality[0], &m_rawSkeletonExtra[34], 4);
+ memcpy(&m_trackingState[0], &m_rawSkeletonExtra[38], 4);
+ }
+
+ retVal = getDynamicControlData(kSkeletonBundleID, m_rawSkeleton, sizeof(m_rawSkeleton), 5);
+ if(retVal == 0)
+ {
+ for(int i=0; i < Skeleton::JointCount; i++)
+ {
+ memcpy(&m_skeletons[0].m_joints[i].x, &m_rawSkeleton[i*12+2], 4);
+ memcpy(&m_skeletons[0].m_joints[i].y, &m_rawSkeleton[i*12+6], 4);
+ memcpy(&m_skeletons[0].m_joints[i].z, &m_rawSkeleton[i*12+10], 4);
+ m_skeletons[0].m_joints[i].trackingState = (Skeleton::JointTrackingState)m_rawSkeletonExtra[i+2];
+ }
+ }
+
+ // TODO: Read skeleton #2
+ }
+}