OpenNiDevice.cpp

Go to the documentation of this file.

 
#include  "OpenNiDevice.h"
#include  "tools++.h"
 
using namespace jbxl;
 
 
#ifdef  ENABLE_OPENNI
 
 
COpenNiDevice::COpenNiDevice(void)
{
    context      = NULL;
    image        = NULL;
    user         = NULL;
    depth        = NULL;
    player       = NULL;
 
    imageMD      = NULL;
    depthMD      = NULL;
    sceneMD      = NULL;
 
    m_enu_dev    = EnuDev_Unknown;
    m_has_camera = TRUE;
    m_err_mesg   = make_Buffer(LMESG);
 
    m_state      = NI_STATE_DETECT_STOPPED;
    m_xsize      = OPENNI_DEPTH_XSIZE;
    m_ysize      = OPENNI_DEPTH_YSIZE;
    m_nfps       = OPENNI_DEPTH_FPS;
 
    //
    recorder     = NULL;
    skeleton     = NULL;
//  pose         = NULL;
 
    usb_dev      = NULL;
    devPath      = NULL;
 
    userCallbacks   = NULL;
    poseCallbacks   = NULL;
    calibCallbacks  = NULL;
 
    memset(dUsers, 0, sizeof(XnUserID)*OPENNI_USERS_NUM);
}
 
 
BOOL  COpenNiDevice::init(BOOL use_image)
{
    outputMode.nXRes = OPENNI_DEPTH_XSIZE; 
    outputMode.nYRes = OPENNI_DEPTH_YSIZE; 
    outputMode.nFPS  = OPENNI_DEPTH_FPS; 
 
    m_enu_dev = EnuDev_None;
 
    BOOL ret = create_Context();
    if (ret && use_image) create_Image();
    if (ret) ret = create_Depth();
    if (ret) m_enu_dev = EnuDev_PS1080;
 
    return ret;
}
 
 
BOOL  COpenNiDevice::init(char* fname, BOOL use_image)
{
    m_enu_dev = EnuDev_None;
 
    if (fname==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::init: File name is NULL", &m_err_mesg);
        return FALSE;
    }
    //
    BOOL ret = create_Context(fname);
    if (ret) ret = create_Player(TRUE);
    if (!ret) {
        copy_s2Buffer("ERROR: COpenNiDevice::init: create error!!", &m_err_mesg);
        return FALSE;
    }
    //
    xn::NodeInfoList nodeList;
    player->EnumerateNodes(nodeList);
 
    for (xn::NodeInfoList::Iterator node=nodeList.Begin(); node!=nodeList.End(); node++) {
        if ((*node).GetDescription().Type==XN_NODE_TYPE_IMAGE) {
            if (use_image) {
                ret = create_Image(TRUE);
                if (!ret) break;
            }
        }
        if ((*node).GetDescription().Type==XN_NODE_TYPE_DEPTH) {
            ret = create_Depth(TRUE);
            if (!ret) break;
        }
    }
    //
    if (image!=NULL && use_image) {
        image->GetMapOutputMode(outputMode);
    }
    else if (depth!=NULL) {
        depth->GetMapOutputMode(outputMode);
    }
    else {
        copy_s2Buffer("ERROR: COpenNiDevice::init: No Generators", &m_err_mesg);
        return FALSE;
    }
 
    m_enu_dev = EnuDev_OniFile;
    return TRUE;
}
 
 
void  COpenNiDevice::free(void)
{
    clear_Tracking();
    clear_CallBacks();
    clear_Skeleton();
 
    delete_User();
    delete_Depth();
    delete_Image();
    delete_Player();
    delete_Context();
 
    free_Buffer(&m_err_mesg);
}
 
 
 
// Create/Delete Generators
 
