G:/work/subversion/scol-technologies/trunk/plugins/OpenNiSCOL/OpenNiSCOL/objects/KinectDevice.cpp

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2010 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

You may alternatively use this source under the terms of a specific version of
the OpenSpace3D Unrestricted License provided you have obtained such a license from
I-maginer.
-----------------------------------------------------------------------------
*/

#include "KinectDevice.h"

#include "../core/Exception.h"
#include "../generator/Depth.h"
#include "../generator/Image.h"
#include "../generator/User.h"
#include "../generator/Gesture.h"
#include "../generator/Hands.h"

#include "../DeviceManager.h"

#include "../ScolPlugin.h"

namespace NIDevice
{
  namespace Objects
  {
    KinectDevice::KinectDevice(unsigned int id) : Thread()
    {
      context = 0;
      mId = id;
      m_deviceNode = 0;
      bConnected = false;
      bGenerating = false;
      bInit = false;
      bMirror = false;
      iDistCutoff = 10000;
      fAngCutoff = 20.0f; //Kinect IR camera fov 57.8
      m_DepthGenerator = 0;
      m_ImageGenerator = 0;
      m_UserGenerator = 0;
      m_GestureGenerator = 0;
      m_HandsGenerator = 0;
      m_sklSmoothing = 0.6f;
      
      // NITE session manager
      sessionManager = 0;

      StartThreading(boost::bind(&KinectDevice::GoThread, this));
    } 

    KinectDevice::KinectDevice() : Thread()
    {
    }

    KinectDevice::~KinectDevice()
    {
      StopThreading();
      stopAndDestroyGenerators();
      bGenerating = false;
      destroyContext();
    }

    void KinectDevice::createContext()
    {
      //xn::EnumerationErrors errors;
      nRetVal = context.Init();
      if (nRetVal == XN_STATUS_OK)
        bInit = true;
    }

    void KinectDevice::destroyContext()
    {
      // Delete session manager data object
      SAFE_DELETE(sessionManager);

      context.Shutdown();
      bInit = false;
    }

    xn::Context& KinectDevice::GetContext()
    {
      assert(&context);
      return context;
    }

    xn::Context* KinectDevice::GetContextPtr()
    {
      return &context;
    }

    // Get session manager instance
    XnVSessionManager& KinectDevice::GetSessionManager()
    {
      assert(sessionManager);
      return *sessionManager;
    }

    XnVSessionManager* KinectDevice::GetSessionManagerPtr()
    {
      return sessionManager;
    }

    bool KinectDevice::IsInit()
    {
      return bInit;
    }

    bool KinectDevice::IsConnected()
    {
      return bConnected;
    }

    bool KinectDevice::IsGenerating()
    {
      return bGenerating;
    } 

    void KinectDevice::SetMirrorMode(bool state)
    {
      bMirror = state;
      //context.SetGlobalMirror(bMirror == true ? 1 : 0);
      if (m_DepthGenerator)
      {
        m_DepthGenerator->SetMirror(bMirror);
        m_ImageGenerator->SetMirror(bMirror);
        m_UserGenerator->SetMirror(bMirror);
        m_GestureGenerator->SetMirror(bMirror);
        m_HandsGenerator->SetMirror(bMirror);
      }
    }

    bool KinectDevice::GetMirrorMode()
    {
      return bMirror;
    }

    void KinectDevice::DestroyKinectUser(NIDevice::Objects::KinectUser* existingKinectUser)
    {
      listOfUsers.remove(existingKinectUser);
      SAFE_DELETE(existingKinectUser);
    }

    NIDevice::Objects::KinectUser* KinectDevice::CreateKinectUser()
    {
      NIDevice::Objects::KinectUser* newKinectUser = new NIDevice::Objects::KinectUser(this);
      listOfUsers.push_back(newKinectUser);
      
      //if an openNI user id already exist we initialise scol object user
      newKinectUser->SetId(GetExistingAvailableId());
      if (GetUserGenerator())
        GetUserGenerator()->requestInitialPos(newKinectUser);

      return newKinectUser;
    }

