diff --git a/Modules/Bundles/org.mitk.gui.qt.igttrackingtoolbox/src/internal/QmitkMITKIGTTrackingToolboxView.cpp b/Modules/Bundles/org.mitk.gui.qt.igttrackingtoolbox/src/internal/QmitkMITKIGTTrackingToolboxView.cpp index 051b79074a..29cc893996 100644 --- a/Modules/Bundles/org.mitk.gui.qt.igttrackingtoolbox/src/internal/QmitkMITKIGTTrackingToolboxView.cpp +++ b/Modules/Bundles/org.mitk.gui.qt.igttrackingtoolbox/src/internal/QmitkMITKIGTTrackingToolboxView.cpp @@ -1,485 +1,485 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date$ Version: $Revision$ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ // Blueberry #include #include // Qmitk #include "QmitkMITKIGTTrackingToolboxView.h" #include "QmitkStdMultiWidget.h" // Qt #include #include // MITK #include #include #include #include #include #include #include // vtk #include const std::string QmitkMITKIGTTrackingToolboxView::VIEW_ID = "org.mitk.views.mitkigttrackingtoolbox"; QmitkMITKIGTTrackingToolboxView::QmitkMITKIGTTrackingToolboxView() : QmitkFunctionality() , m_Controls( 0 ) , m_MultiWidget( NULL ) { m_TrackingTimer = new QTimer(this); m_tracking = false; m_logging = false; m_loggedFrames = 0; } QmitkMITKIGTTrackingToolboxView::~QmitkMITKIGTTrackingToolboxView() { } void QmitkMITKIGTTrackingToolboxView::CreateQtPartControl( QWidget *parent ) { // build up qt view, unless already done if ( !m_Controls ) { // create GUI widgets from the Qt Designer's .ui file m_Controls = new Ui::QmitkMITKIGTTrackingToolboxViewControls; m_Controls->setupUi( parent ); //create connections connect( m_Controls->m_LoadTools, SIGNAL(clicked()), this, SLOT(OnLoadTools()) ); connect( m_Controls->m_StartTracking, SIGNAL(clicked()), this, SLOT(OnStartTracking()) ); connect( m_Controls->m_StopTracking, SIGNAL(clicked()), this, SLOT(OnStopTracking()) ); connect( m_TrackingTimer, SIGNAL(timeout()), this, SLOT(UpdateTrackingTimer())); connect( m_Controls->m_ChooseFile, SIGNAL(clicked()), this, SLOT(OnChooseFileClicked())); connect( m_Controls->m_StartLogging, SIGNAL(clicked()), this, SLOT(StartLogging())); connect( m_Controls->m_StopLogging, SIGNAL(clicked()), this, SLOT(StopLogging())); connect( m_Controls->m_configurationWidget, SIGNAL(TrackingDeviceSelectionChanged()), this, SLOT(OnTrackingDeviceChanged())); connect( m_Controls->VolumeSelectionBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(OnTrackingVolumeChanged(QString))); connect( m_Controls->m_AutoDetectTools, SIGNAL(clicked()), this, SLOT(OnAutoDetectTools())); //initialize widgets m_Controls->m_configurationWidget->EnableAdvancedUserControl(false); m_Controls->m_TrackingToolsStatusWidget->SetShowPositions(true); m_Controls->m_TrackingToolsStatusWidget->SetTextAlignment(Qt::AlignLeft); //initialize tracking volume node m_TrackingVolumeNode = mitk::DataNode::New(); m_TrackingVolumeNode->SetName("TrackingVolume"); this->GetDataStorage()->Add(m_TrackingVolumeNode); //initialize buttons m_Controls->m_StopTracking->setEnabled(false); m_Controls->m_StopLogging->setEnabled(false); m_Controls->m_AutoDetectTools->setVisible(false); //only visible if tracking device is Aurora // Update List of available models for selected tool. std::vector Compatibles = mitk::GetDeviceDataForLine( m_Controls->m_configurationWidget->GetTrackingDevice()->GetType()); m_Controls->VolumeSelectionBox->clear(); for(int i = 0; i < Compatibles.size(); i++) { m_Controls->VolumeSelectionBox->addItem(Compatibles[i].Model.c_str()); } } } void QmitkMITKIGTTrackingToolboxView::StdMultiWidgetAvailable (QmitkStdMultiWidget &stdMultiWidget) { m_MultiWidget = &stdMultiWidget; } void QmitkMITKIGTTrackingToolboxView::StdMultiWidgetNotAvailable() { m_MultiWidget = NULL; } void QmitkMITKIGTTrackingToolboxView::OnLoadTools() { //read in filename QString filename = QFileDialog::getOpenFileName(NULL,tr("Open Toolfile"), "/", tr("All Files (*.*)")); //later perhaps: tr("Toolfile (*.tfl)" if (filename.isNull()) return; //read tool storage from disk mitk::NavigationToolStorageDeserializer::Pointer myDeserializer = mitk::NavigationToolStorageDeserializer::New(GetDataStorage()); m_toolStorage = myDeserializer->Deserialize(filename.toStdString()); if (m_toolStorage.IsNull()) { MessageBox(myDeserializer->GetErrorMessage()); m_toolStorage = NULL; return; } //update label Poco::Path myPath = Poco::Path(filename.toStdString()); //use this to seperate filename from path QString toolLabel = QString("Loaded Tools: ") + QString::number(m_toolStorage->GetToolCount()) + " Tools from " + myPath.getFileName().c_str(); m_Controls->m_toolLabel->setText(toolLabel); //update tool preview m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); } void QmitkMITKIGTTrackingToolboxView::OnStartTracking() { //check if everything is ready to start tracking if (this->m_toolStorage.IsNull()) { MessageBox("Error: No Tools Loaded Yet!"); return; } else if (this->m_toolStorage->GetToolCount() == 0) { MessageBox("Error: No Way To Track Without Tools!"); return; } //build the IGT pipeline mitk::TrackingDevice::Pointer trackingDevice = this->m_Controls->m_configurationWidget->GetTrackingDevice(); //trackingDevice->setD mitk::TrackingDeviceSourceConfigurator::Pointer myTrackingDeviceSourceFactory = mitk::TrackingDeviceSourceConfigurator::New(this->m_toolStorage,trackingDevice); m_TrackingDeviceSource = myTrackingDeviceSourceFactory->CreateTrackingDeviceSource(this->m_ToolVisualizationFilter); if (m_TrackingDeviceSource.IsNull()) { MessageBox(myTrackingDeviceSourceFactory->GetErrorMessage()); return; } //initialize tracking try { m_TrackingDeviceSource->Connect(); m_TrackingDeviceSource->StartTracking(); } catch (...) { MessageBox("Error while starting the tracking device!"); return; } m_TrackingTimer->start(1000/(m_Controls->m_UpdateRate->value())); m_Controls->m_TrackingControlLabel->setText("Status: tracking"); //connect the tool visualization widget for(int i=0; iGetNumberOfOutputs(); i++) { m_Controls->m_TrackingToolsStatusWidget->AddNavigationData(m_TrackingDeviceSource->GetOutput(i)); } m_Controls->m_TrackingToolsStatusWidget->ShowStatusLabels(); if (m_Controls->m_ShowToolQuaternions->isChecked()) {m_Controls->m_TrackingToolsStatusWidget->SetShowQuaternions(true);} else {m_Controls->m_TrackingToolsStatusWidget->SetShowQuaternions(false);} //disable loading new tools this->m_Controls->m_LoadTools->setEnabled(false); this->m_Controls->m_AutoDetectTools->setEnabled(false); //set configuration finished this->m_Controls->m_configurationWidget->ConfigurationFinished(); -//show tracking volume +//show tracking volume (old code) if (m_Controls->m_ShowTrackingVolume->isChecked()) { mitk::TrackingVolumeGenerator::Pointer volumeGenerator = mitk::TrackingVolumeGenerator::New(); volumeGenerator->SetTrackingDeviceType(m_TrackingDeviceSource->GetTrackingDevice()->GetType()); volumeGenerator->Update(); mitk::Surface::Pointer volumeSurface = volumeGenerator->GetOutput(); m_TrackingVolumeNode->SetData(volumeSurface); m_TrackingVolumeNode->SetOpacity(0.25); mitk::Color red; red.SetRed(1); m_TrackingVolumeNode->SetColor(red); } m_tracking = true; //disable Buttons m_Controls->m_StopTracking->setEnabled(true); m_Controls->m_StartTracking->setEnabled(false); DisableOptionsButtons(); this->GlobalReinit(); } void QmitkMITKIGTTrackingToolboxView::OnStopTracking() { if (!m_tracking) return; m_TrackingTimer->stop(); m_TrackingDeviceSource->StopTracking(); m_TrackingDeviceSource->Disconnect(); this->m_Controls->m_configurationWidget->Reset(); m_Controls->m_TrackingControlLabel->setText("Status: stopped"); if (m_logging) StopLogging(); this->m_Controls->m_LoadTools->setEnabled(true); this->m_Controls->m_AutoDetectTools->setEnabled(true); m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); m_TrackingVolumeNode->SetData(NULL); m_tracking = false; //enable Buttons m_Controls->m_StopTracking->setEnabled(false); m_Controls->m_StartTracking->setEnabled(true); EnableOptionsButtons(); this->GlobalReinit(); } void QmitkMITKIGTTrackingToolboxView::OnTrackingDeviceChanged() { mitk::TrackingDeviceType Type = m_Controls->m_configurationWidget->GetTrackingDevice()->GetType(); // Code to enable auto detection if (Type == mitk::NDIAurora) {m_Controls->m_AutoDetectTools->setVisible(true);} else {m_Controls->m_AutoDetectTools->setVisible(false);} // Code to select appropriate tracking volumes std::vector Compatibles = mitk::GetDeviceDataForLine(Type); m_Controls->VolumeSelectionBox->clear(); for(int i = 0; i < Compatibles.size(); i++) { m_Controls->VolumeSelectionBox->addItem(Compatibles[i].Model.c_str()); } } void QmitkMITKIGTTrackingToolboxView::OnTrackingVolumeChanged(QString qstr) { if (qstr.isNull()) return; if (qstr.isEmpty()) return; if (m_Controls->m_ShowTrackingVolume->isChecked()) { mitk::TrackingVolumeGenerator::Pointer volumeGenerator = mitk::TrackingVolumeGenerator::New(); std::string str = qstr.toStdString(); mitk::TrackingDeviceData data = mitk::GetDeviceDataByName(str); volumeGenerator->SetTrackingDeviceData(data); volumeGenerator->Update(); mitk::Surface::Pointer volumeSurface = volumeGenerator->GetOutput(); m_TrackingVolumeNode->SetData(volumeSurface); m_TrackingVolumeNode->SetOpacity(0.25); mitk::Color red; red.SetRed(1); m_TrackingVolumeNode->SetColor(red); } } void QmitkMITKIGTTrackingToolboxView::OnAutoDetectTools() { if (m_Controls->m_configurationWidget->GetTrackingDevice()->GetType() == mitk::NDIAurora) { mitk::NDITrackingDevice::Pointer currentDevice = dynamic_cast(m_Controls->m_configurationWidget->GetTrackingDevice().GetPointer()); currentDevice->OpenConnection(); currentDevice->StartTracking(); mitk::NavigationToolStorage::Pointer autoDetectedStorage = mitk::NavigationToolStorage::New(this->GetDataStorage()); for (int i=0; iGetToolCount(); i++) { //create a navigation tool with sphere as surface std::stringstream toolname; toolname << "AutoDetectedTool" << i; mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); newTool->SetSerialNumber(dynamic_cast(currentDevice->GetTool(i))->GetSerialNumber()); newTool->SetIdentifier(toolname.str()); newTool->SetTrackingDeviceType(mitk::NDIAurora); mitk::DataNode::Pointer newNode = mitk::DataNode::New(); mitk::Surface::Pointer mySphere = mitk::Surface::New(); vtkSphereSource *vtkData = vtkSphereSource::New(); vtkData->SetRadius(3.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); mySphere->SetVtkPolyData(vtkData->GetOutput()); vtkData->Delete(); newNode->SetData(mySphere); newNode->SetName(toolname.str()); newTool->SetDataNode(newNode); autoDetectedStorage->AddTool(newTool); } //save detected tools m_toolStorage = autoDetectedStorage; //update label QString toolLabel = QString("Loaded Tools: ") + QString::number(m_toolStorage->GetToolCount()) + " Tools (Auto Detected)"; m_Controls->m_toolLabel->setText(toolLabel); //update tool preview m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); currentDevice->StopTracking(); currentDevice->CloseConnection(); if (m_toolStorage->GetToolCount()>0) { //ask the user if he wants to save the detected tools QMessageBox msgBox; msgBox.setText("Found " + QString::number(m_toolStorage->GetToolCount()) + " tools!"); msgBox.setInformativeText("Do you want to save this tools as tool storage, so you can load them again?"); msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); msgBox.setDefaultButton(QMessageBox::No); int ret = msgBox.exec(); if (ret == 16384) //yes { //ask the user for a filename QString fileName = QFileDialog::getSaveFileName(NULL, tr("Save File"),"",tr("*.*")); mitk::NavigationToolStorageSerializer::Pointer mySerializer = mitk::NavigationToolStorageSerializer::New(); if (!mySerializer->Serialize(fileName.toStdString(),m_toolStorage)) MessageBox(mySerializer->GetErrorMessage()); return; } else if (ret == 65536) //no { return; } } } } void QmitkMITKIGTTrackingToolboxView::MessageBox(std::string s) { QMessageBox msgBox; msgBox.setText(s.c_str()); msgBox.exec(); } void QmitkMITKIGTTrackingToolboxView::UpdateTrackingTimer() { m_ToolVisualizationFilter->Update(); mitk::RenderingManager::GetInstance()->RequestUpdateAll(); if (m_logging) { this->m_loggingFilter->Update(); m_loggedFrames = this->m_loggingFilter->GetRecordCounter(); this->m_Controls->m_LoggedFramesLabel->setText("Logged Frames: "+QString::number(m_loggedFrames)); //check if logging stopped automatically if((m_loggedFrames>1)&&(!m_loggingFilter->GetRecording())){StopLogging();} } m_Controls->m_TrackingToolsStatusWidget->Refresh(); } void QmitkMITKIGTTrackingToolboxView::OnChooseFileClicked() { QString filename = QFileDialog::getSaveFileName(NULL,tr("Choose Logging File"), "/", "*.*"); this->m_Controls->m_LoggingFileName->setText(filename); } void QmitkMITKIGTTrackingToolboxView::StartLogging() { if (!m_logging) { //initialize logging filter m_loggingFilter = mitk::NavigationDataRecorder::New(); m_loggingFilter->SetRecordingMode(mitk::NavigationDataRecorder::NormalFile); if (m_Controls->m_xmlFormat->isChecked()) m_loggingFilter->SetOutputFormat(mitk::NavigationDataRecorder::xml); else if (m_Controls->m_csvFormat->isChecked()) m_loggingFilter->SetOutputFormat(mitk::NavigationDataRecorder::csv); m_loggingFilter->SetFileName(m_Controls->m_LoggingFileName->text().toStdString().c_str()); if (m_Controls->m_LoggingLimit->isChecked()){m_loggingFilter->SetRecordCountLimit(m_Controls->m_LoggedFramesLimit->value());} //connect filter for(int i=0; iGetNumberOfOutputs(); i++){m_loggingFilter->AddNavigationData(m_ToolVisualizationFilter->GetOutput(i));} //start filter m_loggingFilter->StartRecording(); //update labels / logging variables this->m_Controls->m_LoggingLabel->setText("Logging ON"); this->m_Controls->m_LoggedFramesLabel->setText("Logged Frames: 0"); m_loggedFrames = 0; m_logging = true; DisableLoggingButtons(); } } void QmitkMITKIGTTrackingToolboxView::StopLogging() { if (m_logging) { //update label this->m_Controls->m_LoggingLabel->setText("Logging OFF"); m_loggingFilter->StopRecording(); m_logging = false; EnableLoggingButtons(); } } void QmitkMITKIGTTrackingToolboxView::GlobalReinit() { // get all nodes that have not set "includeInBoundingBox" to false mitk::NodePredicateNot::Pointer pred = mitk::NodePredicateNot::New(mitk::NodePredicateProperty::New("includeInBoundingBox", mitk::BoolProperty::New(false))); mitk::DataStorage::SetOfObjects::ConstPointer rs = this->GetDataStorage()->GetSubset(pred); // calculate bounding geometry of these nodes mitk::TimeSlicedGeometry::Pointer bounds = this->GetDataStorage()->ComputeBoundingGeometry3D(rs, "visible"); // initialize the views to the bounding geometry mitk::RenderingManager::GetInstance()->InitializeViews(bounds); } void QmitkMITKIGTTrackingToolboxView::DisableLoggingButtons() { m_Controls->m_StartLogging->setEnabled(false); m_Controls->m_LoggingFileName->setEnabled(false); m_Controls->m_ChooseFile->setEnabled(false); m_Controls->m_LoggingLimit->setEnabled(false); m_Controls->m_LoggedFramesLimit->setEnabled(false); m_Controls->m_csvFormat->setEnabled(false); m_Controls->m_xmlFormat->setEnabled(false); m_Controls->m_StopLogging->setEnabled(true); } void QmitkMITKIGTTrackingToolboxView::EnableLoggingButtons() { m_Controls->m_StartLogging->setEnabled(true); m_Controls->m_LoggingFileName->setEnabled(true); m_Controls->m_ChooseFile->setEnabled(true); m_Controls->m_LoggingLimit->setEnabled(true); m_Controls->m_LoggedFramesLimit->setEnabled(true); m_Controls->m_csvFormat->setEnabled(true); m_Controls->m_xmlFormat->setEnabled(true); m_Controls->m_StopLogging->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::DisableOptionsButtons() { m_Controls->m_ShowTrackingVolume->setEnabled(false); m_Controls->m_UpdateRate->setEnabled(false); m_Controls->m_ShowToolQuaternions->setEnabled(false); m_Controls->m_OptionsUpdateRateLabel->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::EnableOptionsButtons() { m_Controls->m_ShowTrackingVolume->setEnabled(true); m_Controls->m_UpdateRate->setEnabled(true); m_Controls->m_ShowToolQuaternions->setEnabled(true); m_Controls->m_OptionsUpdateRateLabel->setEnabled(true); } diff --git a/Modules/IGT/IGTTrackingDevices/mitkClaronTrackingDevice.cpp b/Modules/IGT/IGTTrackingDevices/mitkClaronTrackingDevice.cpp index 7a0da04437..0612bfb581 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkClaronTrackingDevice.cpp +++ b/Modules/IGT/IGTTrackingDevices/mitkClaronTrackingDevice.cpp @@ -1,328 +1,328 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date $ Version: $Revision $ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #include "mitkClaronTrackingDevice.h" #include "mitkClaronTool.h" #include "mitkIGTConfig.h" #include "mitkTimeStamp.h" #include #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::ClaronTrackingDevice::ClaronTrackingDevice(): mitk::TrackingDevice() { //set the type of this tracking device - this->m_Type = ClaronMicron; + this->m_Data = mitk::Micron; 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::TimeStamp::GetInstance()->Start(this); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } else { m_ErrorMessage = "Error while trying to start the device!"; return false; } } 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); m_ErrorMessage = "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); } 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(); this->SetErrorMessage("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; } \ No newline at end of file diff --git a/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.cpp b/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.cpp index a7f0cc3ba3..faa5c7aa00 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.cpp +++ b/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.cpp @@ -1,1322 +1,1322 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date$ Version: $Revision$ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #include "mitkNDITrackingDevice.h" #include "mitkTimeStamp.h" #include #include #include typedef itk::MutexLockHolder MutexLockHolder; const unsigned char CR = 0xD; // == '\r' - carriage return const unsigned char LF = 0xA; // == '\n' - line feed mitk::NDITrackingDevice::NDITrackingDevice() : TrackingDevice(),m_DeviceName(""), m_PortNumber(mitk::SerialCommunication::COM5), m_BaudRate(mitk::SerialCommunication::BaudRate9600), m_DataBits(mitk::SerialCommunication::DataBits8), m_Parity(mitk::SerialCommunication::None), m_StopBits(mitk::SerialCommunication::StopBits1), m_HardwareHandshake(mitk::SerialCommunication::HardwareHandshakeOff), m_NDITrackingVolume(Standard), m_IlluminationActivationRate(Hz20), m_DataTransferMode(TX), m_6DTools(), m_ToolsMutex(NULL), m_SerialCommunication(NULL), m_SerialCommunicationMutex(NULL), m_DeviceProtocol(NULL), m_MultiThreader(NULL), m_ThreadID(0), m_OperationMode(ToolTracking6D), m_MarkerPointsMutex(NULL), m_MarkerPoints() { - this->m_Type = TrackingSystemNotSpecified; //NDIPolaris; // = 0; //set the type = 0 (=Polaris, default) + this->m_Data = mitk::Unspecified; m_6DTools.clear(); m_SerialCommunicationMutex = itk::FastMutexLock::New(); m_DeviceProtocol = NDIProtocol::New(); m_DeviceProtocol->SetTrackingDevice(this); m_DeviceProtocol->UseCRCOn(); m_MultiThreader = itk::MultiThreader::New(); m_ToolsMutex = itk::FastMutexLock::New(); m_MarkerPointsMutex = itk::FastMutexLock::New(); m_MarkerPoints.reserve(50); // a maximum of 50 marker positions can be reported by the tracking device } bool mitk::NDITrackingDevice::UpdateTool(mitk::TrackingTool* tool) { if (this->GetState() != Setup) { mitk::NDIPassiveTool* ndiTool = dynamic_cast(tool); if (ndiTool == NULL) return false; std::string portHandle = ndiTool->GetPortHandle(); //return false if the SROM Data has not been set if (ndiTool->GetSROMData() == NULL) return false; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PVWR(&portHandle, ndiTool->GetSROMData(), ndiTool->GetSROMDataLength()); if (returnvalue != NDIOKAY) return false; returnvalue = m_DeviceProtocol->PINIT(&portHandle); if (returnvalue != NDIOKAY) return false; returnvalue = m_DeviceProtocol->PENA(&portHandle, ndiTool->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) return false; return true; } else { return false; } } mitk::NDITrackingDevice::~NDITrackingDevice() { /* stop tracking and disconnect from tracking device */ if (GetState() == Tracking) { this->StopTracking(); } if (GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if ((m_ThreadID != 0) && (m_MultiThreader.IsNotNull())) { m_MultiThreader->TerminateThread(m_ThreadID); } m_MultiThreader = NULL; /* free serial communication interface */ if (m_SerialCommunication.IsNotNull()) { m_SerialCommunication->ClearReceiveBuffer(); m_SerialCommunication->ClearSendBuffer(); m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; } } void mitk::NDITrackingDevice::SetPortNumber(const PortNumber _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting PortNumber to " << _arg); if (this->m_PortNumber != _arg) { this->m_PortNumber = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetDeviceName(std::string _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting eviceName to " << _arg); if (this->m_DeviceName != _arg) { this->m_DeviceName = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetBaudRate(const BaudRate _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting BaudRate to " << _arg); if (this->m_BaudRate != _arg) { this->m_BaudRate = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetDataBits(const DataBits _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting DataBits to " << _arg); if (this->m_DataBits != _arg) { this->m_DataBits = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetParity(const Parity _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting Parity to " << _arg); if (this->m_Parity != _arg) { this->m_Parity = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetStopBits(const StopBits _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting StopBits to " << _arg); if (this->m_StopBits != _arg) { this->m_StopBits = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetHardwareHandshake(const HardwareHandshake _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting HardwareHandshake to " << _arg); if (this->m_HardwareHandshake != _arg) { this->m_HardwareHandshake = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetIlluminationActivationRate(const IlluminationActivationRate _arg) { if (this->GetState() == Tracking) return; itkDebugMacro("setting IlluminationActivationRate to " << _arg); if (this->m_IlluminationActivationRate != _arg) { this->m_IlluminationActivationRate = _arg; this->Modified(); if (this->GetState() == Ready) // if the connection to the tracking system is established, send the new rate to the tracking device too m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate); } } void mitk::NDITrackingDevice::SetDataTransferMode(const DataTransferMode _arg) { itkDebugMacro("setting DataTransferMode to " << _arg); if (this->m_DataTransferMode != _arg) { this->m_DataTransferMode = _arg; this->Modified(); } } mitk::NDIErrorCode mitk::NDITrackingDevice::Send(const std::string* input, bool addCRC) { if (input == NULL) return SERIALSENDERROR; std::string message; if (addCRC == true) message = *input + CalcCRC(input) + std::string(1, CR); else message = *input + std::string(1, CR); //unsigned int messageLength = message.length() + 1; // +1 for CR // Clear send buffer this->ClearSendBuffer(); // Send the date to the device MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Send(message); if (returnvalue == 0) return SERIALSENDERROR; else return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::Receive(std::string* answer, unsigned int numberOfBytes) { if (answer == NULL) return SERIALRECEIVEERROR; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Receive(*answer, numberOfBytes); // never read more bytes than the device has send, the function will block until enough bytes are send... if (returnvalue == 0) return SERIALRECEIVEERROR; else return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveByte(char* answer) { if (answer == NULL) return SERIALRECEIVEERROR; std::string m; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Receive(m, 1); if ((returnvalue == 0) ||(m.size() != 1)) return SERIALRECEIVEERROR; *answer = m.at(0); return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveLine(std::string* answer) { if (answer == NULL) return SERIALRECEIVEERROR; std::string m; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex do { long returnvalue = m_SerialCommunication->Receive(m, 1); if ((returnvalue == 0) ||(m.size() != 1)) return SERIALRECEIVEERROR; *answer += m; } while (m.at(0) != LF); return NDIOKAY; } void mitk::NDITrackingDevice::ClearSendBuffer() { MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex m_SerialCommunication->ClearSendBuffer(); } void mitk::NDITrackingDevice::ClearReceiveBuffer() { MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex m_SerialCommunication->ClearReceiveBuffer(); } const std::string mitk::NDITrackingDevice::CalcCRC(const std::string* input) { if (input == NULL) return ""; /* the crc16 calculation code is taken from the NDI API guide example code section */ static int oddparity[16] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0}; unsigned int data; // copy of the input string's current character unsigned int crcValue = 0; // the crc value is stored here unsigned int* puCRC16 = &crcValue; // the algorithm uses a pointer to crcValue, so it's easier to provide that than to change the algorithm for (unsigned int i = 0; i < input->length(); i++) { data = (*input)[i]; data = (data ^ (*(puCRC16) & 0xff)) & 0xff; *puCRC16 >>= 8; if (oddparity[data & 0x0f] ^ oddparity[data >> 4]) { *(puCRC16) ^= 0xc001; } data <<= 6; *puCRC16 ^= data; data <<= 1; *puCRC16 ^= data; } // crcValue contains now the CRC16 value. Convert it to a string and return it char returnvalue[13]; sprintf(returnvalue,"%04X", crcValue); // 4 hexadecimal digit with uppercase format return std::string(returnvalue); } bool mitk::NDITrackingDevice::OpenConnection() { //this->m_ModeMutex->Lock(); if (this->GetState() != Setup) { this->SetErrorMessage("Can only try to open the connection if in setup mode"); return false; } // m_SerialCommunication = mitk::SerialCommunication::New(); /* init local com port to standard com settings for a NDI tracking device: 9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake */ if (m_DeviceName.empty()) m_SerialCommunication->SetPortNumber(m_PortNumber); else m_SerialCommunication->SetDeviceName(m_DeviceName); m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600); m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8); m_SerialCommunication->SetParity(mitk::SerialCommunication::None); m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); if (m_SerialCommunication->OpenConnection() == 0) // 0 == ERROR_VALUE { this->SetErrorMessage("Can not open serial port"); m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; return false; } /* Reset Tracking device by sending a serial break for 500ms */ m_SerialCommunication->SendBreak(400); /* Read answer from tracking device (RESETBE6F) */ static const std::string reset("RESETBE6F\r"); std::string answer = ""; this->Receive(&answer, reset.length()); // read answer (should be RESETBE6F) this->ClearReceiveBuffer(); // flush the receive buffer of all remaining data (carriage return, strings other than reset if (reset.compare(answer) != 0) // check for RESETBE6F { this->SetErrorMessage("Hardware Reset of tracking device did not work"); if (m_SerialCommunication.IsNotNull()) { m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; } return false; } /* Now the tracking device is reset, start initialization */ NDIErrorCode returnvalue; /* set device com settings to new values and wait for the device to change them */ returnvalue = m_DeviceProtocol->COMM(m_BaudRate, m_DataBits, m_Parity, m_StopBits, m_HardwareHandshake); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not set comm settings in trackingdevice"); return false; } //after changing COMM wait at least 100ms according to NDI Api documentation page 31 itksys::SystemTools::Delay(500); /* now change local com settings accordingly */ m_SerialCommunication->CloseConnection(); m_SerialCommunication->SetBaudRate(m_BaudRate); m_SerialCommunication->SetDataBits(m_DataBits); m_SerialCommunication->SetParity(m_Parity); m_SerialCommunication->SetStopBits(m_StopBits); m_SerialCommunication->SetHardwareHandshake(m_HardwareHandshake); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); m_SerialCommunication->OpenConnection(); /* initialize the tracking device */ returnvalue = m_DeviceProtocol->INIT(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not initialize the tracking device"); return false; } if (this->GetType() == mitk::TrackingSystemNotSpecified) // if the type of tracking device is not specified, try to query the connected device { mitk::TrackingDeviceType deviceType; returnvalue = m_DeviceProtocol->VER(deviceType); if ((returnvalue != NDIOKAY) || (deviceType == mitk::TrackingSystemNotSpecified)) { this->SetErrorMessage("Could not determine tracking device type. Please set manually and try again."); return false; } this->SetType(deviceType); } /**** Optional Polaris specific code, Work in progress // start diagnostic mode returnvalue = m_DeviceProtocol->DSTART(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not start diagnostic mode"); return false; } else // we are in diagnostic mode { // initialize extensive IR checking returnvalue = m_DeviceProtocol->IRINIT(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not initialize intense infrared light checking"); return false; } bool intenseIR = false; returnvalue = m_DeviceProtocol->IRCHK(&intenseIR); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not execute intense infrared light checking"); return false; } if (intenseIR == true) // do something - warn the user, raise exception, write to protocol or similar std::cout << "Warning: Intense infrared light detected. Accurate tracking will probably not be possible.\n"; // stop diagnictic mode returnvalue = m_DeviceProtocol->DSTOP(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not stop diagnostic mode"); return false; } } *** end of optional polaris code ***/ /** * now add tools to the tracking system **/ /* First, check if the tracking device has port handles that need to be freed and free them */ returnvalue = FreePortHandles(); // non-critical, therefore no error handling /** * POLARIS: initialize the tools that were added manually **/ { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex std::string portHandle; Tool6DContainerType::iterator endIt = m_6DTools.end(); for(Tool6DContainerType::iterator it = m_6DTools.begin(); it != endIt; ++it) { /* get a port handle for the tool */ returnvalue = m_DeviceProtocol->PHRQ(&portHandle); if (returnvalue == NDIOKAY) { (*it)->SetPortHandle(portHandle.c_str()); /* now write the SROM file of the tool to the tracking system using PVWR */ - if (this->m_Type == NDIPolaris) + if (this->m_Data.Line == NDIPolaris) { returnvalue = m_DeviceProtocol->PVWR(&portHandle, (*it)->GetSROMData(), (*it)->GetSROMDataLength()); if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not write SROM file for tool '") + (*it)->GetToolName() + std::string("' to tracking device")).c_str()); return false; } returnvalue = m_DeviceProtocol->PINIT(&portHandle); if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not initialize tool '") + (*it)->GetToolName()).c_str()); return false; } if ((*it)->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&portHandle, (*it)->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not enable port '") + portHandle + std::string("' for tool '")+ (*it)->GetToolName() + std::string("'")).c_str()); return false; } } } } } } // end of toolsmutexlockholder scope /* check for wired tools and add them too */ if (this->DiscoverWiredTools() == false) // query the tracking device for wired tools and add them to our tool list return false; // \TODO: could we continue anyways? /*POLARIS: set the illuminator activation rate */ - if (this->m_Type == NDIPolaris) + if (this->m_Data.Line == NDIPolaris) { returnvalue = m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not set the illuminator activation rate"); return false; } } /* finish - now all tools should be added, initialized and enabled, so that tracking can be started */ this->SetState(Ready); this->SetErrorMessage(""); return true; } bool mitk::NDITrackingDevice::InitializeWiredTools() { NDIErrorCode returnvalue; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not obtain a list of port handles that are connected"); return false; // ToDo: Is this a fatal error? } /* if there are port handles that need to be initialized, initialize them. Furthermore instantiate tools for each handle that has no tool yet. */ std::string ph; for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); mitk::NDIPassiveTool* pt = this->GetInternalTool(ph); if ( pt == NULL) // if we don't have a tool, something is wrong. Tools should be discovered first by calling DiscoverWiredTools() continue; if (pt->GetSROMData() == NULL) continue; returnvalue = m_DeviceProtocol->PVWR(&ph, pt->GetSROMData(), pt->GetSROMDataLength()); if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not write SROM file for tool '") + pt->GetToolName() + std::string("' to tracking device")).c_str()); return false; } returnvalue = m_DeviceProtocol->PINIT(&ph); if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not initialize tool '") + pt->GetToolName()).c_str()); return false; } if (pt->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&ph, pt->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not enable port '") + portHandle + std::string("' for tool '")+ pt->GetToolName() + std::string("'")).c_str()); return false; } } } return true; } mitk::TrackingDeviceType mitk::NDITrackingDevice::TestConnection() { if (this->GetState() != Setup) { return mitk::TrackingSystemNotSpecified; } m_SerialCommunication = mitk::SerialCommunication::New(); //m_DeviceProtocol = mitk::NDIProtocol::New(); //m_DeviceProtocol->SetTrackingDevice(this); //m_DeviceProtocol->UseCRCOn(); /* init local com port to standard com settings for a NDI tracking device: 9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake */ if (m_DeviceName.empty()) m_SerialCommunication->SetPortNumber(m_PortNumber); else m_SerialCommunication->SetDeviceName(m_DeviceName); m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600); m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8); m_SerialCommunication->SetParity(mitk::SerialCommunication::None); m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); if (m_SerialCommunication->OpenConnection() == 0) // error { m_SerialCommunication = NULL; return mitk::TrackingSystemNotSpecified; } /* Reset Tracking device by sending a serial break for 500ms */ m_SerialCommunication->SendBreak(400); /* Read answer from tracking device (RESETBE6F) */ static const std::string reset("RESETBE6F\r"); std::string answer = ""; this->Receive(&answer, reset.length()); // read answer (should be RESETBE6F) this->ClearReceiveBuffer(); // flush the receive buffer of all remaining data (carriage return, strings other than reset if (reset.compare(answer) != 0) // check for RESETBE6F { this->SetErrorMessage("Hardware Reset of tracking device did not work"); m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; return mitk::TrackingSystemNotSpecified; } /* Now the tracking device is reset, start initialization */ NDIErrorCode returnvalue; /* initialize the tracking device */ //returnvalue = m_DeviceProtocol->INIT(); //if (returnvalue != NDIOKAY) //{ // this->SetErrorMessage("Could not initialize the tracking device"); // return mitk::TrackingSystemNotSpecified; //} mitk::TrackingDeviceType deviceType; returnvalue = m_DeviceProtocol->VER(deviceType); if ((returnvalue != NDIOKAY) || (deviceType == mitk::TrackingSystemNotSpecified)) { m_SerialCommunication = NULL; return mitk::TrackingSystemNotSpecified; } m_SerialCommunication = NULL; return deviceType; } bool mitk::NDITrackingDevice::CloseConnection() { if (this->GetState() != Setup) { //init before closing to force the field generator from aurora to switch itself off m_DeviceProtocol->INIT(); /* close the serial connection */ m_SerialCommunication->CloseConnection(); /* invalidate all tools */ this->InvalidateAll(); /* return to setup mode */ this->SetState(Setup); this->SetErrorMessage(""); m_SerialCommunication = NULL; } return true; } ITK_THREAD_RETURN_TYPE mitk::NDITrackingDevice::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; } NDITrackingDevice *trackingDevice = (NDITrackingDevice*)pInfo->UserData; if (trackingDevice != NULL) { if (trackingDevice->GetOperationMode() == ToolTracking6D) trackingDevice->TrackTools(); // call TrackTools() from the original object else if (trackingDevice->GetOperationMode() == MarkerTracking3D) trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object else if (trackingDevice->GetOperationMode() == ToolTracking5D) trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object else if (trackingDevice->GetOperationMode() == HybridTracking) { trackingDevice->TrackToolsAndMarkers(); } } trackingDevice->m_ThreadID = 0; // erase thread id, now that this thread will end. return ITK_THREAD_RETURN_VALUE; } bool mitk::NDITrackingDevice::StartTracking() { if (this->GetState() != Ready) return false; 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(); m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method mitk::TimeStamp::GetInstance()->Start(this); return true; } void mitk::NDITrackingDevice::TrackTools() { if (this->GetState() != Tracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->TSTART(); if (returnvalue != NDIOKAY) return; /* 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)) { if (this->m_DataTransferMode == TX) { returnvalue = this->m_DeviceProtocol->TX(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors break; } else { returnvalue = this->m_DeviceProtocol->BX(); if (returnvalue != NDIOKAY) break; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->TSTOP(); if (returnvalue != NDIOKAY) { /* insert error handling/notification here */ ; // how can this thread tell the application, that an error has occurred? } return; // returning from this function (and ThreadStartTracking()) this will end the thread and transfer control back to main thread by releasing trackingFinishedLockHolder } void mitk::NDITrackingDevice::TrackMarkerPositions() { if (m_OperationMode == ToolTracking6D) return; if (this->GetState() != Tracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->DSTART(); // Start Diagnostic Mode if (returnvalue != NDIOKAY) return; 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)) { m_MarkerPointsMutex->Lock(); // lock points data structure returnvalue = this->m_DeviceProtocol->POS3D(&m_MarkerPoints); // update points data structure with new position data from tracking device m_MarkerPointsMutex->Unlock(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors { std::cout << "Error in POS3D: could not read data. Possibly no markers present." << std::endl; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); itksys::SystemTools::Delay(1); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->DSTOP(); if (returnvalue != NDIOKAY) return; // how can this thread tell the application, that an error has occured? this->SetState(Ready); return; // returning from this function (and ThreadStartTracking()) this will end the thread } void mitk::NDITrackingDevice::TrackToolsAndMarkers() { if (m_OperationMode != HybridTracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->TSTART(); // Start Diagnostic Mode if (returnvalue != NDIOKAY) return; 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)) { m_MarkerPointsMutex->Lock(); // lock points data structure returnvalue = this->m_DeviceProtocol->TX(true, &m_MarkerPoints); // update points data structure with new position data from tracking device m_MarkerPointsMutex->Unlock(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors { std::cout << "Error in TX: could not read data. Possibly no markers present." << std::endl; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->TSTOP(); if (returnvalue != NDIOKAY) return; // how can this thread tell the application, that an error has occurred? this->SetState(Ready); return; // returning from this function (and ThreadStartTracking()) this will end the thread } mitk::TrackingTool* mitk::NDITrackingDevice::GetTool(unsigned int toolNumber) const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex if (toolNumber < m_6DTools.size()) return m_6DTools.at(toolNumber); return NULL; } mitk::TrackingTool* mitk::NDITrackingDevice::GetToolByName(std::string name) const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::const_iterator end = m_6DTools.end(); for (Tool6DContainerType::const_iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) if (name.compare((*iterator)->GetToolName()) == 0) return *iterator; return NULL; } mitk::NDIPassiveTool* mitk::NDITrackingDevice::GetInternalTool(std::string portHandle) { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) if (portHandle.compare((*iterator)->GetPortHandle()) == 0) return *iterator; return NULL; } unsigned int mitk::NDITrackingDevice::GetToolCount() const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex return m_6DTools.size(); } bool mitk::NDITrackingDevice::Beep(unsigned char count) { if (this->GetState() != Setup) { return (m_DeviceProtocol->BEEP(count) == NDIOKAY); } else { return false; } } mitk::TrackingTool* mitk::NDITrackingDevice::AddTool( const char* toolName, const char* fileName, TrackingPriority p /*= NDIPassiveTool::Dynamic*/ ) { mitk::NDIPassiveTool::Pointer t = mitk::NDIPassiveTool::New(); if (t->LoadSROMFile(fileName) == false) return NULL; t->SetToolName(toolName); t->SetTrackingPriority(p); if (this->InternalAddTool(t) == false) return NULL; return t.GetPointer(); } bool mitk::NDITrackingDevice::InternalAddTool(mitk::NDIPassiveTool* tool) { if (tool == NULL) return false; NDIPassiveTool::Pointer p = tool; /* if the connection to the tracking device is already established, add the new tool to the device now */ if (this->GetState() == Ready) { /* get a port handle for the tool */ std::string newPortHandle; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PHRQ(&newPortHandle); if (returnvalue == NDIOKAY) { p->SetPortHandle(newPortHandle.c_str()); /* now write the SROM file of the tool to the tracking system using PVWR */ returnvalue = m_DeviceProtocol->PVWR(&newPortHandle, p->GetSROMData(), p->GetSROMDataLength()); if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not write SROM file for tool '") + p->GetToolName() + std::string("' to tracking device")).c_str()); return false; } /* initialize the port handle */ returnvalue = m_DeviceProtocol->PINIT(&newPortHandle); if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not initialize port '") + newPortHandle + std::string("' for tool '")+ p->GetToolName() + std::string("'")).c_str()); return false; } /* enable the port handle */ if (p->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&newPortHandle, p->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not enable port '") + newPortHandle + std::string("' for tool '")+ p->GetToolName() + std::string("'")).c_str()); return false; } } } /* now that the tool is added to the device, add it to list too */ m_ToolsMutex->Lock(); this->m_6DTools.push_back(p); m_ToolsMutex->Unlock(); this->Modified(); return true; } else if (this->GetState() == Setup) { /* In Setup mode, we only add it to the list, so that OpenConnection() can add it later */ m_ToolsMutex->Lock(); this->m_6DTools.push_back(p); m_ToolsMutex->Unlock(); this->Modified(); return true; } else // in Tracking mode, no tools can be added return false; } bool mitk::NDITrackingDevice::RemoveTool(mitk::TrackingTool* tool) { mitk::NDIPassiveTool* ndiTool = dynamic_cast(tool); if (ndiTool == NULL) return false; std::string portHandle = ndiTool->GetPortHandle(); /* a valid portHandle has length 2. If a valid handle exists, the tool is already added to the tracking device, so we have to remove it there if the connection to the tracking device has already been established. */ if ((portHandle.length() == 2) && (this->GetState() == Ready)) // do not remove a tool in tracking mode { NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PHF(&portHandle); if (returnvalue != NDIOKAY) return false; /* Now that the tool is removed from the tracking device, remove it from our tool list too */ MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex (scope is inside the if-block Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) { if (iterator->GetPointer() == ndiTool) { m_6DTools.erase(iterator); this->Modified(); return true; } } return false; } else if (this->GetState() == Setup) // in Setup Mode, we are not connected to the tracking device, so we can just remove the tool from the tool list { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) { if ((*iterator).GetPointer() == ndiTool) { m_6DTools.erase(iterator); this->Modified(); return true; } } return false; } return false; } void mitk::NDITrackingDevice::InvalidateAll() { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) (*iterator)->SetDataValid(false); } bool mitk::NDITrackingDevice::SetOperationMode(OperationMode mode) { if (GetState() == Tracking) return false; m_OperationMode = mode; return true; } mitk::OperationMode mitk::NDITrackingDevice::GetOperationMode() { return m_OperationMode; } bool mitk::NDITrackingDevice::GetMarkerPositions(MarkerPointContainerType* markerpositions) { m_MarkerPointsMutex->Lock(); *markerpositions = m_MarkerPoints; // copy the internal vector to the one provided m_MarkerPointsMutex->Unlock(); return (markerpositions->size() != 0) ; } bool mitk::NDITrackingDevice::DiscoverWiredTools() { /* First, check for disconnected tools and remove them */ this->FreePortHandles(); /* check for new tools, add and initialize them */ NDIErrorCode returnvalue; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not obtain a list of port handles that are connected"); return false; // ToDo: Is this a fatal error? } /* if there are port handles that need to be initialized, initialize them. Furthermore instantiate tools for each handle that has no tool yet. */ std::string ph; /* we need to remember the ports which are occupied to be able to readout the serial numbers of the connected tools later */ std::vector occupiedPorts = std::vector(); int numberOfToolsAtStart = this->GetToolCount(); //also remember the number of tools at start to identify the automatically detected tools later for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); if (this->GetInternalTool(ph) != NULL) // if we already have a tool with this handle continue; // then skip the initialization //instantiate an object for each tool that is connected mitk::NDIPassiveTool::Pointer newTool = mitk::NDIPassiveTool::New(); newTool->SetPortHandle(ph.c_str()); newTool->SetTrackingPriority(mitk::NDIPassiveTool::Dynamic); //set a name for identification newTool->SetToolName((std::string("Port ") + ph).c_str()); returnvalue = m_DeviceProtocol->PINIT(&ph); if (returnvalue != NDIINITIALIZATIONFAILED) //if the initialization failed (AURORA) it can not be enabled. A srom file will have to be specified manually first. Still return true to be able to continue { if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not initialize port '") + ph + std::string("' for tool '")+ newTool->GetToolName() + std::string("'")).c_str()); return false; } /* enable the port handle */ returnvalue = m_DeviceProtocol->PENA(&ph, newTool->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { this->SetErrorMessage((std::string("Could not enable port '") + ph + std::string("' for tool '")+ newTool->GetToolName() + std::string("'")).c_str()); return false; } } //we have to temporarily unlock m_ModeMutex here to avoid a deadlock with another lock inside InternalAddTool() if (this->InternalAddTool(newTool) == false) this->SetErrorMessage("Error while adding new tool"); else occupiedPorts.push_back(i); } // after initialization readout serial numbers of automatically detected tools for (unsigned int i = 0; i < occupiedPorts.size(); i++) { ph = portHandle.substr(occupiedPorts.at(i), 2); std::string portInfo; NDIErrorCode returnvaluePort = m_DeviceProtocol->PHINF(ph, &portInfo); if ((returnvaluePort==NDIOKAY) && (portInfo.size()>31)) dynamic_cast(this->GetTool(i+numberOfToolsAtStart))->SetSerialNumber(portInfo.substr(23,8)); itksys::SystemTools::Delay(10); } return true; } mitk::NDIErrorCode mitk::NDITrackingDevice::FreePortHandles() { /* first search for port handles that need to be freed: e.g. because of a reset of the tracking system */ NDIErrorCode returnvalue = NDIOKAY; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(FREED, &portHandle); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not obtain a list of port handles that need to be freed"); return returnvalue; // ToDo: Is this a fatal error? } /* if there are port handles that need to be freed, free them */ if (portHandle.empty() == true) return returnvalue; std::string ph; for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); mitk::NDIPassiveTool* t = this->GetInternalTool(ph); if (t != NULL) // if we have a tool for the port handle that needs to be freed { if (this->RemoveTool(t) == false) // remove it (this will free the port too) returnvalue = NDIERROR; } else // we don't have a tool, the port handle exists only in the tracking device { returnvalue = m_DeviceProtocol->PHF(&ph); // free it there // What to do if port handle could not be freed? This seems to be a non critical error if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not free all port handles"); // return false; // could not free all Handles } } } return returnvalue; } int mitk::NDITrackingDevice::GetMajorFirmwareRevisionNumber() { std::string revision; if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9) ) { this->SetErrorMessage("Could not receive firmware revision number!"); return 0; } const std::string majrevno = revision.substr(2,3); //cut out "004" from "D.004.001" return std::atoi(majrevno.c_str()); } const char* mitk::NDITrackingDevice::GetFirmwareRevisionNumber() { static std::string revision; if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9) ) { this->SetErrorMessage("Could not receive firmware revision number!"); revision = ""; return revision.c_str(); } return revision.c_str(); } bool mitk::NDITrackingDevice::GetSupportedVolumes(unsigned int* numberOfVolumes, mitk::NDITrackingDevice::NDITrackingVolumeContainerType* volumes, mitk::NDITrackingDevice::TrackingVolumeDimensionType* volumesDimensions) { if (numberOfVolumes == NULL || volumes == NULL || volumesDimensions == NULL) return false; static std::string info; if (m_DeviceProtocol->SFLIST(&info) != mitk::NDIOKAY || info.empty()) { this->SetErrorMessage("Could not receive tracking volume information of tracking system!"); return false; } /*info contains the following: (+n times:) */ (*numberOfVolumes) = (unsigned int) std::atoi(info.substr(0,1).c_str()); for (unsigned int i=0; i<(*numberOfVolumes); i++) { //e.g. for cube: "9-025000+025000-025000+025000-055000-005000+000000+000000+000000+00000011" //for dome: "A+005000+048000+005000+066000+000000+000000+000000+000000+000000+00000011" std::string::size_type offset, end; offset = (i*73)+1; end = 73+(i*73); std::string currentVolume = info.substr(offset, end);//i=0: from 1 to 73 characters; i=1: from 75 to 148 char; // if i>0 then we have a return statement infront if (i>0) currentVolume = currentVolume.substr(1, currentVolume.size()); std::string standard = "0"; std::string pyramid = "4"; std::string spectraPyramid = "5"; std::string vicraVolume = "7"; std::string cube = "9"; std::string dome = "A"; if (currentVolume.compare(0,1,standard)==0) volumes->push_back(mitk::Standard); if (currentVolume.compare(0,1,pyramid)==0) volumes->push_back(mitk::Pyramid); if (currentVolume.compare(0,1,spectraPyramid)==0) volumes->push_back(mitk::SpectraPyramid); if (currentVolume.compare(0,1,vicraVolume)==0) volumes->push_back(mitk::VicraVolume); else if (currentVolume.compare(0,1,cube)==0) volumes->push_back(mitk::Cube);//alias cube else if (currentVolume.compare(0,1,dome)==0) volumes->push_back(mitk::Dome); //fill volumesDimensions for (unsigned int index = 0; index < 10; index++) { std::string::size_type offD, endD; offD = 1+(index*7); //7 digits per dimension and the first is the type of volume endD = offD+7; int dimension = std::atoi(currentVolume.substr(offD, endD).c_str()); dimension /= 100; //given in mm. 7 digits are xxxx.xx according to NDI //strange, the last two digits (11) also for the metal flag get read also... volumesDimensions->push_back(dimension); } } return true; } bool mitk::NDITrackingDevice::SetVolume(NDITrackingVolume volume) { if (m_DeviceProtocol->VSEL(volume) != mitk::NDIOKAY) { this->SetErrorMessage("Could not set volume!"); return false; } return true; } diff --git a/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.h b/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.h index 0637d7cca4..ce6a6464b5 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.h +++ b/Modules/IGT/IGTTrackingDevices/mitkNDITrackingDevice.h @@ -1,306 +1,306 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date$ Version: $Revision$ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #ifndef MITKNDITRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 #define MITKNDITRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 #include "mitkTrackingDevice.h" #include #include #include "itkFastMutexLock.h" #include #include "mitkTrackingTypes.h" #include "mitkNDIProtocol.h" #include "mitkNDIPassiveTool.h" #include "mitkSerialCommunication.h" namespace mitk { class NDIProtocol; /** Documentation * \brief superclass for specific NDI tracking Devices that use serial communication. * * implements the TrackingDevice interface for NDI tracking devices (POLARIS, AURORA) * * \ingroup IGT */ class MitkIGT_EXPORT NDITrackingDevice : public TrackingDevice { friend class NDIProtocol; public: typedef std::vector Tool6DContainerType; ///< List of 6D tools of the correct type for this tracking device typedef std::vector TrackingVolumeDimensionType; ///< List of the supported tracking volume dimensions. typedef mitk::TrackingDeviceType NDITrackingDeviceType; ///< This enumeration includes the two types of NDI tracking devices (Polaris, Aurora). typedef std::vector NDITrackingVolumeContainerType; ///< vector of tracking volumes typedef mitk::SerialCommunication::PortNumber PortNumber; ///< Port number of the serial connection typedef mitk::SerialCommunication::BaudRate BaudRate; ///< Baud rate of the serial connection typedef mitk::SerialCommunication::DataBits DataBits; ///< Number of data bits used in the serial connection typedef mitk::SerialCommunication::Parity Parity; ///< Parity mode used in the serial connection typedef mitk::SerialCommunication::StopBits StopBits; ///< Number of stop bits used in the serial connection typedef mitk::SerialCommunication::HardwareHandshake HardwareHandshake; ///< Hardware handshake mode of the serial connection typedef mitk::NDIPassiveTool::TrackingPriority TrackingPriority; ///< Tracking priority used for tracking a tool mitkClassMacro(NDITrackingDevice, TrackingDevice); itkNewMacro(Self); /** * \brief Set the type of the NDI Tracking Device because it can not jet handle this itself */ - itkSetMacro(Type, TrackingDeviceType); + //itkSetMacro(Type, TrackingDeviceType); /** * \brief initialize the connection to the tracking device * * OpenConnection() establishes the connection to the tracking device by: * - initializing the serial port with the given parameters (port number, baud rate, ...) * - connection to the tracking device * - initializing the device * - initializing all manually added passive tools (user supplied srom file) * - initializing active tools that are connected to the tracking device */ virtual bool OpenConnection(); /** * \brief Closes the connection * * CloseConnection() resets the tracking device, invalidates all tools and then closes the serial port. */ virtual bool CloseConnection(); bool InitializeWiredTools(); /** * \brief TestConnection() tries to connect to a NDI tracking device on the current port/device and returns which device it has found * * TestConnection() tries to connect to a NDI tracking device on the current port/device. * \return It returns the type of the device that * answers at the port/device or mitk::TrackingSystemNotSpecified if no NDI tracking device is available at that port */ virtual mitk::TrackingDeviceType TestConnection(); /** * \brief retrieves all wired tools from the tracking device * * This method queries the tracking device for all wired tools, initializes them and creates TrackingTool representation objects * for them * \return true if no error occured, false if an error occured. Check GetErrorMessage() in case of error. */ bool DiscoverWiredTools(); /** * \brief Start the tracking. * * A new thread is created, which continuously reads the position and orientation information of each tool and stores them inside the tools. * Depending on the current operation mode (see SetOperationMode()), either the 6D tools (ToolTracking6D), 5D tools (ToolTracking5D), * 3D marker positions (MarkerTracking3D) or both 6D tools and 3D markers (HybridTracking) are updated. * Call StopTracking() to stop the tracking thread. */ virtual bool StartTracking(); /** * \brief return the tool with index toolNumber */ virtual TrackingTool* GetTool(unsigned int toolNumber) const; virtual mitk::TrackingTool* GetToolByName(std::string name) const; /** * \brief return current number of tools */ virtual unsigned int GetToolCount() const; /** * \brief Create a passive 6D tool with toolName and fileName and add it to the list of tools * * This method will create a new NDIPassiveTool object, load the SROM file fileName, * set the tool name toolName and the tracking priority p and then add * it to the list of tools. It returns a pointer of type mitk::TrackingTool to the tool * that can be used to read tracking data from it. * This is the only way to add tools to NDITrackingDevice. * * \warning adding tools is not possible in tracking mode, only in setup and ready. */ mitk::TrackingTool* AddTool(const char* toolName, const char* fileName, TrackingPriority p = NDIPassiveTool::Dynamic); /** * \brief Remove a passive 6D tool from the list of tracked tools. * * \warning removing tools is not possible in tracking mode, only in setup and ready modes. */ virtual bool RemoveTool(TrackingTool* tool); /** * \brief reloads the srom file and reinitializes the tool */ virtual bool UpdateTool(mitk::TrackingTool* tool); virtual void SetPortNumber(const PortNumber _arg); ///< set port number for serial communication itkGetConstMacro(PortNumber, PortNumber); ///< returns the port number for serial communication virtual void SetDeviceName(std::string _arg); ///< set device name (e.g. COM1, /dev/ttyUSB0). If this is set, PortNumber will be ignored itkGetStringMacro(DeviceName); ///< returns the device name for serial communication virtual void SetBaudRate(const BaudRate _arg); ///< set baud rate for serial communication itkGetConstMacro(BaudRate, BaudRate); ///< returns the baud rate for serial communication virtual void SetDataBits(const DataBits _arg); ///< set number of data bits itkGetConstMacro(DataBits, DataBits); ///< returns the data bits for serial communication virtual void SetParity(const Parity _arg); ///< set parity mode itkGetConstMacro(Parity, Parity); ///< returns the parity mode virtual void SetStopBits(const StopBits _arg); ///< set number of stop bits itkGetConstMacro(StopBits, StopBits); ///< returns the number of stop bits virtual void SetHardwareHandshake(const HardwareHandshake _arg); ///< set use hardware handshake for serial communication itkGetConstMacro(HardwareHandshake, HardwareHandshake); ///< returns the hardware handshake setting virtual void SetIlluminationActivationRate(const IlluminationActivationRate _arg); ///< set activation rate of IR illumator for polaris itkGetConstMacro(IlluminationActivationRate, IlluminationActivationRate); ///< returns the activation rate of IR illumator for polaris virtual void SetDataTransferMode(const DataTransferMode _arg); ///< set data transfer mode to text (TX) or binary (BX). \warning: only TX is supportet at the moment itkGetConstMacro(DataTransferMode, DataTransferMode); ///< returns the data transfer mode virtual bool Beep(unsigned char count); ///< Beep the tracking device 1 to 9 times NDIErrorCode GetErrorCode(const std::string* input); ///< returns the error code for a string that contains an error code in hexadecimal format virtual bool SetOperationMode(OperationMode mode); ///< set operation mode to 6D tool tracking, 3D marker tracking or 6D&3D hybrid tracking (see OperationMode) virtual OperationMode GetOperationMode(); ///< get current operation mode /** * \brief Get 3D marker positions (operation mode must be set to MarkerTracking3D or HybridTracking) */ virtual bool GetMarkerPositions(MarkerPointContainerType* markerpositions); /** * \brief Get major revision number from tracking device * should not be called directly after starting to track **/ virtual int GetMajorFirmwareRevisionNumber(); /** * \brief Get revision number from tracking device as string * should not be called directly after starting to track **/ virtual const char* GetFirmwareRevisionNumber(); /** * \brief Get number of supported tracking volumes, a vector containing the supported volumes and * a vector containing the signed dimensions in mm. For each volume 10 boundaries are stored in the order of * the supported volumes (see AURORA API GUIDE: SFLIST p.54). **/ virtual bool GetSupportedVolumes(unsigned int* numberOfVolumes, NDITrackingVolumeContainerType* volumes, TrackingVolumeDimensionType* volumesDimensions); /** * \brief Sets the desired tracking volume. Returns true if the volume type could be set. Usage: ndiTracker->SetVolume(mitk::Dome); **/ virtual bool SetVolume(NDITrackingVolume volume); protected: /** * \brief Add a passive 6D tool to the list of tracked tools. This method is used by AddTool * * \warning adding tools is not possible in tracking mode, only in setup and ready. */ virtual bool InternalAddTool(NDIPassiveTool* tool); /* Methods for NDIProtocol friend class */ virtual void InvalidateAll(); ///< invalidate all tools NDIPassiveTool* GetInternalTool(std::string portHandle); ///< returns the tool object that has been assigned the port handle or NULL if no tool can be found /** * \brief free all port handles that need to be freed * * This method retrieves a list of all port handles that need to be freed (e.g. tool got disconnected) * and frees the handles at the tracking device and it removes the tools from the internal tool list * \warning This method can remove TrackingTools from the tool list! After calling this method, GetTool(i) could return * a different tool, because tool indices could have changed. * \return returns NDIOKAY if everything was sucessfull, returns an error code otherwise */ NDIErrorCode FreePortHandles(); NDIErrorCode Send(const std::string* message, bool addCRC = true); ///< Send message to tracking device NDIErrorCode Receive(std::string* answer, unsigned int numberOfBytes); ///< receive numberOfBytes bytes from tracking device NDIErrorCode ReceiveByte(char* answer); ///< lightweight receive function, that reads just one byte NDIErrorCode ReceiveLine(std::string* answer); ///< receive characters until the first LF (The LF is included in the answer string) void ClearSendBuffer(); ///< empty send buffer of serial communication interface void ClearReceiveBuffer(); ///< empty receive buffer of serial communication interface const std::string CalcCRC(const std::string* input); ///< returns the CRC16 for input as a std::string public://TODO /** * \brief TrackTools() continuously polls serial interface for new 6d tool positions until StopTracking is called. * * Continuously tracks the 6D position of all tools until StopTracking() is called. * This function is executed by the tracking thread (through StartTracking() and ThreadStartTracking()). * It should not be called directly. */ virtual void TrackTools(); /** * \brief continuously polls serial interface for new 3D marker positions until StopTracking is called. * * Continuously tracks the 3D position of all markers until StopTracking() is called. * This function is executed by the tracking thread (through StartTracking() and ThreadStartTracking()). * It should not be called directly. */ virtual void TrackMarkerPositions(); /** * \brief continuously polls serial interface for new 3D marker positions and 6D tool positions until StopTracking is called. * * Continuously tracks the 3D position of all markers and the 6D position of all tools until StopTracking() is called. * This function is executed by the tracking thread (through StartTracking() and ThreadStartTracking()). * It should not be called directly. */ virtual void TrackToolsAndMarkers(); /** * \brief static start method for the tracking thread. */ static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void* data); protected: NDITrackingDevice(); ///< Constructor virtual ~NDITrackingDevice(); ///< Destructor std::string m_DeviceName;///< Device Name PortNumber m_PortNumber; ///< COM Port Number BaudRate m_BaudRate; ///< COM Port Baud Rate DataBits m_DataBits; ///< Number of Data Bits per token Parity m_Parity; ///< Parity mode for communication StopBits m_StopBits; ///< number of stop bits per token HardwareHandshake m_HardwareHandshake; ///< use hardware handshake for serial port connection NDITrackingVolume m_NDITrackingVolume; ///< which tracking volume is currently used (if device supports multiple volumes) (\warning This parameter is not used yet) IlluminationActivationRate m_IlluminationActivationRate; ///< update rate of IR illuminator for Polaris DataTransferMode m_DataTransferMode; ///< use TX (text) or BX (binary) (\warning currently, only TX mode is supported) Tool6DContainerType m_6DTools; ///< list of 6D tools itk::FastMutexLock::Pointer m_ToolsMutex; ///< mutex for coordinated access of tool container mitk::SerialCommunication::Pointer m_SerialCommunication; ///< serial communication interface itk::FastMutexLock::Pointer m_SerialCommunicationMutex; ///< mutex for coordinated access of serial communication interface NDIProtocol::Pointer m_DeviceProtocol; ///< create and parse NDI protocol strings itk::MultiThreader::Pointer m_MultiThreader; ///< creates tracking thread that continuously polls serial interface for new tracking data int m_ThreadID; ///< ID of tracking thread OperationMode m_OperationMode; ///< tracking mode (6D tool tracking, 3D marker tracking,...) itk::FastMutexLock::Pointer m_MarkerPointsMutex; ///< mutex for marker point data container MarkerPointContainerType m_MarkerPoints; ///< container for markers (3D point tracking mode) }; } // namespace mitk #endif /* MITKNDITRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 */ diff --git a/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.cpp b/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.cpp index 8955642534..9b343f4722 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.cpp +++ b/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.cpp @@ -1,101 +1,100 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date$ Version: $Revision$ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #include "mitkTrackingDevice.h" #include "mitkTimeStamp.h" #include "mitkTrackingTool.h" #include typedef itk::MutexLockHolder MutexLockHolder; -mitk::TrackingDevice::TrackingDevice() : - m_Type(TrackingSystemNotSpecified), +mitk::TrackingDevice::TrackingDevice() : + m_Data(mitk::Unspecified), m_State(mitk::TrackingDevice::Setup), m_StopTracking(false), m_ErrorMessage("") { m_StopTrackingMutex = itk::FastMutexLock::New(); m_StateMutex = itk::FastMutexLock::New(); m_TrackingFinishedMutex = itk::FastMutexLock::New(); m_TrackingFinishedMutex->Lock(); // execution rights are owned by the application thread at the beginning } mitk::TrackingDevice::~TrackingDevice() { } mitk::TrackingDevice::TrackingDeviceState mitk::TrackingDevice::GetState() const { MutexLockHolder lock(*m_StateMutex); return m_State; } void mitk::TrackingDevice::SetState( TrackingDeviceState state ) { itkDebugMacro("setting m_State to " << state); MutexLockHolder lock(*m_StateMutex); // lock and unlock the mutex if (m_State == state) { return; } m_State = state; this->Modified(); } mitk::TrackingDeviceType mitk::TrackingDevice::GetType() const{ return m_Data.Line; } void mitk::TrackingDevice::SetType(mitk::TrackingDeviceType deviceType){ m_Data = mitk::GetFirstCompatibleDeviceDataForLine(deviceType); - m_Type = m_Data.Line; } bool mitk::TrackingDevice::StopTracking() { if (this->GetState() == Tracking) // Only if the object is in the correct state { m_StopTrackingMutex->Lock(); // m_StopTracking is used by two threads, so we have to ensure correct thread handling m_StopTracking = true; m_StopTrackingMutex->Unlock(); //we have to wait here that the other thread recognizes the STOP-command and executes it m_TrackingFinishedMutex->Lock(); mitk::TimeStamp::GetInstance()->Stop(this); // notify realtime clock // StopTracking was called, thus the mode should be changed back // to Ready now that the tracking loop has ended. this->SetState(Ready); } return true; } mitk::TrackingTool* mitk::TrackingDevice::GetToolByName( std::string name ) const { unsigned int toolCount = this->GetToolCount(); for (unsigned int i = 0; i < toolCount; ++i) if (name == this->GetTool(i)->GetToolName()) return this->GetTool(i); return NULL; } diff --git a/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.h b/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.h index cd454839fa..615e6ad58a 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.h +++ b/Modules/IGT/IGTTrackingDevices/mitkTrackingDevice.h @@ -1,142 +1,141 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date$ Version: $Revision$ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #ifndef MITKTRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 #define MITKTRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 #include #include "itkObject.h" #include "mitkCommon.h" #include "mitkTrackingTypes.h" #include "itkFastMutexLock.h" namespace mitk { class TrackingTool; // interface for a tool that can be tracked by the TrackingDevice /**Documentation * \brief Interface for all Tracking Devices * * Defines the methods that are common for all tracking devices. * * \ingroup IGT */ class MitkIGT_EXPORT TrackingDevice : public itk::Object { public: mitkClassMacro(TrackingDevice, itk::Object); enum TrackingDeviceState {Setup, Ready, Tracking}; ///< Type for state variable. The trackingdevice is always in one of these states /** * \brief Opens a connection to the device * * This may only be called if there is currently no connection to the device. * If OpenConnection() is successful, the object will change from Setup state to Ready state */ virtual bool OpenConnection() = 0; /** * \brief Closes the connection to the device * * This may only be called if there is currently a connection to the device, but tracking is * not running (e.g. object is in Ready state) */ virtual bool CloseConnection() = 0; ///< Closes the connection with the device /** * \brief start retrieving tracking data from the device. * * This may only be called after the connection to the device has been established * with a call to OpenConnection() (E.g. object is in Ready mode). This will change the * object state from Ready to Tracking */ virtual bool StartTracking() = 0; /** * \brief stop retrieving tracking data from the device. * stop retrieving tracking data from the device. * This may only be called after StartTracking was called * (e.g. the object is in Tracking mode). * This will change the object state from Tracking to Ready. */ virtual bool StopTracking(); /** * \brief Return tool with index toolNumber * * tools are numbered from 0 to GetToolCount() - 1. */ virtual TrackingTool* GetTool(unsigned int toolNumber) const = 0; /** * \brief Returns the tool with the given tool name * * Note: subclasses can and should implement optimized versions of this method * \return the given tool or NULL if no tool with that name exists */ virtual mitk::TrackingTool* GetToolByName(std::string name) const; /** * \brief Returns number of tracking tools */ virtual unsigned int GetToolCount() const = 0; /** * \brief return current error message */ itkGetStringMacro(ErrorMessage); /** * \brief return current object state (Setup, Ready or Tracking) */ TrackingDeviceState GetState() const; /** * \brief return device type identifier */ TrackingDeviceType GetType() const; /** * \brief set device type */ void SetType(TrackingDeviceType type); protected: /** * \brief set error message */ itkSetStringMacro(ErrorMessage); /** * \brief change object state */ void SetState(TrackingDeviceState state); TrackingDevice(); virtual ~TrackingDevice(); - TrackingDeviceType m_Type; ///< current device type TrackingDeviceData m_Data; ///< current device Data TrackingDeviceState m_State; ///< current object state (Setup, Ready or Tracking) bool m_StopTracking; ///< signal stop to tracking thread itk::FastMutexLock::Pointer m_StopTrackingMutex; ///< mutex to control access to m_StopTracking itk::FastMutexLock::Pointer m_TrackingFinishedMutex; ///< mutex to manage control flow of StopTracking() itk::FastMutexLock::Pointer m_StateMutex; ///< mutex to control access to m_State std::string m_ErrorMessage; ///< current error message }; } // namespace mitk #endif /* MITKTRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 */ diff --git a/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp b/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp index 058ea627a9..54760d28e4 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp +++ b/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp @@ -1,319 +1,319 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date: $ Version: $Revision: $ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #include "mitkVirtualTrackingDevice.h" #include #include #include #include #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::VirtualTrackingDevice::VirtualTrackingDevice() : mitk::TrackingDevice(), m_AllTools(), m_ToolsMutex(NULL), m_MultiThreader(NULL), m_ThreadID(-1), m_RefreshRate(100), m_NumberOfControlPoints(20) { - m_Type = VirtualTracker; + m_Data = mitk::VTracker; m_Bounds[0] = m_Bounds[2] = m_Bounds[4] = -400.0; // initialize bounds to -400 ... +400 (mm) cube m_Bounds[1] = m_Bounds[3] = m_Bounds[5] = 400.0; m_ToolsMutex = itk::FastMutexLock::New(); } mitk::VirtualTrackingDevice::~VirtualTrackingDevice() { if (this->GetState() == Tracking) { this->StopTracking(); } if (this->GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1)) { m_MultiThreader->TerminateThread(m_ThreadID); m_MultiThreader = NULL; } m_AllTools.clear(); } mitk::TrackingTool* mitk::VirtualTrackingDevice::AddTool(const char* toolName) { //if (this->GetState() == Tracking) //{ // return NULL; //} mitk::VirtualTrackingTool::Pointer t = mitk::VirtualTrackingTool::New(); t->SetToolName(toolName); t->SetVelocity(0.1); this->InitializeSpline(t); MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex m_AllTools.push_back(t); return t; } bool mitk::VirtualTrackingDevice::StartTracking() { if (this->GetState() != Ready) return false; this->SetState(Tracking); // go to mode Tracking this->m_StopTrackingMutex->Lock(); this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread mitk::TimeStamp::GetInstance()->Start(this); if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1)) m_MultiThreader->TerminateThread(m_ThreadID); if (m_MultiThreader.IsNull()) m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } bool mitk::VirtualTrackingDevice::StopTracking() { if (this->GetState() == Tracking) // Only if the object is in the correct state { m_StopTrackingMutex->Lock(); // m_StopTracking is used by two threads, so we have to ensure correct thread handling m_StopTracking = true; m_StopTrackingMutex->Unlock(); this->SetState(Ready); } mitk::TimeStamp::GetInstance()->Stop(this); m_TrackingFinishedMutex->Lock(); return true; } unsigned int mitk::VirtualTrackingDevice::GetToolCount() const { MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex return static_cast(this->m_AllTools.size()); } mitk::TrackingTool* mitk::VirtualTrackingDevice::GetTool(unsigned int toolNumber) const { MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex if ( toolNumber < m_AllTools.size()) return this->m_AllTools.at(toolNumber); return NULL; } bool mitk::VirtualTrackingDevice::OpenConnection() { if (m_NumberOfControlPoints < 1) { this->SetErrorMessage("to few control points for spline interpolation"); return false; } srand(time(NULL)); //Init random number generator this->SetState(Ready); return true; } void mitk::VirtualTrackingDevice::InitializeSpline( mitk::VirtualTrackingTool* t ) { if (t == NULL) return; typedef mitk::VirtualTrackingTool::SplineType SplineType; /* create random control points */ SplineType::ControlPointListType controlPoints; controlPoints.reserve(m_NumberOfControlPoints + 1); controlPoints.push_back(this->GetRandomPoint()); // insert point 0 double length = 0.0; // estimate spline length by calculating line segments lengths for (unsigned int i = 1; i < m_NumberOfControlPoints - 1; ++i) // set points 1..n-2 { SplineType::ControlPointType pos; pos = this->GetRandomPoint(); length += controlPoints.at(i - 1).EuclideanDistanceTo(pos); controlPoints.push_back(pos); } controlPoints.push_back(controlPoints.at(0)); // close spline --> insert point last control point with same value as first control point length += controlPoints.at(controlPoints.size() - 2).EuclideanDistanceTo(controlPoints.at(controlPoints.size() - 1)); /* Create knot list. TODO: rethink knot list values and list size. Is there a better solution? */ SplineType::KnotListType knotList; knotList.push_back(0.0); for (unsigned int i = 1; i < controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1; ++i) knotList.push_back(i); knotList.push_back(controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1); t->GetSpline()->SetControlPoints(controlPoints); t->GetSpline()->SetKnots(knotList); t->SetSplineLength(length); } bool mitk::VirtualTrackingDevice::CloseConnection() { bool returnValue = true; if(this->GetState() == Setup) return true; this->SetState(Setup); return returnValue; } mitk::ScalarType mitk::VirtualTrackingDevice::GetSplineChordLength(unsigned int idx) { mitk::VirtualTrackingTool* t = this->GetInternalTool(idx); if (t != NULL) return t->GetSplineLength(); else throw std::invalid_argument("invalid index"); } void mitk::VirtualTrackingDevice::SetToolSpeed(unsigned int idx, mitk::ScalarType roundsPerSecond) { if (roundsPerSecond < 0.0001) throw std::invalid_argument("Minimum tool speed is 0.0001 rounds per second"); mitk::VirtualTrackingTool* t = this->GetInternalTool(idx); if (t != NULL) t->SetVelocity(roundsPerSecond); else throw std::invalid_argument("invalid index"); } mitk::VirtualTrackingTool* mitk::VirtualTrackingDevice::GetInternalTool(unsigned int idx) { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex if (idx < m_AllTools.size()) return m_AllTools.at(idx); else return NULL; } void mitk::VirtualTrackingDevice::TrackTools() { try { 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; /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ if (!localStopTracking) { m_TrackingFinishedMutex->Lock(); } this->m_StopTrackingMutex->Unlock(); mitk::ScalarType t = 0.0; while ((this->GetState() == Tracking) && (localStopTracking == false)) { //for (ToolContainer::iterator itAllTools = m_AllTools.begin(); itAllTools != m_AllTools.end(); itAllTools++) for (unsigned int i = 0; i < this->GetToolCount(); ++i) // use mutexed methods to access tool container { mitk::VirtualTrackingTool::Pointer currentTool = this->GetInternalTool(i); mitk::VirtualTrackingTool::SplineType::PointType pos; /* calculate tool position with spline interpolation */ pos = currentTool->GetSpline()->EvaluateSpline(t); mitk::Point3D mp; mitk::itk2vtk(pos, mp); // convert from SplineType::PointType to mitk::Point3D currentTool->SetPosition(mp); // Currently, a constant speed is used. TODO: use tool velocity setting t += 0.001; if (t >= 1.0) t = 0.0; mitk::Quaternion quat; /* fix quaternion rotation */ quat.x() = 1.0; quat.y() = 1.0; quat.z() = 1.0; quat.r() = 1.0; currentTool->SetOrientation(quat); // TODO: rotate once per cycle around a fixed rotation vector currentTool->SetTrackingError( 2 * (rand() / (RAND_MAX + 1.0))); // tracking error in 0 .. 2 Range currentTool->SetDataValid(true); currentTool->Modified(); } itksys::SystemTools::Delay(m_RefreshRate); /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } // tracking ends if we pass this line m_TrackingFinishedMutex->Unlock(); // transfer control back to main thread } catch(...) { m_TrackingFinishedMutex->Unlock(); this->StopTracking(); this->SetErrorMessage("Error while trying to track tools. Thread stopped."); } } ITK_THREAD_RETURN_TYPE mitk::VirtualTrackingDevice::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; } VirtualTrackingDevice *trackingDevice = static_cast(pInfo->UserData); if (trackingDevice != NULL) trackingDevice->TrackTools(); trackingDevice->m_ThreadID = -1; // reset thread ID because we end the thread here return ITK_THREAD_RETURN_VALUE; } mitk::VirtualTrackingDevice::ControlPointType mitk::VirtualTrackingDevice::GetRandomPoint() { ControlPointType pos; pos[0] = m_Bounds[0] + (m_Bounds[1] - m_Bounds[0]) * (rand() / (RAND_MAX + 1.0)); // X = xMin + xRange * (random number between 0 and 1) pos[1] = m_Bounds[2] + (m_Bounds[3] - m_Bounds[2]) * (rand() / (RAND_MAX + 1.0)); // Y pos[2] = m_Bounds[4] + (m_Bounds[5] - m_Bounds[4]) * (rand() / (RAND_MAX + 1.0)); // Z return pos; }