BOOL  COpenNiDevice::create_Context(char* fname)
{
    if (context==NULL) {
        context = new xn::Context();
        if (context==NULL) {
            copy_s2Buffer("ERROR: COpenNiDevice::create_Context: context is NULL", &m_err_mesg);
            return FALSE;
        }
    }
 
    XnStatus rc = context->Init();
    if (rc==XN_STATUS_OK && fname!=NULL) rc = context->OpenFileRecording(fname);
 
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Context: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        return FALSE;
    }
    return TRUE;
}
 
 
BOOL  COpenNiDevice::create_Image(BOOL easy)
{
    if (!m_has_camera) return FALSE;
 
    if (context==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Image: context is NULL", &m_err_mesg);
        return FALSE;
    }
 
    if (image==NULL) {
        image = new xn::ImageGenerator();
        if (image==NULL) {
            copy_s2Buffer("ERROR: COpenNiDevice::create_Image: fail to create image generator", &m_err_mesg);
            m_has_camera = FALSE;
            return FALSE;
        }
    }
 
    XnStatus rc = XN_STATUS_ERROR;
    if (easy) {
        rc = context->FindExistingNode(XN_NODE_TYPE_IMAGE, *image);
    }
    else {
        rc = image->Create(*context);
        if (rc==XN_STATUS_OK) rc = image->StartGenerating();
        if (rc==XN_STATUS_OK) rc = image->SetMapOutputMode(outputMode); 
    }
 
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Image: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        delete_Image();
        return FALSE;
    }
 
    imageMD = new xn::ImageMetaData();
    //imageData = (uByte*)malloc(outputMode.nXRes*outputMode.nYRes*3);
    //if (imageData!=NULL) memset(imageData, 0, outputMode.nXRes*outputMode.nYRes*3);
 
    return TRUE;
}
 
 
BOOL  COpenNiDevice::create_Depth(BOOL easy)
{
    if (context==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Depth: context is NULL", &m_err_mesg);
        return FALSE;
    }
 
    if (depth==NULL) {
        depth = new xn::DepthGenerator();
        if (depth==NULL) {
            copy_s2Buffer("ERROR: COpenNiDevice::create_Depth: fail to create depth generator", &m_err_mesg);
            return FALSE;
        }
    }
 
    XnStatus rc = XN_STATUS_ERROR;
    if (easy) {
        rc = context->FindExistingNode(XN_NODE_TYPE_DEPTH, *depth);
    }
    else {
        rc = depth->Create(*context);
        if (rc==XN_STATUS_OK) rc = depth->StartGenerating();
        if (rc==XN_STATUS_OK) rc = depth->SetMapOutputMode(outputMode);
        if (rc==XN_STATUS_OK && image!=NULL) {
            rc = depth->GetAlternativeViewPointCap().SetViewPoint(*image);
            /*if (rc==XN_STATUS_OK && depth->IsCapabilitySupported(XN_CAPABILITY_FRAME_SYNC)) {
                if (depth->GetFrameSyncCap().CanFrameSyncWith(*image)) {
                    rc = depth->GetFrameSyncCap().FrameSyncWith(*image);
                }
            }*/
        }
    }
    
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Depth: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        delete_Depth();
        return FALSE;
    }
 
    depthMD = new xn::DepthMetaData();
    //depthData = (XnLabel*)malloc(outputMode.nXRes*outputMode.nYRes*sizeof(XnLabel));
    //if (depthData!=NULL) memset(depthData, 0, outputMode.nXRes*outputMode.nYRes*sizeof(XnLabel));
 
    return TRUE;
}
 
 
BOOL  COpenNiDevice::create_User(BOOL easy)
{
    if (context==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_User: context is NULL", &m_err_mesg);
        return FALSE;
    }
 
    if (user==NULL) {
        user = new xn::UserGenerator();
        if (user==NULL) {
            copy_s2Buffer("ERROR: COpenNiDevice::create_User: fail to create user generator", &m_err_mesg);
            return FALSE;
        }
    }
 
    XnStatus rc = XN_STATUS_ERROR;
    if (easy) {
        rc = context->FindExistingNode(XN_NODE_TYPE_USER, *user);
    }
    else {
        rc = user->Create(*context);
        if (rc==XN_STATUS_OK) rc = user->StartGenerating();
    }
 
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_User: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        delete_User();
        return FALSE;
    }
 
    sceneMD = new xn::SceneMetaData();
    return TRUE;
}
 
 
BOOL  COpenNiDevice::create_Player(BOOL easy)
{
    if (context==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Player: context is NULL", &m_err_mesg);
        return FALSE;
    }   
 
    if (player==NULL) {
        player = new xn::Player();
        if (player==NULL) {
            copy_s2Buffer("ERROR: COpenNiDevice::create_Player: fail to create player generator", &m_err_mesg);
            return FALSE;
        }
    }
 
    XnStatus rc = XN_STATUS_ERROR;
    if (easy) {
        rc = context->FindExistingNode(XN_NODE_TYPE_PLAYER, *player);
    }
    else {
        rc = player->Create(*context, "oni");
    }
    
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Player: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        delete_Player();
        return FALSE;
    }
    return TRUE;
}
 
 
 
