diff --git a/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.cpp b/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.cpp index 6ecded9d6ad72cd09b56778f599aff47eff872d5..aa6a6ae7c4e2dcbe71d2f5cf325fd1fe25ae6d0a 100644 --- a/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.cpp +++ b/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.cpp @@ -530,6 +530,7 @@ VistaLeapMotionDriver::VistaLeapMotionDriver( IVistaDriverCreationMethod* pCreat , m_pListener( NULL ) , m_pLeapController( NULL ) , m_bListenerIsRegistered( false ) +, m_nLastFrameId( ~0u ) { SetUpdateType( IVistaDeviceDriver::UPDATE_CUSTOM_THREADED ); @@ -597,148 +598,156 @@ bool VistaLeapMotionDriver::DoSensorUpdate( VistaType::microtime dTs ) if( m_pThreadAspect->GetDriverUpdatePrepare() ) m_pThreadAspect->GetDriverUpdatePrepare()->PrePoll(); - bool valid_frame=true; - int history_index=0; - - while( valid_frame == true ) + const Leap::Frame oLatestFrame = m_pLeapController->frame(); + VistaType::uint64 nLatestFrameId = oLatestFrame.id(); + + if( nLatestFrameId > m_nLastFrameId + 1 ) { - const Leap::Frame oFrame = m_pLeapController->frame(history_index++); - valid_frame = oFrame.isValid(); - - if( valid_frame == true ) + VistaType::uint64 nFramesToProcess = nLatestFrameId - m_nLastFrameId - 1; + for( int nPast = nFramesToProcess; nPast > 0; --nPast ) { - // Fill hand sensor - unsigned int nNumHands = m_pSensors->GetNumRegisteredSensorsForType( m_nHandsSensorId ); - Leap::HandList oHands = oFrame.hands(); - for( unsigned int nHand = 0; nHand < nNumHands; ++nHand ) - { - int nSensorId = m_pSensors->GetSensorId( m_nHandsSensorId, nHand ); - VistaSensorMeasure* pMeasure = MeasureStart( nSensorId, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); + const Leap::Frame oPastFrame = m_pLeapController->frame( nPast ); + if( oPastFrame.isValid() && oPastFrame.id() > m_nLastFrameId ) + ProcessFrame( oLatestFrame, dTs ); + } + } + ProcessFrame( oLatestFrame, dTs ); + m_nLastFrameId = nLatestFrameId; + + if( m_pThreadAspect->GetDriverUpdatePrepare() ) + m_pThreadAspect->GetDriverUpdatePrepare()->PostPoll(); + return true; +} + +void VistaLeapMotionDriver::ProcessFrame( const Leap::Frame& oFrame, VistaType::microtime dTs ) +{ + // Fill hand sensor + unsigned int nNumHands = m_pSensors->GetNumRegisteredSensorsForType( m_nHandsSensorId ); + Leap::HandList oHands = oFrame.hands(); + for( unsigned int nHand = 0; nHand < nNumHands; ++nHand ) + { + int nSensorId = m_pSensors->GetSensorId( m_nHandsSensorId, nHand ); + VistaSensorMeasure* pMeasure = MeasureStart( nSensorId, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); - VistaLeapMotionMeasures::HandMeasure* pData = pMeasure->getWrite< VistaLeapMotionMeasures::HandMeasure >(); - FillHand( *pData, oHands[nHand], oFrame.timestamp() ); - MeasureStop( nSensorId ); - } + VistaLeapMotionMeasures::HandMeasure* pData = pMeasure->getWrite< VistaLeapMotionMeasures::HandMeasure >(); + FillHand( *pData, oHands[nHand], oFrame.timestamp() ); + MeasureStop( nSensorId ); + } - unsigned int nFingersSensorId = m_pSensors->GetSensorId( m_nFingerSensorId, 0 ); - if( nFingersSensorId != ~0u ) - { - VistaSensorMeasure* pMeasure = MeasureStart( nFingersSensorId, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); - VistaLeapMotionMeasures::FingersMeasure* pData = pMeasure->getWrite< VistaLeapMotionMeasures::FingersMeasure >(); - Leap::FingerList liFingers = oFrame.fingers(); - int nCount = std::max<int>( liFingers.count(), LEAPMEASURES_MAX_FINGERS ); - int nIndex = 0; - for( ; nIndex < nCount; ++nIndex ) - FillFinger( pData->m_aFingers[ nIndex ], liFingers[nIndex], oFrame.timestamp() ); + unsigned int nFingersSensorId = m_pSensors->GetSensorId( m_nFingerSensorId, 0 ); + if( nFingersSensorId != ~0u ) + { + VistaSensorMeasure* pMeasure = MeasureStart( nFingersSensorId, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); + VistaLeapMotionMeasures::FingersMeasure* pData = pMeasure->getWrite< VistaLeapMotionMeasures::FingersMeasure >(); + Leap::FingerList liFingers = oFrame.fingers(); + int nCount = std::max<int>( liFingers.count(), LEAPMEASURES_MAX_FINGERS ); + int nIndex = 0; + for( ; nIndex < nCount; ++nIndex ) + FillFinger( pData->m_aFingers[ nIndex ], liFingers[nIndex], oFrame.timestamp() ); - for( ; nIndex < LEAPMEASURES_MAX_FINGERS; ++nIndex ) - pData->m_aFingers[ nIndex ] = VistaLeapMotionMeasures::Finger(); + for( ; nIndex < LEAPMEASURES_MAX_FINGERS; ++nIndex ) + pData->m_aFingers[ nIndex ] = VistaLeapMotionMeasures::Finger(); - MeasureStop( nFingersSensorId ); - } + MeasureStop( nFingersSensorId ); + } - unsigned int nToolsSensorId = m_pSensors->GetSensorId( m_nToolsSensorId, 0 ); - if( nToolsSensorId != ~0u ) - { - VistaSensorMeasure* pMeasure = MeasureStart( nToolsSensorId, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); + unsigned int nToolsSensorId = m_pSensors->GetSensorId( m_nToolsSensorId, 0 ); + if( nToolsSensorId != ~0u ) + { + VistaSensorMeasure* pMeasure = MeasureStart( nToolsSensorId, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); - VistaLeapMotionMeasures::ToolsMeasure* pData = pMeasure->getWrite< VistaLeapMotionMeasures::ToolsMeasure >(); - Leap::ToolList liTools = oFrame.tools(); - int nCount = std::max<int>( liTools.count(), LEAPMEASURES_MAX_TOOLS ); - int nIndex = 0; - for( ; nIndex < nCount; ++nIndex ) - FillPointable( pData->m_aTools[ nIndex ], liTools[nIndex], oFrame.timestamp() ); + VistaLeapMotionMeasures::ToolsMeasure* pData = pMeasure->getWrite< VistaLeapMotionMeasures::ToolsMeasure >(); + Leap::ToolList liTools = oFrame.tools(); + int nCount = std::max<int>( liTools.count(), LEAPMEASURES_MAX_TOOLS ); + int nIndex = 0; + for( ; nIndex < nCount; ++nIndex ) + FillPointable( pData->m_aTools[ nIndex ], liTools[nIndex], oFrame.timestamp() ); - for( ; nIndex < LEAPMEASURES_MAX_TOOLS; ++nIndex ) - pData->m_aTools[ nIndex ] = VistaLeapMotionMeasures::Tool(); + for( ; nIndex < LEAPMEASURES_MAX_TOOLS; ++nIndex ) + pData->m_aTools[ nIndex ] = VistaLeapMotionMeasures::Tool(); - MeasureStop( nToolsSensorId ); - } + MeasureStop( nToolsSensorId ); + } - Leap::GestureList oList = oFrame.gestures(); - int count = oList.count(); + Leap::GestureList oList = oFrame.gestures(); + int count = oList.count(); - for( int i = 0; i < count; ++i ) - { - Leap::Gesture oGesture = oList[i]; - ProcessGesture( m_nGestureSensorId, oGesture, dTs, oFrame.timestamp() ); - switch( oGesture.type() ) - { - case Leap::Gesture::TYPE_SWIPE: - ProcessGesture( m_nSwipeGestureSensorId, oGesture, dTs, oFrame.timestamp() ); - break; - case Leap::Gesture::TYPE_CIRCLE: - ProcessGesture( m_nCircleGestureSensorId, oGesture, dTs, oFrame.timestamp() ); - break; - case Leap::Gesture::TYPE_KEY_TAP: - ProcessGesture( m_nKeyTapGestureSensorId, oGesture, dTs, oFrame.timestamp() ); - break; - case Leap::Gesture::TYPE_SCREEN_TAP: - ProcessGesture( m_nScreenTapGestureSensorId, oGesture, dTs, oFrame.timestamp() ); - break; - case Leap::Gesture::TYPE_INVALID: - default: - break; - } - } - - - // two conditions to receive images: - // 1) policy was set - // 2) image sensor was specified - if( GetParameters()->GetPolicyFlags() & VistaLeapMotionDriver::Parameters::POLICY_IMAGES ) - { - unsigned int image_sensors[2]; - image_sensors[0] = m_pSensors->GetSensorId( m_nImageSensorId, 0 ); - image_sensors[1] = m_pSensors->GetSensorId( m_nImageSensorId, 1 ); - - Leap::ImageList images = oFrame.images(); - Leap::ImageList::const_iterator begin = images.begin(); - Leap::ImageList::const_iterator end = images.end(); - - for( Leap::ImageList::const_iterator cit = begin; cit != end; ++cit ) - { - unsigned int sensor_id = image_sensors[ (*cit).id() ]; - if( sensor_id != VistaDriverSensorMappingAspect::INVALID_ID ) - { - VistaSensorMeasure *m = MeasureStart( sensor_id, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); - VistaLeapMotionMeasures::ImageMeasure *image = m->getWrite< VistaLeapMotionMeasures::ImageMeasure >(); - - image->m_driver_timestamp = oFrame.timestamp(); - image->m_nId = (*cit).id(); - image->m_nTimeVisible = 1.0f / 120.0f; - image->m_bVisible = true; - - image->m_format = VistaType::uint32( (*cit).format() ); - image->m_height = (*cit).width(); - image->m_width = (*cit).height(); - image->m_bytes_per_pixel = (*cit).bytesPerPixel(); - image->m_ray_offset_x = (*cit).rayOffsetX(); - image->m_ray_offset_y = (*cit).rayOffsetY(); - image->m_ray_scale_x = (*cit).rayScaleX(); - image->m_ray_scale_y = (*cit).rayScaleY(); - - - // copy data fields - VistaType::uint32 data_size = image->m_width * image->m_height * image->m_bytes_per_pixel; - VistaAutoWriteBuffer ib( data_size ); - memcpy( ib.data(), (*cit).data(), data_size ); - - image->m_data = ib; - - MeasureStop( sensor_id ); - } // sensor available - } // for each image - } // POLICY IMAGES enables - - } // valid frame - } // while valid_frame + for( int i = 0; i < count; ++i ) + { + Leap::Gesture oGesture = oList[i]; + ProcessGesture( m_nGestureSensorId, oGesture, dTs, oFrame.timestamp() ); + switch( oGesture.type() ) + { + case Leap::Gesture::TYPE_SWIPE: + ProcessGesture( m_nSwipeGestureSensorId, oGesture, dTs, oFrame.timestamp() ); + break; + case Leap::Gesture::TYPE_CIRCLE: + ProcessGesture( m_nCircleGestureSensorId, oGesture, dTs, oFrame.timestamp() ); + break; + case Leap::Gesture::TYPE_KEY_TAP: + ProcessGesture( m_nKeyTapGestureSensorId, oGesture, dTs, oFrame.timestamp() ); + break; + case Leap::Gesture::TYPE_SCREEN_TAP: + ProcessGesture( m_nScreenTapGestureSensorId, oGesture, dTs, oFrame.timestamp() ); + break; + case Leap::Gesture::TYPE_INVALID: + default: + break; + } + } - if( m_pThreadAspect->GetDriverUpdatePrepare() ) - m_pThreadAspect->GetDriverUpdatePrepare()->PostPoll(); - return true; + + // two conditions to receive images: + // 1) policy was set + // 2) image sensor was specified + if( GetParameters()->GetPolicyFlags() & VistaLeapMotionDriver::Parameters::POLICY_IMAGES ) + { + unsigned int image_sensors[2]; + image_sensors[0] = m_pSensors->GetSensorId( m_nImageSensorId, 0 ); + image_sensors[1] = m_pSensors->GetSensorId( m_nImageSensorId, 1 ); + + Leap::ImageList images = oFrame.images(); + Leap::ImageList::const_iterator begin = images.begin(); + Leap::ImageList::const_iterator end = images.end(); + + for( Leap::ImageList::const_iterator cit = begin; cit != end; ++cit ) + { + unsigned int sensor_id = image_sensors[ (*cit).id() ]; + if( sensor_id != VistaDriverSensorMappingAspect::INVALID_ID ) + { + VistaSensorMeasure *m = MeasureStart( sensor_id, dTs, IVistaDeviceDriver::RETURN_CURRENT_SLOT ); + VistaLeapMotionMeasures::ImageMeasure *image = m->getWrite< VistaLeapMotionMeasures::ImageMeasure >(); + + image->m_driver_timestamp = oFrame.timestamp(); + image->m_nId = (*cit).id(); + image->m_nTimeVisible = 1.0f / 120.0f; + image->m_bVisible = true; + + image->m_format = VistaType::uint32( (*cit).format() ); + image->m_height = (*cit).width(); + image->m_width = (*cit).height(); + image->m_bytes_per_pixel = (*cit).bytesPerPixel(); + image->m_ray_offset_x = (*cit).rayOffsetX(); + image->m_ray_offset_y = (*cit).rayOffsetY(); + image->m_ray_scale_x = (*cit).rayScaleX(); + image->m_ray_scale_y = (*cit).rayScaleY(); + + + // copy data fields + VistaType::uint32 data_size = image->m_width * image->m_height * image->m_bytes_per_pixel; + VistaAutoWriteBuffer ib( data_size ); + memcpy( ib.data(), (*cit).data(), data_size ); + + image->m_data = ib; + + MeasureStop( sensor_id ); + } // sensor available + } // for each image + } // POLICY IMAGES enables } + + void VistaLeapMotionDriver::ProcessGesture( const int nSensorTypeId, const Leap::Gesture& oGesture, const VistaType::microtime nTime, VistaType::uint64 driver_time_stamp ) { unsigned int nSensorId = m_pSensors->GetSensorId( nSensorTypeId, 0 ); @@ -790,6 +799,7 @@ bool VistaLeapMotionDriver::PhysicalEnable( bool bEnable ) } } } + m_nLastFrameId = ~0u; return bRes; } @@ -838,6 +848,8 @@ bool VistaLeapMotionDriver::DoConnect() GetParameters()->ApplyGestureConfiguration(); GetParameters()->ApplyPolicyFlags(); + m_nLastFrameId = ~0u; + return true; } @@ -847,6 +859,7 @@ bool VistaLeapMotionDriver::DoDisconnect() { delete m_pLeapController; m_pLeapController = 0; + m_nLastFrameId = ~0u; } return true; } diff --git a/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.h b/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.h index 8fa4fb92116e4bc88ef29d4c6ffeb583a45106fb..a8d13720fa2bd89f8b778382d244f855efc6d338 100644 --- a/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.h +++ b/VistaCoreLibs/VistaDeviceDrivers/VistaLeapMotionDriver/VistaLeapMotionDriver.h @@ -66,6 +66,7 @@ namespace Leap { class Controller; class Gesture; + class Frame; } /*============================================================================*/ /* CLASS DEFINITIONS */ @@ -174,6 +175,7 @@ protected: virtual bool DoConnect(); virtual bool DoDisconnect(); + void ProcessFrame( const Leap::Frame& oFrame, VistaType::microtime dTs ); void ProcessGesture( const int nSensorTypeId, const Leap::Gesture& oGesture, const VistaType::microtime nTime, VistaType::uint64 driver_time_stamp ); private: @@ -198,7 +200,7 @@ private: unsigned int m_nSwipeGestureSensorId; unsigned int m_nImageSensorId; - + VistaType::uint64 m_nLastFrameId; Parameters* m_pParameters;