Kinect.cpp

/*----------------------------------------------------------------------------*/
/* 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 "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);
}

Kinect::~Kinect()
{
        semTake(m_dataLock, WAIT_FOREVER);
        semDelete(m_dataLock);
}

Kinect *Kinect::GetInstance()
{
        if (_instance == NULL)
                _instance = new Kinect();
        return _instance;
}

int Kinect::GetNumberOfPlayers()
{
        UpdateData();
        return m_numberOfPlayers;
}

Kinect::Point4 Kinect::GetFloorClipPlane()
{
        UpdateData();
        return m_floorClipPlane;
}

Kinect::Point4 Kinect::GetGravityNormal()
{
        UpdateData();
        return m_gravityNormal;
}

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

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

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

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

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