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; } diff --git a/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp b/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp index e917a9dcb5..8402b05be5 100644 --- a/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp +++ b/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp @@ -1,48 +1,48 @@ /*=================================================================== 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 "mitkTrackingTool.h" #include typedef itk::MutexLockHolder MutexLockHolder; mitk::TrackingTool::TrackingTool() -: itk::Object(), m_ToolName(""), m_ErrorMessage("") +: itk::Object(), m_ToolName(""), m_ErrorMessage(""), m_IGTTimeStamp(0) { m_MyMutex = itk::FastMutexLock::New(); } mitk::TrackingTool::~TrackingTool() { m_MyMutex->Unlock(); m_MyMutex = NULL; } const char* mitk::TrackingTool::GetToolName() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return this->m_ToolName.c_str(); } const char* mitk::TrackingTool::GetErrorMessage() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return this->m_ErrorMessage.c_str(); } diff --git a/Modules/IGT/TrackingDevices/mitkTrackingTool.h b/Modules/IGT/TrackingDevices/mitkTrackingTool.h index 16e9e068a3..573c58cd88 100644 --- a/Modules/IGT/TrackingDevices/mitkTrackingTool.h +++ b/Modules/IGT/TrackingDevices/mitkTrackingTool.h @@ -1,59 +1,63 @@ /*=================================================================== 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. ===================================================================*/ #ifndef MITKTRACKINGTOOL_H_HEADER_INCLUDED_ #define MITKTRACKINGTOOL_H_HEADER_INCLUDED_ #include #include #include #include #include namespace mitk { /**Documentation * \brief Interface for all Tracking Tools * * Abstract class that defines the methods that are common for all tracking tools. * * \ingroup IGT */ class MitkIGT_EXPORT TrackingTool : public itk::Object { public: mitkClassMacro(TrackingTool, itk::Object); virtual void SetToolTip(mitk::Point3D toolTipPosition, mitk::Quaternion orientation) = 0; ///< defines a tool tip for this tool in tool coordinates. GetPosition() and GetOrientation() return the data of the tool tip if it is defined. By default no tooltip is defined. virtual void GetPosition(mitk::Point3D& position) const = 0; ///< returns the current position of the tool as an array of three floats (in the tracking device coordinate system) virtual void GetOrientation(mitk::Quaternion& orientation) const = 0; ///< returns the current orientation of the tool as a quaternion in a mitk::Point4D (in the tracking device coordinate system) virtual bool Enable() = 0; ///< enables the tool, so that it will be tracked virtual bool Disable() = 0; ///< disables the tool, so that it will not be tracked anymore virtual bool IsEnabled() const = 0; ///< returns whether the tool is enabled or disabled virtual bool IsDataValid() const = 0; ///< returns true if the current position data is valid (no error during tracking, tracking error below threshold, ...) virtual float GetTrackingError() const = 0; ///< returns one value that corresponds to the overall tracking error. virtual const char* GetToolName() const; ///< every tool has a name that can be used to identify it. virtual const char* GetErrorMessage() const; ///< if the data is not valid, ErrorMessage should contain a string explaining why it is invalid (the Set-method should be implemented in subclasses, it should not be accessible by the user) + itkSetMacro(IGTTimeStamp, double); ///< Sets the IGT timestamp of the tracking tool object + itkGetConstMacro(IGTTimeStamp, double); ///< Gets the IGT timestamp of the tracking tool object. Returns 0 if the timestamp was not set. + protected: TrackingTool(); virtual ~TrackingTool(); std::string m_ToolName; ///< every tool has a name that can be used to identify it. std::string m_ErrorMessage; ///< if a tool is invalid, this member should contain a human readable explanation of why it is invalid + double m_IGTTimeStamp; ///< contains the time at which the tracking data was recorded itk::FastMutexLock::Pointer m_MyMutex; ///< mutex to control concurrent access to the tool }; } // namespace mitk #endif /* MITKTRACKINGTOOL_H_HEADER_INCLUDED_ */