    NIDevice::Objects::KinectUser* KinectDevice::GetUser(int userId)
    {
      NIDevice::Objects::KinectUser* userFound = 0;
      const KinectUserList userList = listOfUsers;
      KinectUserList::iterator iSearchedUser = listOfUsers.begin();
      while ((iSearchedUser != listOfUsers.end()) && (userFound == 0))
      {
        if ((*iSearchedUser)->GetId() == userId)
          userFound = *iSearchedUser;
        
        iSearchedUser++;
      }
      return userFound;
    }
    
    void KinectDevice::GetValidUsersMap(NIDevice::Objects::KinectUser** tUsers)
    {
      const KinectUserList userList = listOfUsers;
      KinectUserList::iterator iSearchedUser = listOfUsers.begin();
      while (iSearchedUser != listOfUsers.end())
      {
        int uid = (*iSearchedUser)->GetId();
        if ((uid > 0) && (uid < 16))
          tUsers[uid] = *iSearchedUser;
        
        iSearchedUser++;
      }
    }

    void KinectDevice::GoThread()
    {
      try
      {
        while(IsRunning())
        {
          if(!IsInit())
            createContext();
          
          else if(IsInit() && IsConnected() && IsGenerating())
          {
            nRetVal = context.WaitAndUpdateAll();
            if(nRetVal != XN_STATUS_OK)
            {
              KINECT_EXCEPT(NIDevice::Core::ExceptionKinect, GetNiDeviceMessage(), GetObjectTypeAsString());
            }
            
            int ucnt = 0;
            int uavailable = GetExistingAvailableId();
            NIDevice::Objects::KinectUser* user = GetUser(-1);
            //attribute an available user Id if possible
            
            //get user pixels and depth pixels
            if (((uavailable != -1) && user) && (GetUserGenerator()->GetGenerator().IsValid() && GetDepthGenerator()->GetGenerator().IsValid()))
            {
              xn::SceneMetaData udata;
              GetUserGenerator()->GetGenerator().GetUserPixels(uavailable, udata);
              const unsigned short* ubuf = udata.Data();
              const unsigned short* dbuf = GetDepthGenerator()->GetDepthBuffer(false);
              //count user pixels
              for (int i=0; i < (udata.XRes() * udata.YRes()); i++)
              {
                if ((ubuf[i] * dbuf[i]) > 0)
                  ucnt++;
              }

              if (ucnt > 0)
              {
                user->SetId(uavailable);
                GetUserGenerator()->GetGenerator().GetPoseDetectionCap().StartPoseDetection("Psi", uavailable);
                PostMessage(HScol, KINECT_NEW_USER_CB, (int)user, 0);
              }
              else
                AddExistingId(uavailable);
            }

            // Update data from NITE session manager
            sessionManager->Update(&context);
          }

          else if(IsInit() && !IsConnected())
            createAndStartGenerators();

          else if(IsInit() && IsConnected() && !IsGenerating())
          {
            stopAndDestroyGenerators();
            destroyContext();
          }
        }
      }
      catch (NIDevice::Core::ExceptionKinect& e)
      {
        MMechostr(MSKDEBUG,"%s",e.GetFullDescription().c_str());
      }
    }

    unsigned int KinectDevice::GetId()
    {
      return mId;
    }

    int KinectDevice::GetExistingAvailableId()
    {
      int found = -1;
      OpenNIUserList::iterator iuser = listOfAvalaibleUsers.begin();
      if (iuser != listOfAvalaibleUsers.end())
      {
        found = (int)*iuser;
        listOfAvalaibleUsers.erase(iuser);
      }
      
      return found;
    }

    void KinectDevice::AddExistingId(unsigned int id)
    {
      OpenNIUserList::iterator iuser = listOfAvalaibleUsers.find(id);
      if (iuser == listOfAvalaibleUsers.end())
      {
        listOfAvalaibleUsers.insert(id);
      }
    }

    void KinectDevice::RemoveExistingId(unsigned int id)
    {
      OpenNIUserList::iterator iuser = listOfAvalaibleUsers.find(id);
      if (iuser != listOfAvalaibleUsers.end())
      {
        listOfAvalaibleUsers.erase(iuser);
      }
    }

    // NITE callbacks for session manager
    void XN_CALLBACK_TYPE SessionStart(const XnPoint3D &ptPosition, void *userCxt)
    {
      MMechostr(MSKDEBUG, ">>> NITE session started.\n");
    }

    void XN_CALLBACK_TYPE SessionEnd(void *userCxt)
    {
      MMechostr(MSKDEBUG, ">>> NITE session started.\n");
    }

