diff --git a/Modules/IGT/DataManagement/mitkTrackingDeviceSource.cpp b/Modules/IGT/DataManagement/mitkTrackingDeviceSource.cpp index bd6db28a1b..4ce29f5774 100644 --- a/Modules/IGT/DataManagement/mitkTrackingDeviceSource.cpp +++ b/Modules/IGT/DataManagement/mitkTrackingDeviceSource.cpp @@ -1,221 +1,224 @@ /*=================================================================== 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 "mitkTrackingDeviceSource.h" #include "mitkTrackingDevice.h" #include "mitkTrackingTool.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTException.h" mitk::TrackingDeviceSource::TrackingDeviceSource() : mitk::NavigationDataSource(), m_TrackingDevice(NULL) { } mitk::TrackingDeviceSource::~TrackingDeviceSource() { if (m_TrackingDevice.IsNotNull()) { if (m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking) { this->StopTracking(); } if (m_TrackingDevice->GetState() == mitk::TrackingDevice::Ready) { this->Disconnect(); } m_TrackingDevice = NULL; } } void mitk::TrackingDeviceSource::GenerateData() { if (m_TrackingDevice.IsNull()) return; if (m_TrackingDevice->GetToolCount() < 1) return; if (this->GetNumberOfIndexedOutputs() != m_TrackingDevice->GetToolCount()) // mismatch between tools and outputs. What should we do? Were tools added to the tracking device after SetTrackingDevice() was called? { //check this: TODO: ////this might happen if a tool is plugged into an aurora during tracking. //this->CreateOutputs(); std::stringstream ss; ss << "mitk::TrackingDeviceSource: not enough outputs available for all tools. " << this->GetNumberOfOutputs() << " outputs available, but " << m_TrackingDevice->GetToolCount() << " tools available in the tracking device."; throw std::out_of_range(ss.str()); } /* update outputs with tracking data from tools */ unsigned int toolCount = m_TrackingDevice->GetToolCount(); for (unsigned int i = 0; i < toolCount; ++i) { mitk::NavigationData* nd = this->GetOutput(i); assert(nd); mitk::TrackingTool* t = m_TrackingDevice->GetTool(i); assert(t); if ((t->IsEnabled() == false) || (t->IsDataValid() == false)) { nd->SetDataValid(false); continue; } nd->SetDataValid(true); mitk::NavigationData::PositionType p; t->GetPosition(p); nd->SetPosition(p); mitk::NavigationData::OrientationType o; t->GetOrientation(o); nd->SetOrientation(o); nd->SetOrientationAccuracy(t->GetTrackingError()); nd->SetPositionAccuracy(t->GetTrackingError()); - nd->SetIGTTimeStamp( mitk::IGTTimeStamp::GetInstance()->GetElapsed() ); + nd->SetIGTTimeStamp(t->GetIGTTimeStamp()); + + //for backward compatibility: check if the timestamp was set, if not create a default timestamp + if (nd->GetIGTTimeStamp()==0) nd->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed()); } } void mitk::TrackingDeviceSource::SetTrackingDevice( mitk::TrackingDevice* td ) { MITK_DEBUG << "Setting TrackingDevice to " << td; if (this->m_TrackingDevice.GetPointer() != td) { this->m_TrackingDevice = td; this->CreateOutputs(); std::stringstream name; // create a human readable name for the source name << td->GetData().Model << " Tracking Source"; this->SetName(name.str()); } } void mitk::TrackingDeviceSource::CreateOutputs(){ //if outputs are set then delete them if (this->GetNumberOfOutputs() > 0) { for (unsigned int numOP = this->GetNumberOfOutputs(); numOP>0; numOP--) this->RemoveOutput(this->GetOutput(numOP)); this->Modified(); } //fill the outputs if a valid tracking device is set if (m_TrackingDevice.IsNull()) return; this->SetNumberOfIndexedOutputs(m_TrackingDevice->GetToolCount()); // create outputs for all tools unsigned int numberOfOutputs = this->GetNumberOfIndexedOutputs(); MITK_DEBUG << "Number of tools at start of method CreateOutputs(): " << m_TrackingDevice->GetToolCount(); MITK_DEBUG << "Number of outputs at start of method CreateOutputs(): " << numberOfOutputs; for (unsigned int idx = 0; idx < numberOfOutputs; ++idx) { if (this->GetOutput(idx) == NULL) { DataObjectPointer newOutput = this->MakeOutput(idx); static_cast(newOutput.GetPointer())->SetName(m_TrackingDevice->GetTool(idx)->GetToolName()); // set NavigationData name to ToolName this->SetNthOutput(idx, newOutput); this->Modified(); } } } void mitk::TrackingDeviceSource::Connect() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (this->IsConnected()) return; try {m_TrackingDevice->OpenConnection();} catch (mitk::IGTException &e) { throw std::runtime_error(std::string("mitk::TrackingDeviceSource: Could not open connection to tracking device. Error: ") + e.GetDescription()); } /* NDI Aurora needs a connection to discover tools that are connected to it. Therefore we need to create outputs for these tools now */ //if (m_TrackingDevice->GetType() == mitk::NDIAurora) //this->CreateOutputs(); } void mitk::TrackingDeviceSource::StartTracking() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking) return; if (m_TrackingDevice->StartTracking() == false) throw std::runtime_error("mitk::TrackingDeviceSource: Could not start tracking"); } void mitk::TrackingDeviceSource::Disconnect() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (m_TrackingDevice->CloseConnection() == false) throw std::runtime_error("mitk::TrackingDeviceSource: Could not close connection to tracking device"); } void mitk::TrackingDeviceSource::StopTracking() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (m_TrackingDevice->StopTracking() == false) throw std::runtime_error("mitk::TrackingDeviceSource: Could not stop tracking"); } void mitk::TrackingDeviceSource::UpdateOutputInformation() { if(this->GetTrackingDevice()->GetToolCount() != this->GetNumberOfIndexedOutputs()) this->CreateOutputs(); this->Modified(); // make sure that we need to be updated Superclass::UpdateOutputInformation(); } //unsigned int mitk::TrackingDeviceSource::GetToolCount() //{ // if (m_TrackingDevice) // return m_TrackingDevice->GetToolCount(); // return 0; //} bool mitk::TrackingDeviceSource::IsConnected() { if (m_TrackingDevice.IsNull()) return false; return (m_TrackingDevice->GetState() == mitk::TrackingDevice::Ready) || (m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking); } bool mitk::TrackingDeviceSource::IsTracking() { if (m_TrackingDevice.IsNull()) return false; return m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking; } diff --git a/Modules/IGT/TrackingDevices/mitkClaronTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkClaronTrackingDevice.cpp index 33f44e8239..c78c983108 100644 --- a/Modules/IGT/TrackingDevices/mitkClaronTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkClaronTrackingDevice.cpp @@ -1,325 +1,328 @@ /*=================================================================== 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 "mitkClaronTrackingDevice.h" #include "mitkClaronTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::ClaronTrackingDevice::ClaronTrackingDevice(): mitk::TrackingDevice() { //set the type of this tracking device this->m_Data = mitk::DeviceDataMicronTrackerH40; this->m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = 0; m_Device = mitk::ClaronInterface::New(); //############################# standard directories (from cmake) ################################## if (m_Device->IsMicronTrackerInstalled()) { #ifdef MITK_MICRON_TRACKER_TEMP_DIR m_ToolfilesDir = std::string(MITK_MICRON_TRACKER_TEMP_DIR); m_ToolfilesDir.append("/MT-tools"); #endif #ifdef MITK_MICRON_TRACKER_CALIBRATION_DIR m_CalibrationDir = std::string(MITK_MICRON_TRACKER_CALIBRATION_DIR); #endif } else { m_ToolfilesDir = "Error - No Microntracker installed"; m_CalibrationDir = "Error - No Microntracker installed"; } //################################################################################################## m_Device->Initialize(m_CalibrationDir, m_ToolfilesDir); } mitk::ClaronTrackingDevice::~ClaronTrackingDevice() { } mitk::TrackingTool* mitk::ClaronTrackingDevice::AddTool( const char* toolName, const char* fileName ) { mitk::ClaronTool::Pointer t = mitk::ClaronTool::New(); if (t->LoadFile(fileName) == false) { return NULL; } t->SetToolName(toolName); if (this->InternalAddTool(t) == false) return NULL; return t.GetPointer(); } bool mitk::ClaronTrackingDevice::InternalAddTool(ClaronTool::Pointer tool) { m_AllTools.push_back(tool); return true; } std::vector mitk::ClaronTrackingDevice::DetectTools() { std::vector returnValue; std::vector allHandles = m_Device->GetAllActiveTools(); for (std::vector::iterator iter = allHandles.begin(); iter != allHandles.end(); ++iter) { ClaronTool::Pointer newTool = ClaronTool::New(); newTool->SetToolName(m_Device->GetName(*iter)); newTool->SetCalibrationName(m_Device->GetName(*iter)); newTool->SetToolHandle(*iter); returnValue.push_back(newTool); } return returnValue; } bool mitk::ClaronTrackingDevice::StartTracking() { //By Alfred: next line because no temp directory is set if MicronTracker is not installed if (!m_Device->IsMicronTrackerInstalled()) return false; //################################################################################## //be sure that the temp-directory is empty at start: delete all files in the tool files directory itksys::SystemTools::RemoveADirectory(m_ToolfilesDir.c_str()); itksys::SystemTools::MakeDirectory(m_ToolfilesDir.c_str()); //copy all toolfiles into the temp directory for (unsigned int i=0; iGetFile().c_str(), m_ToolfilesDir.c_str()); } this->SetState(Tracking); // go to mode Tracking this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); //restart the Microntracker, so it will load the new tool files m_Device->StopTracking(); m_Device->Initialize(m_CalibrationDir,m_ToolfilesDir); m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread if (m_Device->StartTracking()) { mitk::IGTTimeStamp::GetInstance()->Start(this); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } else {mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!";} } bool mitk::ClaronTrackingDevice::StopTracking() { Superclass::StopTracking(); //delete all files in the tool files directory itksys::SystemTools::RemoveADirectory(m_ToolfilesDir.c_str()); return true; } unsigned int mitk::ClaronTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::ClaronTrackingDevice::GetTool(unsigned int toolNumber) const { if ( toolNumber >= this->GetToolCount()) return NULL; else return this->m_AllTools[toolNumber]; } bool mitk::ClaronTrackingDevice::OpenConnection() { bool returnValue; //Create the temp directory itksys::SystemTools::MakeDirectory(m_ToolfilesDir.c_str()); m_Device->Initialize(m_CalibrationDir,m_ToolfilesDir); returnValue = m_Device->StartTracking(); if (returnValue) { this->SetState(Ready); } else { //reset everything if (m_Device.IsNull()) { m_Device = mitk::ClaronInterface::New(); m_Device->Initialize(m_CalibrationDir, m_ToolfilesDir); } m_Device->StopTracking(); this->SetState(Setup); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to open connection to the MicronTracker."; } return returnValue; } bool mitk::ClaronTrackingDevice::CloseConnection() { bool returnValue = true; if (this->GetState() == Setup) return true; returnValue = m_Device->StopTracking(); //delete the temporary directory itksys::SystemTools::RemoveADirectory(m_ToolfilesDir.c_str()); this->SetState(Setup); return returnValue; } mitk::ClaronInterface* mitk::ClaronTrackingDevice::GetDevice() { return m_Device; } std::vector mitk::ClaronTrackingDevice::GetAllTools() { return this->m_AllTools; } void mitk::ClaronTrackingDevice::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(); while ((this->GetState() == Tracking) && (localStopTracking == false)) { this->GetDevice()->GrabFrame(); std::vector detectedTools = this->DetectTools(); std::vector allTools = this->GetAllTools(); std::vector::iterator itAllTools; for(itAllTools = allTools.begin(); itAllTools != allTools.end(); itAllTools++) { mitk::ClaronTool::Pointer currentTool = *itAllTools; //test if current tool was detected std::vector::iterator itDetectedTools; bool foundTool = false; for(itDetectedTools = detectedTools.begin(); itDetectedTools != detectedTools.end(); itDetectedTools++) { mitk::ClaronTool::Pointer aktuDet = *itDetectedTools; std::string tempString(currentTool->GetCalibrationName()); if (tempString.compare(aktuDet->GetCalibrationName())==0) { currentTool->SetToolHandle(aktuDet->GetToolHandle()); foundTool = true; } } if (!foundTool) { currentTool->SetToolHandle(0); } if (currentTool->GetToolHandle() != 0) { currentTool->SetDataValid(true); //get tip position of tool: std::vector pos_vector = this->GetDevice()->GetTipPosition(currentTool->GetToolHandle()); //write tip position into tool: mitk::Point3D pos; pos[0] = pos_vector[0]; pos[1] = pos_vector[1]; pos[2] = pos_vector[2]; currentTool->SetPosition(pos); //get tip quaternion of tool std::vector quat = this->GetDevice()->GetTipQuaternions(currentTool->GetToolHandle()); //write tip quaternion into tool mitk::Quaternion orientation(quat[1], quat[2], quat[3], quat[0]); currentTool->SetOrientation(orientation); + + //TODO: read the timestamp data from the tracking device interface + currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed()); } else { mitk::Point3D origin; origin.Fill(0); currentTool->SetPosition(origin); currentTool->SetOrientation(mitk::Quaternion(0,0,0,0)); currentTool->SetDataValid(false); } } /* 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."; } } bool mitk::ClaronTrackingDevice::IsMicronTrackerInstalled() { return this->m_Device->IsMicronTrackerInstalled(); } ITK_THREAD_RETURN_TYPE mitk::ClaronTrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == NULL) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == NULL) { return ITK_THREAD_RETURN_VALUE; } ClaronTrackingDevice *trackingDevice = (ClaronTrackingDevice*)pInfo->UserData; if (trackingDevice != NULL) trackingDevice->TrackTools(); return ITK_THREAD_RETURN_VALUE; }