diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp index 08667fc2ea..170710f9e3 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp @@ -1,202 +1,249 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkPolhemusTrackingDevice.h" #include "mitkPolhemusTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include #include #include "mitkPolhemusTrackerTypeInformation.h" +#include typedef itk::MutexLockHolder MutexLockHolder; mitk::PolhemusTrackingDevice::PolhemusTrackingDevice(): mitk::TrackingDevice() { //set the type of this tracking device this->m_Data = mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty(); this->m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = 0; m_Device = mitk::PolhemusInterface::New(); } mitk::PolhemusTrackingDevice::~PolhemusTrackingDevice() { } bool mitk::PolhemusTrackingDevice::IsDeviceInstalled() { return true; } mitk::TrackingTool* mitk::PolhemusTrackingDevice::AddTool( const char* toolName) { mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New(); t->SetToolName(toolName); if (this->InternalAddTool(t) == false) return nullptr; return t.GetPointer(); } bool mitk::PolhemusTrackingDevice::InternalAddTool(PolhemusTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::PolhemusTrackingDevice::StartTracking() { bool success = m_Device->StartTracking(); if (success) { mitk::IGTTimeStamp::GetInstance()->Start(this); this->SetState(Tracking); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } else { this->SetState(Ready); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!"; } return success; } bool mitk::PolhemusTrackingDevice::StopTracking() { return Superclass::StopTracking(); } unsigned int mitk::PolhemusTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::PolhemusTrackingDevice::GetTool(unsigned int toolNumber) const { if ( toolNumber >= this->GetToolCount()) return nullptr; else return this->m_AllTools[toolNumber]; } bool mitk::PolhemusTrackingDevice::OpenConnection() { //reset everything if (m_Device.IsNull()) {m_Device = mitk::PolhemusInterface::New();} m_Device->Connect(); this->SetState(Ready); return true; } bool mitk::PolhemusTrackingDevice::CloseConnection() { bool returnValue = true; if (this->GetState() == Setup) return true; returnValue = m_Device->Disconnect(); this->SetState(Setup); return returnValue; } mitk::PolhemusInterface* mitk::PolhemusTrackingDevice::GetDevice() { return m_Device; } std::vector mitk::PolhemusTrackingDevice::GetAllTools() { return this->m_AllTools; } void mitk::PolhemusTrackingDevice::TrackTools() { try { /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); Sleep(100);//Wait a bit until the tracker is ready... while ((this->GetState() == Tracking) && (localStopTracking == false)) { std::vector lastData = this->GetDevice()->GetLastFrame(); if (lastData.size() != m_AllTools.size()) { MITK_WARN << "Tool count is corrupt. Aborting!"; } else { std::vector allTools = this->GetAllTools(); for (int i = 0; i < allTools.size(); i++) { mitk::PolhemusTool::Pointer currentTool = allTools.at(i); currentTool->SetDataValid(true); currentTool->SetPosition(lastData.at(i).pos); currentTool->SetOrientation(lastData.at(i).rot); currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed()); } } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } } catch(...) { this->StopTracking(); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to track tools. Thread stopped."; } } ITK_THREAD_RETURN_TYPE mitk::PolhemusTrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } PolhemusTrackingDevice *trackingDevice = (PolhemusTrackingDevice*)pInfo->UserData; if (trackingDevice != nullptr) trackingDevice->TrackTools(); return ITK_THREAD_RETURN_VALUE; } + +bool mitk::PolhemusTrackingDevice::AutoDetectToolsAvailable() +{ + return true; +} + +mitk::NavigationToolStorage::Pointer mitk::PolhemusTrackingDevice::AutoDetectTools() +{ + this->OpenConnection(); + std::vector singeFrameData = this->m_Device->GetSingleFrame(); + MITK_INFO << "Found " << singeFrameData.size() << " tools."; + this->CloseConnection(); + mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); + for each (mitk::PolhemusInterface::trackingData t in singeFrameData) + { + + mitk::DataNode::Pointer newNode = mitk::DataNode::New(); + std::stringstream name; + name << "Sensor-" << ((int)t.id); + newNode->SetName(name.str()); + + mitk::Surface::Pointer myCone = mitk::Surface::New(); + vtkConeSource *vtkData = vtkConeSource::New(); + vtkData->SetAngle(5.0); + vtkData->SetResolution(50); + vtkData->SetHeight(6.0f); + vtkData->SetRadius(2.0f); + vtkData->SetCenter(0.0, 0.0, 0.0); + vtkData->Update(); + myCone->SetVtkPolyData(vtkData->GetOutput()); + vtkData->Delete(); + newNode->SetData(myCone); + + mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); + newTool->SetDataNode(newNode); + + std::stringstream identifier; + identifier << "AutoDetectedTool-" << ((int)t.id); + newTool->SetIdentifier(identifier.str()); + + newTool->SetTrackingDeviceType(mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty().Line); + returnValue->AddTool(newTool); + } + return returnValue; + +} diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h index 01165ce83a..3fec74c3c8 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h @@ -1,132 +1,141 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #ifndef MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ #define MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ #include #include #include #include #include namespace mitk { /** Documentation: * \brief An object of this class represents Polhemus tracking device. You can add tools to this * device, then open the connection and start tracking. The tracking device will then * continuously update the tool coordinates. * \ingroup IGT */ class MITKIGT_EXPORT PolhemusTrackingDevice : public TrackingDevice { public: mitkClassMacro(PolhemusTrackingDevice, TrackingDevice); itkFactorylessNewMacro(Self) itkCloneMacro(Self) /** * \brief Starts the tracking. * \return Returns true if the tracking is started. Throws an exception if an error occures. * @throw mitk::IGTHardwareException Throws an exception if there is an error during start tracking. */ virtual bool StartTracking() override; /** * \brief Stops the tracking. * \return Returns true if the tracking is stopped. */ virtual bool StopTracking() override; /** * \brief Opens the connection to the device. This have to be done before the tracking is started. * @throw mitk::IGTHardwareException Throws an exception if there is an error during open connection. */ virtual bool OpenConnection() override; /** * \brief Closes the connection and clears all resources. */ virtual bool CloseConnection() override; /** * \return Returns the number of tools which have been added to the device. */ virtual unsigned int GetToolCount() const override; /** * \param toolNumber The number of the tool which should be given back. * \return Returns the tool which the number "toolNumber". Returns NULL, if there is * no tool with this number. */ TrackingTool* GetTool(unsigned int toolNumber) const override; /** * \brief Create a new Polhemus tool with toolName and add it to the list of tools * * This method will create a new PolhemusTool object, * set the tool name toolName 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 PolhemusTrackingDevice. * * \warning adding tools is not possible in tracking mode, only in setup and ready. */ mitk::TrackingTool* AddTool(const char* toolName); bool IsDeviceInstalled(); + /** @return Returns true if this device can autodetects its tools. */ + virtual bool AutoDetectToolsAvailable(); + + /** Autodetects tools from this device and returns them as a navigation tool storage. + * @return Returns the detected tools. Returns an empty storage if no tools are present + * or if detection is not possible + */ + virtual mitk::NavigationToolStorage::Pointer AutoDetectTools(); + protected: PolhemusTrackingDevice(); ~PolhemusTrackingDevice(); /** * \brief Adds a tool to the tracking device. * * \param tool The tool which will be added. * \return Returns true if the tool has been added, false otherwise. */ bool InternalAddTool(PolhemusTool::Pointer tool); /** * \brief This method tracks tools as long as the variable m_Mode is set to "Tracking". * Tracking tools means grabbing frames from the camera an updating the tools. * @throw mitk::IGTHardwareException Throws an exception if there is an error during tracking of tools. */ void TrackTools(); /** * \return Returns all tools of the tracking device. */ std::vector GetAllTools(); /** * \return Gives back the device which is represented by an object of the class PolhemusInterface. */ PolhemusInterface* GetDevice(); static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void* data); std::vector m_AllTools; ///< vector holding all tools PolhemusInterface::Pointer m_Device; ///< represents the interface to the tracking hardware itk::MultiThreader::Pointer m_MultiThreader; int m_ThreadID; }; }//mitk #endif /* MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ */