Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial checkin of unified hierarchy of WPILib 2015

Browse Source
This commit is contained in:
Brad Miller
2013-12-15 18:30:16 -05:00
commit 3178911eef
1560 changed files with 410007 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

196
wpilibc/src/main/native/Kinect.cpp Normal file
View File

@@ -0,0 +1,196 @@
/*----------------------------------------------------------------------------*/
/* 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 "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <cstring>
#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 = initializeMutex(SEMAPHORE_Q_PRIORITY | SEMAPHORE_INVERSION_SAFE | SEMAPHORE_DELETE_SAFE);
nUsageReporting::report(nUsageReporting::kResourceType_Kinect, 0);
}
Kinect::~Kinect()
{
takeMutex(m_dataLock, SEMAPHORE_WAIT_FOREVER);
deleteMutex(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_t 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_t 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
}
}