// Delete Generators
 
void  COpenNiDevice::delete_Context(void)
{
    if (context!=NULL) {
        context->Release();
        delete(context);
    }
    context = NULL;
}
 
 
void  COpenNiDevice::delete_Image(void)
{
    if (image!=NULL) {
        if (image->IsValid()) {
            if (image->IsGenerating()) image->StopGenerating();
        }
        image->Release();
        delete(image);
    }
    if (imageMD!=NULL) delete(imageMD);
 
    image   = NULL;
    imageMD = NULL;
}
 
 
void  COpenNiDevice::delete_Depth(void)
{
    if (depth!=NULL) {
        if (depth->IsValid()) {
            if (depth->IsGenerating()) depth->StopGenerating();
        }
        depth->Release();
        delete(depth);
    }
    if (depthMD!=NULL) delete(depthMD);
 
    depth   = NULL;
    depthMD = NULL;
}
 
 
void  COpenNiDevice::delete_User(void)
{
    if (user!=NULL) {
        if (user->IsValid()) {
            if (user->IsGenerating()) user->StopGenerating();
        }
        user->Release();
        delete(user);
    }
    if (sceneMD!=NULL) delete(sceneMD);
 
    user    = NULL;
    sceneMD = NULL;
}
 
 
void   COpenNiDevice::delete_Player(void)
{
    if (player!=NULL) {
        player->Release();
        delete(player);
        player = NULL;  
    }   
}
 
 
 
// Create Capabilities
 
BOOL  COpenNiDevice::setup_Tracking(int profile, double smooth)
{
    if (user==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::setup_Tracking: user is NULL", &m_err_mesg);
        return FALSE;
    }
    if (!user->IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
        copy_s2Buffer("ERROR: COpenNiDevice::setup_Tracking: not support skeleton", &m_err_mesg);
        return FALSE;
    }
 
    if (skeleton==NULL) {
        skeleton = new xn::SkeletonCapability(*user);
        if (skeleton==NULL) {
            copy_s2Buffer("ERROR: COpenNiDevice::setup_Tracking: fail to make skeleton capability", &m_err_mesg);
            return FALSE;
        }
    }
 
    //
    skeleton->SetSkeletonProfile((XnSkeletonProfile)profile);
    skeleton->SetSmoothing((XnFloat)smooth);
    //
    return TRUE;
}
 
 
 
// Clear Capabilities
 
//
void  COpenNiDevice::clear_Skeleton(void)
{
    if (skeleton!=NULL) delete(skeleton);
    skeleton = NULL;
}
 
 
void  COpenNiDevice::clear_Tracking(void)
{
    if (user!=NULL) {
        nUsers = OPENNI_USERS_NUM;
        user->GetUsers(dUsers, nUsers);
        for (int i=0; i<nUsers; i++) {
            if (skeleton!=NULL && skeleton->IsTracking(dUsers[i])) {
                skeleton->StopTracking(dUsers[i]);
            }
        }
    }
}
 
 
 
// File
 
