diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp index 95ceb41457..c05d70f7dd 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp @@ -1,332 +1,332 @@ /*=================================================================== 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" #include 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, int toolPort) { //Only add tool if port isn't already used. for (auto _tool : m_AllTools) { if (_tool->GetToolPort() == toolPort) { MITK_DEBUG << "There is already a tool connected to this port. Returning existing tool"; return _tool; } } mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New(); t->SetToolName(toolName); t->SetToolPort(toolPort); 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); this->m_StopTrackingMutex->Lock(); this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); 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() { m_Device->StopTracking(); 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(); } if (!m_Device->Connect()) //Connect the device, if it fails, throw an error. { MITK_ERROR << "Cannot connect Polhemus device!"; CloseConnection(); return false; } //Ready must be set here, 'cause if tools don't match we need to be able to disconnect. this->SetState(Ready); //check if connected ports of Polhemus matches the tools in the toolStorage. std::vector toolPorts = m_Device->GetToolPorts(); //first, check size. if (this->GetToolCount() != toolPorts.size()) { MITK_ERROR << "Cannot connect device, number of tools in toolstorage doesn't match the number of tools connected to Polhemus device!"; CloseConnection(); return false; } //second, check if toolStorage identifier is included in this port. for (auto _tool : m_AllTools) { if (std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()) == toolPorts.end()) { MITK_ERROR << "Cannot connect device, tool " << _tool->GetToolPort() << " is not connected to its port."; CloseConnection(); return false; } else { //erase this port to avoid that two tools want to connect to the same port. toolPorts.erase(std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort())); } } m_Device->SetHemisphereTrackingEnabled(m_HemisphereTrackingEnabled); 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!"; + MITK_WARN << "Tool count is corrupt. Hardware gives " << lastData.size() << " tools, MITK expects " << m_AllTools.size() << " tools. Aborting!"; } else { 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; } bool mitk::PolhemusTrackingDevice::AutoDetectToolsAvailable() { return true; } mitk::NavigationToolStorage::Pointer mitk::PolhemusTrackingDevice::AutoDetectTools() { std::vector singeFrameData = this->m_Device->AutoDetectTools(); MITK_INFO << "Found " << singeFrameData.size() << " tools."; mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); for each (mitk::PolhemusInterface::trackingData t in singeFrameData) { mitk::DataNode::Pointer newNode = mitk::DataNode::New(); std::stringstream name; name << "Sensor-" << ((int)t.id); newNode->SetName(name.str()); mitk::Surface::Pointer myCone = mitk::Surface::New(); vtkConeSource *vtkData = vtkConeSource::New(); vtkData->SetAngle(5.0); vtkData->SetResolution(50); vtkData->SetHeight(6.0f); vtkData->SetRadius(2.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); myCone->SetVtkPolyData(vtkData->GetOutput()); vtkData->Delete(); newNode->SetData(myCone); mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); newTool->SetDataNode(newNode); //The identifier defines, which plug is used (e.g. "Sens 2" --> 2). std::stringstream identifier; identifier << ((int)t.id); newTool->SetIdentifier(identifier.str()); newTool->SetTrackingDeviceType(mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty().Line); returnValue->AddTool(newTool); } return returnValue; } void mitk::PolhemusTrackingDevice::SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled) { //We need to remember if HemisphereTracking is switch on for this reason: /* m_Device->SetHemi works only if the device is connected. However, GUI can also change if it is not connected. In this case, we remember it in the m_HemisphereTrackingEnabled variable. And when connecting, we know, which status is wanted from the user by GUI. */ m_HemisphereTrackingEnabled = _HemisphereTrackingEnabled; this->m_Device->SetHemisphereTrackingEnabled(_HemisphereTrackingEnabled); } void mitk::PolhemusTrackingDevice::ToggleHemisphere(int _tool) { this->m_Device->ToggleHemisphere(_tool); } void mitk::PolhemusTrackingDevice::SetHemisphere(int _tool, mitk::Vector3D _hemisphere) { //If you set a hemisphere vector which is unequal (0|0|0), this means, that there is no hemisphere tracking any more //disable the option, so that it can be reactivated... Also if it is just a single tool. if (_hemisphere.GetNorm() != 0) m_HemisphereTrackingEnabled = false; this->m_Device->SetHemisphere(_tool, _hemisphere); } mitk::Vector3D mitk::PolhemusTrackingDevice::GetHemisphere(int _tool) { return this->m_Device->GetHemisphere(_tool); } bool mitk::PolhemusTrackingDevice::GetHemisphereTrackingEnabled(int _tool) { return this->m_Device->GetHemisphereTrackingEnabled(_tool); } void mitk::PolhemusTrackingDevice::AdjustHemisphere(int _tool) { return this->m_Device->AdjustHemisphere(_tool); } \ No newline at end of file