    void KinectDevice::createAndStartGenerators()
    {
      // Create generators
      m_DepthGenerator = new NIDevice::Generator::Depth(this);
      m_ImageGenerator = new NIDevice::Generator::Image(this);
      m_UserGenerator = new NIDevice::Generator::User(this);
      m_GestureGenerator = new NIDevice::Generator::Gesture(this);
      m_HandsGenerator = new NIDevice::Generator::Hands(this);

      // Create and initialize point tracker for NITE session manager
      sessionManager = new XnVSessionManager();
      sessionManager->Initialize(&context, "Click,Wave", "RaiseHand");  // Set "Wave" as focus gesture and "RaiseHand" as quick refocus gesture
      sessionManager->SetQuickRefocusTimeout(0);                        // Set infinite timeout for quick refocus gesture
      sessionManager->RegisterSession(NULL, &SessionStart, &SessionEnd);
      
      //init smoothing
      if (m_UserGenerator)
        m_UserGenerator->GetGenerator().GetSkeletonCap().SetSmoothing(m_sklSmoothing);
      
      //init cutoff
      if (m_DepthGenerator)
      {
        m_DepthGenerator->SetDistCutoff(iDistCutoff);
        m_DepthGenerator->SetAngCutoff(fAngCutoff);
      }
      // Start and update all the generators
      context.StartGeneratingAll();
      context.WaitNoneUpdateAll();

      bConnected = true;
      bGenerating = true;

      SetMirrorMode(bMirror);

      //cannot be used as name
      //context.FindExistingNode(XN_NODE_TYPE_DEVICE,m_deviceNode);
      //const XnChar* descritption = m_deviceNode.GetInfo().GetCreationInfo();

      PostMessage(HScol, KINECT_CONNECTED_CB, (int)this,NULL);
    }

    void KinectDevice::stopAndDestroyGenerators()
    {
      // End NITE session
      if (NULL != sessionManager)
        sessionManager->EndSession();

      // Stop all the generators
      context.StopGeneratingAll();

      if(m_DepthGenerator)
      {
        SAFE_DELETE(m_DepthGenerator);
      }

      if(m_ImageGenerator)
      {
        SAFE_DELETE(m_ImageGenerator);
      }

      if(m_UserGenerator)
      {
        SAFE_DELETE(m_UserGenerator);
      }

      if(m_GestureGenerator)
      {
        SAFE_DELETE(m_GestureGenerator);
      }

      if(m_HandsGenerator)
      {
        SAFE_DELETE(m_HandsGenerator);
      }

      listOfAvalaibleUsers.clear();

      PostMessage(HScol, KINECT_DISCONNECTED_CB, (int)this,NULL);
    }

    NIDevice::Generator::Depth* KinectDevice::GetDepthGenerator()
    {
      return m_DepthGenerator;
    }

    NIDevice::Generator::Image* KinectDevice::GetImageGenerator()
    {
      return m_ImageGenerator;
    }

    NIDevice::Generator::User* KinectDevice::GetUserGenerator()
    {
      return m_UserGenerator;
    }

    NIDevice::Generator::Gesture* KinectDevice::GetGestureGenerator()
    {
      return m_GestureGenerator;
    }

    NIDevice::Generator::Hands* KinectDevice::GetHandsGenerator()
    {
      return m_HandsGenerator;
    }

    void KinectDevice::SetSkeletonSmoothing(float value)
    {
      m_sklSmoothing = value;
      if (m_UserGenerator)
        m_UserGenerator->GetGenerator().GetSkeletonCap().SetSmoothing(m_sklSmoothing);
    }

    float KinectDevice::GetSkeletonSmoothing()
    {
      return m_sklSmoothing;
    }

    void KinectDevice::SetDistCutoff(int dist)
    {
      iDistCutoff = dist;
      if (m_DepthGenerator)
        m_DepthGenerator->SetDistCutoff(dist);
    }

    int KinectDevice::GetDistCutoff()
    {
      return iDistCutoff;
    }

    void KinectDevice::SetAngCutoff(float ang)
    {
      fAngCutoff = ang;
      if (m_DepthGenerator)
        m_DepthGenerator->SetAngCutoff(ang);
    }

    float KinectDevice::GetAngCutoff()
    {
      return fAngCutoff;
    }
  }
}