diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTool.h b/Modules/IGT/TrackingDevices/mitkPolhemusTool.h index b16589a3b5..9fa679e67c 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTool.h +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTool.h @@ -1,84 +1,83 @@ /*============================================================================ The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center (DKFZ) All rights reserved. Use of this source code is governed by a 3-clause BSD license that can be found in the LICENSE file. ============================================================================*/ #ifndef MITKPolhemusTOOL_H_HEADER_INCLUDED_ #define MITKPolhemusTOOL_H_HEADER_INCLUDED_ #include #include -#include namespace mitk { class PolhemusTrackingDevice; /** Documentation: * \brief An object of this class represents a tool of a Polhemus tracking device. * A tool has to be added to a tracking device which will then * continuously update the tool coordinates. * \ingroup IGT */ class MITKIGT_EXPORT PolhemusTool : public TrackingTool { public: friend class PolhemusTrackingDevice; mitkClassMacro(PolhemusTool, TrackingTool); enum DistortionLevel { UNDEFINED, ///< Distortion level is not determined. NO_DISTORTION, ///< System operational with a good quality magnetic signal. MINOR_DISTORTION, ///< System operational with a marginal magnetic signal. SIGNIFICANT_DISTORTION ///< System operational with a poor magnetic signal. }; /** * \brief Sets the port of the tool. (e.g. 1 for port "SENS 1" etc.) */ virtual void SetToolPort(int _ToolPort); /** * \brief Sets the port of the tool. (e.g. 1 for port "SENS 1" etc.) */ virtual int GetToolPort(); /** * \brief Sets the distortion level by mapping the integer value (read from the response frame) to the corresponding enumeration literal. * * According to the Polhemus Liberty documentation: * - 0 means system operational with a good quality magnetic signal. No distortion. * - 1 means system operational with a marginal magnetic signal. Minor distortion. * - 2 means system operational with a poor magnetic signal. Significant distortion. * * \param level The distortion level represented as 0, 1 or 2. */ void SetDistortionLevel(const int level); /** * \brief Returns the distortion level. * \return The distortion level. UNDEFINED, if distortion level is not determined. */ DistortionLevel GetDistortionLevel() const; protected: itkFactorylessNewMacro(Self); itkCloneMacro(Self) PolhemusTool(); virtual ~PolhemusTool(); //This is the port, on which the tool is connected. It is identical with the "ToolIdentifier" set on NavigationDataTools. //If tool is connected on port "SENS 2", the m_ToolPort is 2 etc. int m_ToolPort; DistortionLevel m_DistortionLevel; }; }//mitk #endif // MITKPolhemusTOOL_H_HEADER_INCLUDED_ diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp index 1d8a9374b1..94dc4bc874 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp @@ -1,66 +1,66 @@ /*============================================================================ The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center (DKFZ) All rights reserved. Use of this source code is governed by a 3-clause BSD license that can be found in the LICENSE file. ============================================================================*/ #include "mitkPolhemusTrackerTypeInformation.h" #include "mitkPolhemusTrackingDevice.h" namespace mitk { std::string PolhemusTrackerTypeInformation::GetTrackingDeviceName() { return "Polhemus tracker"; } TrackingDeviceData PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty() { TrackingDeviceData data = { PolhemusTrackerTypeInformation::GetTrackingDeviceName(), "Polhemus Liberty Tracker", "cube", "X" }; return data; } PolhemusTrackerTypeInformation::PolhemusTrackerTypeInformation() { m_DeviceName = PolhemusTrackerTypeInformation::GetTrackingDeviceName(); m_TrackingDeviceData.push_back(GetDeviceDataPolhemusTrackerLiberty()); } PolhemusTrackerTypeInformation::~PolhemusTrackerTypeInformation() { } mitk::TrackingDeviceSource::Pointer PolhemusTrackerTypeInformation::CreateTrackingDeviceSource( mitk::TrackingDevice::Pointer trackingDevice, mitk::NavigationToolStorage::Pointer navigationTools, std::string* errorMessage, std::vector* toolCorrespondencesInToolStorage) { mitk::TrackingDeviceSource::Pointer returnValue = mitk::TrackingDeviceSource::New(); mitk::PolhemusTrackingDevice::Pointer thisDevice = dynamic_cast(trackingDevice.GetPointer()); *toolCorrespondencesInToolStorage = std::vector(); //add the tools to the tracking device for (unsigned int i = 0; i < navigationTools->GetToolCount(); i++) { mitk::NavigationTool::Pointer thisNavigationTool = navigationTools->GetTool(i); toolCorrespondencesInToolStorage->push_back(i); bool toolAddSuccess = thisDevice->AddTool(thisNavigationTool->GetToolName().c_str(), std::stoi(thisNavigationTool->GetIdentifier())); if (!toolAddSuccess) { //todo error handling errorMessage->append("Can't add tool, is the toolfile valid?"); - return NULL; + return nullptr; } thisDevice->GetTool(i)->SetToolTipPosition(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolAxisOrientation()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp index 628637fc3d..156213927c 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp @@ -1,325 +1,304 @@ /*============================================================================ The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center (DKFZ) All rights reserved. Use of this source code is governed by a 3-clause BSD license that can be found in the LICENSE file. ============================================================================*/ #include "mitkPolhemusTrackingDevice.h" #include "mitkPolhemusTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include -#include +#include #include "mitkPolhemusTrackerTypeInformation.h" -typedef itk::MutexLockHolder MutexLockHolder; - mitk::PolhemusTrackingDevice::PolhemusTrackingDevice() : mitk::TrackingDevice() { //set the type of this tracking device this->m_Data = mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty(); - - this->m_MultiThreader = itk::MultiThreader::New(); - m_ThreadID = 0; - m_Device = mitk::PolhemusInterface::New(); } mitk::PolhemusTrackingDevice::~PolhemusTrackingDevice() { } bool mitk::PolhemusTrackingDevice::IsDeviceInstalled() { return true; } mitk::TrackingTool* mitk::PolhemusTrackingDevice::AddTool(const char* toolName, int toolPort) { //Only add tool if port isn't already used. for (auto _tool : m_AllTools) { if (_tool->GetToolPort() == toolPort) { MITK_DEBUG << "There is already a tool connected to this port. Returning existing tool"; return _tool; } } mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New(); t->SetToolName(toolName); t->SetToolPort(toolPort); if (this->InternalAddTool(t) == false) return nullptr; return t.GetPointer(); } bool mitk::PolhemusTrackingDevice::InternalAddTool(PolhemusTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::PolhemusTrackingDevice::StartTracking() { bool success = m_Device->StartTracking(); if (success) { mitk::IGTTimeStamp::GetInstance()->Start(this); this->SetState(Tracking); - this->m_StopTrackingMutex->Lock(); + this->m_StopTrackingMutex.lock(); this->m_StopTracking = false; - this->m_StopTrackingMutex->Unlock(); - m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method - return true; + this->m_StopTrackingMutex.unlock(); + if (m_Thread.joinable()) {m_Thread.detach();} + m_Thread = std::thread(&PolhemusTrackingDevice::ThreadStartTracking, this); // start a new thread that executes the TrackTools() methodreturn true; } else { this->SetState(Ready); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!"; } return success; } bool mitk::PolhemusTrackingDevice::StopTracking() { m_Device->StopTracking(); return Superclass::StopTracking(); } unsigned int mitk::PolhemusTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::PolhemusTrackingDevice::GetTool(unsigned int toolNumber) const { if (toolNumber >= this->GetToolCount()) return nullptr; else return this->m_AllTools[toolNumber]; } bool mitk::PolhemusTrackingDevice::OpenConnection() { //reset everything if (m_Device.IsNull()) { m_Device = mitk::PolhemusInterface::New(); } if (!m_Device->Connect()) //Connect the device, if it fails, throw an error. { MITK_ERROR << "Cannot connect Polhemus device!"; CloseConnection(); return false; } //Ready must be set here, 'cause if tools don't match we need to be able to disconnect. this->SetState(Ready); //check if connected ports of Polhemus matches the tools in the toolStorage. std::vector toolPorts = m_Device->GetToolPorts(); //first, check size. if (this->GetToolCount() != toolPorts.size()) { MITK_ERROR << "Cannot connect device, number of tools in toolstorage doesn't match the number of tools connected to Polhemus device!"; CloseConnection(); return false; } //second, check if toolStorage identifier is included in this port. for (auto _tool : m_AllTools) { if (std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()) == toolPorts.end()) { MITK_ERROR << "Cannot connect device, tool " << _tool->GetToolPort() << " is not connected to its port."; CloseConnection(); return false; } else { //erase this port to avoid that two tools want to connect to the same port. toolPorts.erase(std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort())); } } m_Device->SetHemisphereTrackingEnabled(m_HemisphereTrackingEnabled); return true; } bool mitk::PolhemusTrackingDevice::CloseConnection() { bool returnValue = true; if (this->GetState() == Setup) return true; returnValue = m_Device->Disconnect(); this->SetState(Setup); return returnValue; } mitk::PolhemusInterface* mitk::PolhemusTrackingDevice::GetDevice() { return m_Device; } std::vector mitk::PolhemusTrackingDevice::GetAllTools() { return this->m_AllTools; } void mitk::PolhemusTrackingDevice::TrackTools() { try { /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ - MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope + std::lock_guard 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 + this->m_StopTrackingMutex.lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; - this->m_StopTrackingMutex->Unlock(); + 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. Hardware gives " << lastData.size() << " tools, MITK expects " << m_AllTools.size() << " tools. Aborting!"; } else { std::vector allTools = this->GetAllTools(); for (size_t i = 0; i < allTools.size(); i++) { mitk::PolhemusTool::Pointer currentTool = allTools.at(i); const int distortionLevel = lastData.at(i).distortionLevel; if (distortionLevel == 0) { currentTool->SetDataValid(true); } else { currentTool->SetDataValid(false); } currentTool->SetDistortionLevel(distortionLevel); 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(); + this->m_StopTrackingMutex.lock(); localStopTracking = m_StopTracking; - this->m_StopTrackingMutex->Unlock(); + 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) +void mitk::PolhemusTrackingDevice::ThreadStartTracking() { - /* 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; + this->TrackTools(); } bool mitk::PolhemusTrackingDevice::AutoDetectToolsAvailable() { return true; } mitk::NavigationToolStorage::Pointer mitk::PolhemusTrackingDevice::AutoDetectTools() { std::vector singeFrameData = this->m_Device->AutoDetectTools(); MITK_INFO << "Found " << singeFrameData.size() << " tools."; mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); for each (mitk::PolhemusInterface::trackingData t in singeFrameData) { mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); std::stringstream name; name << "Sensor-" << ((int)t.id); newTool->GetDataNode()->SetName(name.str()); //The identifier defines, which plug is used (e.g. "Sens 2" --> 2). std::stringstream identifier; identifier << ((int)t.id); newTool->SetIdentifier(identifier.str()); newTool->SetTrackingDeviceType(mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty().Line); returnValue->AddTool(newTool); } return returnValue; } void mitk::PolhemusTrackingDevice::SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled) { //We need to remember if HemisphereTracking is switch on for this reason: /* m_Device->SetHemi works only if the device is connected. However, GUI can also change if it is not connected. In this case, we remember it in the m_HemisphereTrackingEnabled variable. And when connecting, we know, which status is wanted from the user by GUI. */ m_HemisphereTrackingEnabled = _HemisphereTrackingEnabled; this->m_Device->SetHemisphereTrackingEnabled(_HemisphereTrackingEnabled); } void mitk::PolhemusTrackingDevice::ToggleHemisphere(int _tool) { this->m_Device->ToggleHemisphere(_tool); } void mitk::PolhemusTrackingDevice::SetHemisphere(int _tool, mitk::Vector3D _hemisphere) { //If you set a hemisphere vector which is unequal (0|0|0), this means, that there is no hemisphere tracking any more //disable the option, so that it can be reactivated... Also if it is just a single tool. if (_hemisphere.GetNorm() != 0) m_HemisphereTrackingEnabled = false; this->m_Device->SetHemisphere(_tool, _hemisphere); } mitk::Vector3D mitk::PolhemusTrackingDevice::GetHemisphere(int _tool) { return this->m_Device->GetHemisphere(_tool); } bool mitk::PolhemusTrackingDevice::GetHemisphereTrackingEnabled(int _tool) { return this->m_Device->GetHemisphereTrackingEnabled(_tool); } void mitk::PolhemusTrackingDevice::AdjustHemisphere(int _tool) { return this->m_Device->AdjustHemisphere(_tool); } diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h index 0041bfa508..19261168a9 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h @@ -1,161 +1,160 @@ /*============================================================================ The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center (DKFZ) All rights reserved. Use of this source code is governed by a 3-clause BSD license that can be found in the LICENSE file. ============================================================================*/ #ifndef MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ #define MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ #include #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. * The tools which are used by Polhemus need to be connected to the correct port. * The port of the tool is stored as m_ToolPort in PolhemusTool AND as identifier in the NavigationTool (ToolStorage). * \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, int toolPort); 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(); /** Enables/disables hemisphere tracking for all sensors. */ void SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled); /** Is Hemisphere Tracking Enabled for this tool? */ bool GetHemisphereTrackingEnabled(int _tool); /** Toggles the current hemisphere. Parameter _tool describes, for which tool the hemisphere should change. Default -1 toggles all tools.*/ void ToggleHemisphere(int _tool = -1); /** Sets the Hemisphere of tool _tool to the vector _hemisphere */ void SetHemisphere(int _tool, mitk::Vector3D _hemisphere); /** Get the Hemisphere for _tool as mitk vector */ mitk::Vector3D GetHemisphere(int _tool); /** Adjust the Hemisphere for this tool. User needs to make sure, that the tool is located in hemisphere (1|0|0) when calling this function. In contrast to SetHemisphere(1,0,0), this method restores the original HemisphereTracking settings at the end. */ void AdjustHemisphere(int _tool); 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); + void ThreadStartTracking(); 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; + std::thread m_Thread; bool m_HemisphereTrackingEnabled; }; }//mitk #endif /* MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ */