diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp index 2980558672..26c73b3819 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp @@ -1,164 +1,164 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include BYTE MotionBuf[0x1FA400]; mitk::PolhemusInterface::PolhemusInterface() { m_pdiDev.Trace(TRUE, 7); m_pdiDev.SetPnoBuffer(MotionBuf, 0x1FA400); m_pdiDev.SetMetric(true); //use cm instead of inches CPDImdat pdiMDat; pdiMDat.Empty(); pdiMDat.Append(PDI_MODATA_FRAMECOUNT); pdiMDat.Append(PDI_MODATA_POS); pdiMDat.Append(PDI_MODATA_ORI); m_pdiDev.SetSDataList(-1, pdiMDat); DWORD dwFrameSize = 8 + 12 + 12 + 4; } mitk::PolhemusInterface::~PolhemusInterface() { } bool mitk::PolhemusInterface::StartTracking() { LPCTSTR szWindowClass = _T("PDIconsoleWinClass"); HINSTANCE hInst = GetModuleHandle(0); HWND hwnd = CreateWindowEx( WS_EX_NOACTIVATE,//WS_EX_STATICEDGE, // szWindowClass, _T("MyWindowName"), WS_POPUP, 0, 0, 1, 1, HWND_MESSAGE, 0, hInst, 0); m_continousTracking = true; return m_pdiDev.StartContPno(hwnd); } bool mitk::PolhemusInterface::StopTracking() { m_continousTracking = false; return true; } bool mitk::PolhemusInterface::Connect() { m_pdiDev.Trace(TRUE, 7); if (m_pdiDev.CnxReady()) { return true; } ePiCommType eType = m_pdiDev.DiscoverCnx(); switch (eType) { case PI_CNX_USB: MITK_INFO << "USB Connection: " << m_pdiDev.GetLastResultStr(); break; case PI_CNX_SERIAL: MITK_INFO << "Serial Connection: " << m_pdiDev.GetLastResultStr(); break; default: MITK_INFO << "DiscoverCnx result: " << m_pdiDev.GetLastResultStr(); break; } return m_pdiDev.CnxReady(); } bool mitk::PolhemusInterface::Disconnect() { if (m_continousTracking) { - if (!m_pdiDev.Disconnect()) return false; m_continousTracking = false; + if (!m_pdiDev.Disconnect()) return false; } return true; } std::vector mitk::PolhemusInterface::GetLastFrame() { PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev.LastPnoPtr(pBuf, dwSize)) {MITK_WARN << m_pdiDev.GetLastResultStr();} std::vector returnValue = ParsePolhemusRawData(pBuf, dwSize); if (returnValue.empty()) { MITK_WARN << "Cannot parse data / no tools present"; } return returnValue; } unsigned int mitk::PolhemusInterface::GetNumberOfTools() { if (m_continousTracking) return GetLastFrame().size(); else return GetSingleFrame().size(); } std::vector mitk::PolhemusInterface::GetSingleFrame() { if (m_continousTracking) return std::vector(); PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev.ReadSinglePnoBuf(pBuf, dwSize)) { MITK_WARN << m_pdiDev.GetLastResultStr(); } return ParsePolhemusRawData(pBuf, dwSize); } std::vector mitk::PolhemusInterface::ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize) { std::vector returnValue; DWORD i = 0; while (i eulerQuat(rollAngle, elevationAngle, azimuthAngle); currentTrackingData.rot = eulerQuat; returnValue.push_back(currentTrackingData); i += shSize; } return returnValue; } diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp index f969eb7e3c..08667fc2ea 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp @@ -1,202 +1,202 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkPolhemusTrackingDevice.h" #include "mitkPolhemusTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include #include #include "mitkPolhemusTrackerTypeInformation.h" typedef itk::MutexLockHolder MutexLockHolder; mitk::PolhemusTrackingDevice::PolhemusTrackingDevice(): mitk::TrackingDevice() { //set the type of this tracking device this->m_Data = mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty(); this->m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = 0; m_Device = mitk::PolhemusInterface::New(); } mitk::PolhemusTrackingDevice::~PolhemusTrackingDevice() { } bool mitk::PolhemusTrackingDevice::IsDeviceInstalled() { return true; } mitk::TrackingTool* mitk::PolhemusTrackingDevice::AddTool( const char* toolName) { mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New(); t->SetToolName(toolName); if (this->InternalAddTool(t) == false) return nullptr; return t.GetPointer(); } bool mitk::PolhemusTrackingDevice::InternalAddTool(PolhemusTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::PolhemusTrackingDevice::StartTracking() { bool success = m_Device->StartTracking(); if (success) { mitk::IGTTimeStamp::GetInstance()->Start(this); this->SetState(Tracking); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } else { this->SetState(Ready); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!"; } return success; } bool mitk::PolhemusTrackingDevice::StopTracking() { - Superclass::StopTracking(); - this->SetState(Ready); - return true; + return Superclass::StopTracking(); } unsigned int mitk::PolhemusTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::PolhemusTrackingDevice::GetTool(unsigned int toolNumber) const { if ( toolNumber >= this->GetToolCount()) return nullptr; else return this->m_AllTools[toolNumber]; } bool mitk::PolhemusTrackingDevice::OpenConnection() { //reset everything if (m_Device.IsNull()) {m_Device = mitk::PolhemusInterface::New();} m_Device->Connect(); this->SetState(Ready); return true; } bool mitk::PolhemusTrackingDevice::CloseConnection() { bool returnValue = true; if (this->GetState() == Setup) return true; returnValue = m_Device->Disconnect(); this->SetState(Setup); return returnValue; } mitk::PolhemusInterface* mitk::PolhemusTrackingDevice::GetDevice() { return m_Device; } std::vector mitk::PolhemusTrackingDevice::GetAllTools() { return this->m_AllTools; } void mitk::PolhemusTrackingDevice::TrackTools() { try { /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); + Sleep(100);//Wait a bit until the tracker is ready... while ((this->GetState() == Tracking) && (localStopTracking == false)) { std::vector lastData = this->GetDevice()->GetLastFrame(); if (lastData.size() != m_AllTools.size()) { MITK_WARN << "Tool count is corrupt. Aborting!"; - return; } - - std::vector allTools = this->GetAllTools(); - for(int i=0; iSetDataValid(true); - currentTool->SetPosition(lastData.at(i).pos); - currentTool->SetOrientation(lastData.at(i).rot); - currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed()); + std::vector allTools = this->GetAllTools(); + for (int i = 0; i < allTools.size(); i++) + { + mitk::PolhemusTool::Pointer currentTool = allTools.at(i); + currentTool->SetDataValid(true); + currentTool->SetPosition(lastData.at(i).pos); + currentTool->SetOrientation(lastData.at(i).rot); + currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed()); + } } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } } catch(...) { this->StopTracking(); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to track tools. Thread stopped."; } } ITK_THREAD_RETURN_TYPE mitk::PolhemusTrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } PolhemusTrackingDevice *trackingDevice = (PolhemusTrackingDevice*)pInfo->UserData; if (trackingDevice != nullptr) trackingDevice->TrackTools(); return ITK_THREAD_RETURN_VALUE; }