diff --git a/Modules/IGT/Testing/mitkOpenIGTLinkTrackingDeviceTest.cpp b/Modules/IGT/Testing/mitkOpenIGTLinkTrackingDeviceTest.cpp index 145a1517c2..715d3261e2 100644 --- a/Modules/IGT/Testing/mitkOpenIGTLinkTrackingDeviceTest.cpp +++ b/Modules/IGT/Testing/mitkOpenIGTLinkTrackingDeviceTest.cpp @@ -1,77 +1,81 @@ /*=================================================================== 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. ===================================================================*/ //testing headers #include #include //headers of IGT classes releated to the tested class #include class mitkOpenIGTLinkTrackingDeviceTestSuite : public mitk::TestFixture { CPPUNIT_TEST_SUITE(mitkOpenIGTLinkTrackingDeviceTestSuite); MITK_TEST(TestInstantiation); MITK_TEST(TestSetConnectionParameters); MITK_TEST(TestDiscoverToolMethod); CPPUNIT_TEST_SUITE_END(); private: /** Members used inside the different test methods. All members are initialized via setUp().*/ mitk::OpenIGTLinkTrackingDevice::Pointer m_OpenIGTLinkTrackingDevice; public: /**@brief Setup Always call this method before each Test-case to ensure correct and new intialization of the used members for a new test case. (If the members are not used in a test, the method does not need to be called).*/ void setUp() { m_OpenIGTLinkTrackingDevice = mitk::OpenIGTLinkTrackingDevice::New(); } void tearDown() { } void TestInstantiation() { // let's create objects of our classes mitk::OpenIGTLinkTrackingDevice::Pointer testDevice = mitk::OpenIGTLinkTrackingDevice::New(); CPPUNIT_ASSERT_MESSAGE("Testing instantiation of OpenIGTLinkTrackingDevice",testDevice.IsNotNull()); } void TestSetConnectionParameters() { m_OpenIGTLinkTrackingDevice->SetHostname("localhost"); m_OpenIGTLinkTrackingDevice->SetPortNumber(10); CPPUNIT_ASSERT_MESSAGE("Testing method SetHostname() ...", m_OpenIGTLinkTrackingDevice->GetHostname()=="localhost"); CPPUNIT_ASSERT_MESSAGE("Testing method SetPort() ...", m_OpenIGTLinkTrackingDevice->GetPortNumber()==10); } void TestDiscoverToolMethod() { CPPUNIT_ASSERT_MESSAGE("Testing DiscoverTools() without initialization. (Warnings are expected)", m_OpenIGTLinkTrackingDevice->DiscoverTools()==false); m_OpenIGTLinkTrackingDevice->SetPortNumber(10); CPPUNIT_ASSERT_MESSAGE("Testing DiscoverTools() with initialization, but without existing server. (Warnings are expected)", m_OpenIGTLinkTrackingDevice->DiscoverTools()==false); m_OpenIGTLinkTrackingDevice->SetHostname("193.174.50.103"); m_OpenIGTLinkTrackingDevice->SetPortNumber(18944); m_OpenIGTLinkTrackingDevice->DiscoverTools(); + m_OpenIGTLinkTrackingDevice->OpenConnection(); + m_OpenIGTLinkTrackingDevice->StartTracking(); + + Sleep(20000); } }; MITK_TEST_SUITE_REGISTRATION(mitkOpenIGTLinkTrackingDevice) diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp index f879521c27..b4649da2f2 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp @@ -1,252 +1,302 @@ /*=================================================================== 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)); + //m_OpenIGTLinkClient->SendMessage(msgFactory->CreateInstance(message)); - Sleep(WaitingTime); //wait for data to arrive + //Sleep(WaitingTime); //wait for data to arrive + + Sleep(20000); 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); } return true; } +void mitk::OpenIGTLinkTrackingDevice::UpdateTools() +{ + 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() { - //TODO: Implement + // todo: check tracking state + try + { + m_IGTLDeviceSource->StartCommunication(); + } + 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); + return false; } bool mitk::OpenIGTLinkTrackingDevice::StopTracking() { + m_OpenIGTLinkClient->RemoveObserver(m_MessageReceivedObserverTag); //disconnect itk events + Superclass::StopTracking(); 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() { - bool returnValue = false; - if (!m_OpenIGTLinkClient->TestConnection()) //TestConnection() not implemented yet! + try { - MITK_WARN << "No connection available, aborting!"; + 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; } - - return returnValue; + return true; } bool mitk::OpenIGTLinkTrackingDevice::CloseConnection() { bool returnValue = false; //TODO: Implement return returnValue; } 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 4da92001f6..b38ba413d5 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h @@ -1,153 +1,158 @@ /*=================================================================== 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_ */