diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp index 6da7470a30..1b74c7d343 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp @@ -1,361 +1,329 @@ /*=================================================================== 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 "mitkOpenIGTLinkTrackingDevice.h" #include "mitkOpenIGTLinkTrackingTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include "mitkTrackingTypes.h" #include #include #include #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::OpenIGTLinkTrackingDevice::OpenIGTLinkTrackingDevice(): mitk::TrackingDevice() { //set the type of this tracking device this->m_Data = mitk::DeviceDataOpenIGTLinkTrackingDeviceConnection; - this->m_MultiThreader = itk::MultiThreader::New(); - m_ThreadID = 0; - m_OpenIGTLinkClient = mitk::IGTLClient::New(); m_OpenIGTLinkClient->EnableInfiniteBufferingMode(m_OpenIGTLinkClient->GetReceiveQueue(),false); m_OpenIGTLinkClient->SetName("OpenIGTLink Tracking Device"); m_IGTLDeviceSource = mitk::IGTLDeviceSource::New(); m_IGTLDeviceSource->SetIGTLDevice(m_OpenIGTLinkClient); - } mitk::OpenIGTLinkTrackingDevice::~OpenIGTLinkTrackingDevice() { } int mitk::OpenIGTLinkTrackingDevice::GetPortNumber() { return m_OpenIGTLinkClient->GetPortNumber(); } std::string mitk::OpenIGTLinkTrackingDevice::GetHostname() { return m_OpenIGTLinkClient->GetHostname(); } void mitk::OpenIGTLinkTrackingDevice::SetPortNumber(int portNumber) { m_OpenIGTLinkClient->SetPortNumber(portNumber); } void mitk::OpenIGTLinkTrackingDevice::SetHostname(std::string hostname) { m_OpenIGTLinkClient->SetHostname(hostname); } mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::AddTool( const char* toolName, const char* fileName ) { mitk::OpenIGTLinkTrackingTool::Pointer t;// = mitk::OpenIGTLinkTrackingTool::New(); //TODO: Implement if (this->InternalAddTool(t) == false) return NULL; return t.GetPointer(); } bool mitk::OpenIGTLinkTrackingDevice::InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::OpenIGTLinkTrackingDevice::DiscoverTools(int WaitingTime) { if (m_OpenIGTLinkClient->GetPortNumber() == -1) { MITK_WARN << "Connection not initialized, aborting (invalid port number)."; return false; } try { m_IGTLDeviceSource->Connect(); m_IGTLDeviceSource->StartCommunication(); } catch(std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } //send a message to the server: start tracking stream mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory(); std::string message = "STT_TDATA"; //m_OpenIGTLinkClient->SendMessage(msgFactory->CreateInstance(message)); Sleep(WaitingTime); //wait for data to arrive m_IGTLDeviceSource->Update(); //check the tracking stream for the number and type of tools //igtl::MessageBase::Pointer receivedMessage = m_OpenIGTLinkClient->GetNextMessage(); mitk::IGTLMessage::Pointer receivedMessage = m_IGTLDeviceSource->GetOutput(); if (receivedMessage.IsNull()) { MITK_WARN << "No message was received. Is there really a server?"; return false; } else if (!receivedMessage->IsDataValid()) { MITK_WARN << "Received invalid message."; return false; } const char* msgType = receivedMessage->GetIGTLMessageType(); if( !(strcmp(msgType,"TDATA") == 0) ) { MITK_INFO << "Server does not send tracking data. Received data is not of the type TDATA. Received type: " << msgType; return true; } igtl::TrackingDataMessage* tdMsg = (igtl::TrackingDataMessage*)(receivedMessage->GetMessage().GetPointer()); if(!tdMsg) { MITK_WARN << "Cannot cast message object as expected, aborting!"; return false; } int numberOfTools = tdMsg->GetNumberOfTrackingDataElements(); MITK_INFO << "Found " << numberOfTools << " tools"; for(int i=0; iGetTrackingDataElement(i,currentTrackingData); std::string name = currentTrackingData->GetName(); if (name == "") //if no name was given create a default name { std::stringstream defaultName; defaultName << "OpenIGTLinkTool#" << i; name = defaultName.str(); } MITK_INFO << "Added tool " << name << " to tracking device."; newTool->SetToolName(name); this->InternalAddTool(newTool); } m_IGTLDeviceSource->StopCommunication(); this->SetState(Ready); return true; } void mitk::OpenIGTLinkTrackingDevice::UpdateTools() { if (this->GetState() != Tracking) { MITK_ERROR << "Method was called in the wrong state, something went wrong!"; return; } m_IGTLMsgToNavDataFilter->Update(); for (int i=0; iGetToolCount(); i++) { mitk::NavigationData::Pointer currentNavData = m_IGTLMsgToNavDataFilter->GetOutput(i); m_AllTools.at(i)->SetDataValid(currentNavData->IsDataValid()); m_AllTools.at(i)->SetPosition(currentNavData->GetPosition()); m_AllTools.at(i)->SetOrientation(currentNavData->GetOrientation()); m_AllTools.at(i)->SetIGTTimeStamp(currentNavData->GetIGTTimeStamp()); mitk::Point3D pos; m_AllTools.at(i)->GetPosition(pos); MITK_INFO << "Updated Tool " << i << " Pos: " << pos; } } bool mitk::OpenIGTLinkTrackingDevice::StartTracking() { //check tracking state if (this->GetState() != Ready) { MITK_WARN << "Cannot start tracking, device is not ready!"; return false; } try { m_IGTLDeviceSource->StartCommunication(); //send a message to the server: start tracking stream mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory(); std::string message = "STT_TDATA"; //m_OpenIGTLinkClient->SendMessage(msgFactory->CreateInstance(message)); } catch(std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while starting communication: " << e.what(); return false; } //create internal igtl pipeline m_IGTLMsgToNavDataFilter = mitk::IGTLMessageToNavigationDataFilter::New(); m_IGTLMsgToNavDataFilter->SetNumberOfExpectedOutputs(this->GetToolCount()); m_IGTLMsgToNavDataFilter->ConnectTo(m_IGTLDeviceSource); //connect itk events typedef itk::SimpleMemberCommand< mitk::OpenIGTLinkTrackingDevice > CurCommandType; CurCommandType::Pointer messageReceivedCommand = CurCommandType::New(); messageReceivedCommand->SetCallbackFunction(this, &mitk::OpenIGTLinkTrackingDevice::UpdateTools); m_MessageReceivedObserverTag = m_OpenIGTLinkClient->AddObserver(mitk::MessageReceivedEvent(),messageReceivedCommand); this->SetState(Tracking); return true; } bool mitk::OpenIGTLinkTrackingDevice::StopTracking() { //check tracking state if (this->GetState() != Tracking) { MITK_WARN << "Cannot open connection, device is already connected!"; return false; } m_OpenIGTLinkClient->RemoveObserver(m_MessageReceivedObserverTag); //disconnect itk events try { m_IGTLDeviceSource->StopCommunication(); } catch(std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while stopping communication: " << e.what(); return false; } this->SetState(Ready); return true; } unsigned int mitk::OpenIGTLinkTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::GetTool(unsigned int toolNumber) const { if ( toolNumber >= this->GetToolCount()) return NULL; else return this->m_AllTools[toolNumber]; } bool mitk::OpenIGTLinkTrackingDevice::OpenConnection() { //check tracking state if (this->GetState() != Setup) { MITK_WARN << "Cannot open connection, device is already connected!"; return false; } try { m_IGTLDeviceSource->Connect(); } catch(std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } this->SetState(Ready); return true; } bool mitk::OpenIGTLinkTrackingDevice::CloseConnection() { //check tracking state if (this->GetState() != Ready) { MITK_WARN << "Cannot close connection, device is in the wrong state!"; return false; } try { m_IGTLDeviceSource->Disconnect(); } catch(std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to disconnect: " << e.what(); return false; } this->SetState(Setup); return true; } std::vector mitk::OpenIGTLinkTrackingDevice::GetAllTools() { return this->m_AllTools; } - - -void mitk::OpenIGTLinkTrackingDevice::TrackTools() -{ - //TODO: Implement -} - - - -ITK_THREAD_RETURN_TYPE mitk::OpenIGTLinkTrackingDevice::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; - } - OpenIGTLinkTrackingDevice *trackingDevice = (OpenIGTLinkTrackingDevice*)pInfo->UserData; - - if (trackingDevice != NULL) - trackingDevice->TrackTools(); - - return ITK_THREAD_RETURN_VALUE; -} diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h index b38ba413d5..9f05bce29c 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h @@ -1,158 +1,143 @@ /*=================================================================== 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 MITKOPENIGTLINKTRACKINGDEVICE_H_HEADER_INCLUDED_ #define MITKOPENIGTLINKTRACKINGDEVICE_H_HEADER_INCLUDED_ #include #include #include #include #include #include #include namespace mitk { /** Documentation: * \brief An object of this class represents the MicronTracker 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 OpenIGTLinkTrackingDevice : public TrackingDevice { public: mitkClassMacro(OpenIGTLinkTrackingDevice, TrackingDevice); itkFactorylessNewMacro(Self) itkCloneMacro(Self) /** Sets the port number for the Open IGT Link connection. Default value is -1 (invalid). */ void SetPortNumber(int portNumber); /** Sets the hostname for the Open IGT Link connection. Default value is 127.0.0.1 (localhost). */ void mitk::OpenIGTLinkTrackingDevice::SetHostname(std::string hostname); int GetPortNumber(); std::string GetHostname(); /** * \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(); /** * \brief Stops the tracking. * \return Returns true if the tracking is stopped. */ virtual bool StopTracking(); /** * \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(); /** * \brief Closes the connection and clears all resources. */ virtual bool CloseConnection(); /** * \return Returns the number of tools which have been added to the device. */ virtual unsigned int GetToolCount() const; /** * \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; /** * \brief Discover the tools available from the connected OpenIGTLink device and adds these tools to this tracking device. Therefore, a connection * is opened, the tools are discovered and added. * \param WaitingTime Defines how long the method waits for an answer from the server (in milliseconds). Default value is 2000 (2 seconds). * \return Returns true if the connection was established and the tools were discovered successfully and - if at least one tool was found - were added to this device. * Retruns false if no valid connection is available. */ bool DiscoverTools(int WaitingTime = 2000); /** * \brief Create a new OpenIGTLink tool with toolName and fileName and add it to the list of tools * * Note that tools are usually provided by the OpenIGTLink connection. In most cases, the method DiscoverTools() should be used * instead which automatically finds the provided tools. If you use this method to manually add tools be sure that you add the * same number and type of tools that are provided by the connected device. Otherwise problems might occur when you try to start * tracking. */ mitk::TrackingTool* AddTool(const char* toolName, const char* fileName); protected: OpenIGTLinkTrackingDevice(); ~OpenIGTLinkTrackingDevice(); /** * \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(OpenIGTLinkTrackingTool::Pointer tool); /** Updates the tools from the open IGT link connection. Is called every time a message received event is invoked.*/ void UpdateTools(); unsigned long m_MessageReceivedObserverTag; - /** - * \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(); //OpenIGTLink connection class mitk::IGTLClient::Pointer m_OpenIGTLinkClient; //OpenIGTLink pipeline mitk::IGTLDeviceSource::Pointer m_IGTLDeviceSource; mitk::IGTLMessageToNavigationDataFilter::Pointer m_IGTLMsgToNavDataFilter; - - static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void* data); - std::vector m_AllTools; ///< vector holding all tools - itk::MultiThreader::Pointer m_MultiThreader; - int m_ThreadID; - }; }//mitk #endif /* MITKOpenIGTLinkTRACKINGDEVICE_H_HEADER_INCLUDED_ */