BOOL  COpenNiDevice::create_Recorder(char* file_name, BOOL use_image)
{
    if (context==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Recorder: context is NULL", &m_err_mesg);
        return FALSE;
    }   
    if (depth==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Recorder: depth is NULL", &m_err_mesg);
        return FALSE;
    }
 
    recorder = new xn::Recorder();
    rc = recorder->Create(*context);
    if (rc==XN_STATUS_OK) rc = recorder->SetDestination(XN_RECORD_MEDIUM_FILE, file_name);
    if (rc==XN_STATUS_OK && use_image && image!=NULL) rc = recorder->AddNodeToRecording(*image, XN_CODEC_JPEG);
    if (rc==XN_STATUS_OK) rc = recorder->AddNodeToRecording(*depth, XN_CODEC_16Z);  
    
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::create_Recorder: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        return FALSE;
    }
 
    return TRUE;
}
 
 
void   COpenNiDevice::delete_Recorder(void)
{
    if (recorder!=NULL) {
        int prev_state = m_state;
        m_state = NI_STATE_SAVE_WORKING;
        {
            Sleep(NI_WORKING_WAIT_TIME);
            if (depth!=NULL) recorder->RemoveNodeFromRecording(*depth);
            if (image!=NULL) recorder->RemoveNodeFromRecording(*image);
            //if (use_camera && image!=NULL) recorder->RemoveNodeFromRecording(*image);
            recorder->Release();
            delete(recorder);
        }
        m_state = prev_state;
    }   
    recorder = NULL;
    return;
}
 
 
BOOL  COpenNiDevice::start_Recorde(char* file_name, BOOL use_image)
{
    BOOL ret = FALSE;
    unlink(file_name);
 
    int prev_state = m_state;
    m_state = NI_STATE_SAVE_WORKING;
    {
        Sleep(NI_WORKING_WAIT_TIME);
        ret = create_Recorder(file_name, use_image);
        if (ret) {
            rc = recorder->Record();
            if (rc!=XN_STATUS_OK) {
                copy_s2Buffer("ERROR: COpenNiDevice::start_Recorde: ", &m_err_mesg);
                cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
                ret = FALSE;
            }
        }
    }
    m_state = prev_state;
 
    if (!ret) {
        delete_Recorder();
        unlink(file_name);
    }
    return ret;
}
 
 
void  COpenNiDevice::stop_Recorde(void)
{
    delete_Recorder();
}
 
 
 
// USB
 
BOOL  COpenNiDevice::open_USB_Device(void)
{
    rc = ::xnUSBInit();
    if (rc!=XN_STATUS_OK && rc!=XN_STATUS_USB_ALREADY_INIT) return FALSE;
    
#ifdef WIN32
    XnUInt32 count;
    rc = ::xnUSBEnumerateDevices(NI_VID_MICROSOFT, NI_PID_NUI_MOTOR, (const XnUSBConnectionString**)&devPath, &count);
    if (rc!=XN_STATUS_OK) {
        ::xnUSBShutdown();
        return FALSE;
    }
 
    rc = ::xnUSBOpenDeviceByPath(*devPath, &usb_dev);
    if (rc!=XN_STATUS_OK) {
        ::xnUSBFreeDevicesList(devPath);
        return FALSE;
    }
#else
    rc = ::xnUSBOpenDevice(OPENNI_VID_MICROSOFT, OPENNI_PID_NUI_MOTOR, NULL, NULL, &usb_dev);
    if (rc!=XN_STATUS_OK) {
        ::xnUSBShutdown();
        return FALSE;
    }
#endif
 
    return TRUE;
}
 
 
void  COpenNiDevice::close_USB_Device(void)
{
    if (usb_dev!=NULL) {
        ::xnUSBCloseDevice(usb_dev);
        usb_dev = NULL;
    }
 
#ifdef WIN32
    if (devPath!=NULL) {
        ::xnUSBFreeDevicesList(devPath);
        devPath = NULL;
    }
#endif
 
    ::xnUSBShutdown();
}
 
 
void  COpenNiDevice::set_LED_Color(int col)
{
    if (usb_dev==NULL) return;
 
    ::xnUSBSendControl(usb_dev, XN_USB_CONTROL_TYPE_VENDOR, 0x06, (XnUInt16)col, 0, NULL, 0, 0);
 
    return;
}
 
 
void  COpenNiDevice::set_Tilt_Motor(int ang)
{
    if (usb_dev==NULL) return;
    //if (ang>30 || ang<-30) return;
 
    ::xnUSBSendControl(usb_dev, XN_USB_CONTROL_TYPE_VENDOR, 0x31, (XnUInt16)(ang*2), 0, NULL, 0, 0);
    
    Sleep(1000);
    return;
}
 
 
 
// コールバック
 
BOOL  COpenNiDevice::setup_CallBacks()
{
    BOOL     ret = set_UserCallbacks (&userDetect, &userLost, this);
    if (ret) ret = set_CalibCallbacks(&calibStart, &calibEnd, this);
    if (!ret) return FALSE;
 
    return TRUE;
}
 
 
void  COpenNiDevice::clear_CallBacks()
{
    unset_UserCallbacks();
    unset_CalibCallbacks();
}
 
 
BOOL  COpenNiDevice::set_UserCallbacks(xn::UserGenerator::UserHandler newUser, xn::UserGenerator::UserHandler lostUser, void* cookie)
{
    if (user==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::set_UserCallbacks: user is NULL", &m_err_mesg);
        return FALSE;
    }
 
    rc = user->RegisterUserCallbacks(newUser, lostUser, cookie, userCallbacks);
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::set_UserCallbacks: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        unset_UserCallbacks();
        return FALSE;
    }
    return TRUE;
}
 
 
BOOL  COpenNiDevice::set_CalibCallbacks(xn::SkeletonCapability::CalibrationStart calibStart, xn::SkeletonCapability::CalibrationEnd calibEnd, void* cookie)
{
    if (skeleton==NULL) {
        copy_s2Buffer("ERROR: COpenNiDevice::set_CalibCallbacks: skeleton is NULL", &m_err_mesg);
        return FALSE;
    }
 
    rc = skeleton->RegisterCalibrationCallbacks(calibStart, calibEnd, cookie, calibCallbacks);
    if (rc!=XN_STATUS_OK) {
        copy_s2Buffer("ERROR: COpenNiDevice::set_CalibCallbacks: ", &m_err_mesg);
        cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
        unset_CalibCallbacks();
        return FALSE;
    }
    return TRUE;
}
 
 
void  COpenNiDevice::unset_UserCallbacks()
{
    if (user!=NULL && userCallbacks!=NULL) {
        user->UnregisterUserCallbacks(userCallbacks);
        userCallbacks = NULL;
    }
    return;
}
 
 
void  COpenNiDevice::unset_CalibCallbacks()
{
    if (skeleton!=NULL && calibCallbacks!=NULL) {
        skeleton->UnregisterCalibrationCallbacks(calibCallbacks);
        calibCallbacks = NULL;
    }
    return;
}
 
 
 
// External Callback Functions
//
 
void  XN_CALLBACK_TYPE jbxl::userDetect(xn::UserGenerator& user, unsigned int nId, void* cookie)
{
    DEBUG_INFO("INFO: jbxl::userDetect(): User is detected. (%d)", nId);
    if (cookie==NULL) return;
    
    user.GetSkeletonCap().RequestCalibration(nId, TRUE);
    
    DEBUG_INFO("INFO: jbxl::userDetect(): Start Pose Detection. (%d)", nId);
}
 
 
void  XN_CALLBACK_TYPE jbxl::userLost(xn::UserGenerator& user, unsigned int nId, void* cookie)
{
    DEBUG_INFO("INFO: jbxl::userLost(): Lost user. (%d)", nId);
 
    if (user.GetSkeletonCap().IsValid()) {
        if (user.GetSkeletonCap().IsTracking(nId)) {
            user.GetSkeletonCap().StopTracking(nId);
            DEBUG_INFO("INFO: jbxl::userLost(): Stop Tracking. (%d)", nId);
        }
    }
}
 
 
// Calibration
 
void  XN_CALLBACK_TYPE jbxl::calibStart(xn::SkeletonCapability& skeleton, unsigned int nId, void* cookie)
{
    DEBUG_INFO("INFO: jbxl::calibStart(): Start Calibration. (%d)", nId);
}
 
 
void  XN_CALLBACK_TYPE jbxl::calibEnd(xn::SkeletonCapability& skeleton, unsigned int nId, XnBool success, void* cookie)
{
    DEBUG_INFO("INFO: jbxl::calibEnd(): End Calibration. (%d)", nId);
    if (cookie==NULL) return;
 
    //
    xn::UserGenerator* user = ((COpenNiDevice*)cookie)->user;
    if (user!=NULL) {
        if (success) {
            if (user!=NULL) {
                user->GetSkeletonCap().StartTracking(nId);
                DEBUG_INFO("INFO: jbxl::calibEnd(): Start Tracking. (%d)", nId);
            }
        }
    }
}
 
 
#endif      // ENABLE_OPENNI