diff --git a/Modules/IGTBase/mitkIGTTimeStamp.cpp b/Modules/IGTBase/mitkIGTTimeStamp.cpp new file mode 100644 index 0000000000..3ff1a2b10a --- /dev/null +++ b/Modules/IGTBase/mitkIGTTimeStamp.cpp @@ -0,0 +1,156 @@ +/*=================================================================== + +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 "mitkIGTTimeStamp.h" +#include + +#include "mitkRealTimeClock.h" + +mitk::IGTTimeStamp::Pointer mitk::IGTTimeStamp::s_Instance = NULL; + +mitk::IGTTimeStamp::IGTTimeStamp() : itk::Object() +, m_Time(-1.0), m_ReferenceTime(0.0) +{ +} + +mitk::IGTTimeStamp::~IGTTimeStamp() +{ +} + +mitk::IGTTimeStamp* mitk::IGTTimeStamp::CreateInstance() +{ +return mitk::IGTTimeStamp::GetInstance(); +} + +mitk::IGTTimeStamp* mitk::IGTTimeStamp::GetInstance() +{ + if (IGTTimeStamp::s_Instance.IsNull()) + { + mitk::IGTTimeStamp::Pointer ts = new mitk::IGTTimeStamp; + s_Instance = ts; + return s_Instance; + } + else + return s_Instance; +} + +void mitk::IGTTimeStamp::Start(itk::Object::Pointer device) +{ + if (m_RealTimeClock.IsNull()) + { + Initialize(); + } + if ( s_Instance.IsNotNull() ) + { + if (m_DeviceMap.empty()) + { + m_ReferenceTime = GetCurrentStamp(); + m_Time = 0.0; + } + m_DeviceMap.insert( std::pair(device, this->GetElapsed()) ); + } + else + { + itkGenericOutputMacro("Trying to use mitk::TimeStamp::Start() " + << "without an available singleton instance. Either no instance has " + << "been created (use TimeStamp::CreateInstance) or it has already " + << "been destroyed."); + } +} + +void mitk::IGTTimeStamp::Stop(itk::Object::Pointer device) +{ + if ( s_Instance.IsNotNull() ) + { + m_MapIterator = m_DeviceMap.find(device); + if ( m_MapIterator != m_DeviceMap.end() ) + { + m_DeviceMap.erase( m_MapIterator ); + } + + if (m_DeviceMap.empty()) + { + m_ReferenceTime = 0; + m_Time = -1; + } + } + else + { + itkGenericOutputMacro("Trying to use mitk::TimeStamp::Stop() " + << "without an available singleton instance. Either no instance has " + << "been created (use TimeStamp::CreateInstance) or it has already " + << "been destroyed."); + } +} + + +double mitk::IGTTimeStamp::GetElapsed() +{ + if (m_Time > -1) + { + m_Time = GetCurrentStamp(); + m_Time = m_Time - m_ReferenceTime; + } + return (double) m_Time; +} + + +double mitk::IGTTimeStamp::GetElapsed(itk::Object::Pointer device) +{ + double offset = this->GetOffset( device ); + if ( offset > -1 ) + { + double time = this->GetElapsed(); + return (double) time - this->GetOffset(device); + } + else + { + return (double) -1; + } +} + +double mitk::IGTTimeStamp::GetCurrentStamp() +{ + if (m_RealTimeClock.IsNotNull()) + { + return m_RealTimeClock->GetCurrentStamp(); + } + else return 0.0; +} + +void mitk::IGTTimeStamp::SetRealTimeClock(mitk::RealTimeClock::Pointer Clock) +{ + m_RealTimeClock = Clock; +} + +double mitk::IGTTimeStamp::GetOffset(itk::Object::Pointer Device) +{ + m_MapIterator = m_DeviceMap.find(Device); + if ( m_MapIterator != m_DeviceMap.end() ) + { + return m_MapIterator->second; + } + else + { + return -1.0; + } +} + +void mitk::IGTTimeStamp::Initialize() +{ + if ( m_RealTimeClock.IsNull() ) + m_RealTimeClock = mitk::RealTimeClock::New(); +} diff --git a/Modules/IGTBase/mitkIGTTimeStamp.h b/Modules/IGTBase/mitkIGTTimeStamp.h new file mode 100644 index 0000000000..029b56b33b --- /dev/null +++ b/Modules/IGTBase/mitkIGTTimeStamp.h @@ -0,0 +1,190 @@ +/*=================================================================== + +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 MITKTIMESTAMP_H_HEADER_INCLUDED_ +#define MITKTIMESTAMP_H_HEADER_INCLUDED_ + +#include +#include +#include + +#include "mitkRealTimeClock.h" + + +namespace mitk { + + /** + * \brief Time stamp in milliseconds + * + * This class provides a timestamp in milliseconds. + * It is a Singleton class, that internally uses a mitkRealTimeClock() for + * time-acquisition. + * + * First you have to call Start() in order to set the reference-time to the current time. + * If the user has not created and set his own "RealTimeClock", initialize() will be called and a + * default mitkRealTimeClock() is created. + * In addition the TimeStamp() saves a pointer to the device calling and the respective offset-time. + * The first device will have an offset of 0, the following's offset will be the time elapsed since the + * starting of the first device. This offset can be prompted by calling GetOffset(); + * + * You can always get the time elapsed since calling Start() with GetElapsed(). It returns the + * time spent in milliseconds as a double. + * + * When the TimeStamp is no longer used, you can call Stop(). This erases the pointer to the device + * and the offset. When all devices have "stopped tracking" the reference-time and the current-time are reset to 0. + * + * \ingroup IGT + */ + class MitkIGT_EXPORT IGTTimeStamp : public itk::Object + { + public: + + mitkClassMacro(IGTTimeStamp, itk::Object); + + /** + * \brief creates a new instance of mitkTimeStamp + * + * This method returns a pointer to the currently existing TimeStamp. + * If there is no exisiting instance, a new one is created and returned automatically + * + * DECREPATED: Use GetInstance instead + */ + static IGTTimeStamp* CreateInstance(); + + /** + * \brief returns a pointer to the current instance of mitkTimeStamp + * + * This method returns a pointer to the currently existing TimeStamp. + * If there is no exisiting instance, a new one is created and returned automatically + */ + static IGTTimeStamp* GetInstance(); + + /** + * \brief starts the time-acquisition + * + * Each device is to call this method when it starts tracking. + * The current time is saved as a reference-value (m_Time = 0). + * Internally the device (pointer) and its offset are saved in a map, so that + * no device can call this method twice. + * If the user has not set its own RealTimeClock, a default one is created dependant on the OS + * in use. + * + */ + void Start( itk::Object::Pointer device ); + + /** + * \brief stops the time-acqusition + * + * Each device has to call Stop() when it has finished and its + * pointer will be erased from the map. When the last device has "stopped" + * the reference-time and the current-time will be reset to 0. + * + */ + void Stop( itk::Object::Pointer device ); + + /** + * \brief returns the time elapsed since calling Start() for the first time in milliseconds + * + * GetElapsed() returns the time elapsed since Start() has been called first, no matter + * which itk::Object did the call. + * This method-call can be used if you want to need to have several processes you want to + * monitor and need timestamps in the same space of time, e.g. when using two tracking-devices + * on the same experiment. + */ + double GetElapsed(); + + /** + * \brief returns the time elapsed since 'device' called Start() in milliseconds + * + * GetElapsed(itk::Object device) returns the time elapsed since the given itk::Object called + * Start(). + * This overloaded method should be used when you only have one independent process to keep + * track of, e.g. when you want to measure how long it takes to execute a piece of code. + */ + double GetElapsed(itk::Object::Pointer device); + + + /** + * \brief returns the offset of this device's starting-time to the + * reference-time in ms + * + * Device 'A' is the first device to call Start(). Device 'B' calls Start() + * some time later. This time-difference is the offset, that each device has realtive to the + * device that started the time-acquisition. + * Each device's offset is stored in a map with a pointer to the device. + * + * If this device has not been or is no longer saved in the map of devices, + * -1 will be returned. + * + * + * only used internally + */ + double GetOffset(itk::Object::Pointer Device); + + /** + * \brief setter for the internally used RealTimeClock() + * + * If you want to use a "third-party" RealTimeClock, e.g PocoRealTimeClock, BoostRealTimeClock + * or ITKRealTimeClock, you can set it using this method: + * mitk::RealTimeClock::Pointer RealTimeClock = mitk::RealTimeClock::New(); + * mitk::TimeStamp::GetInstance()->SetRealTimeClock(RealTimeClock); + * + * Right now, none of these RealTimeClocks have been implemented!! + * + * Notice: The mitk-implementation of an os-dependant RealTimeClock is used + * by default. + */ + void SetRealTimeClock(mitk::RealTimeClock::Pointer Clock); + + /** + * \brief creates a new RealTimeClock + * + * Instanciates a new RealTimeClock, that will be specific for the Operating System. + * This will only be called internally when no other RealTimeClock has been set + * by the user. + * + */ + void Initialize(); + + protected: + + IGTTimeStamp(); + + virtual ~IGTTimeStamp(); + + double GetCurrentStamp(); + + /* the current timestamp when GetCurrentStamp() is called. */ + double m_Time; + + /* the timestamp in ms acquired when Start() was called. */ + double m_ReferenceTime; + + /* pointer to the RealTimeClock used internally */ + mitk::RealTimeClock::Pointer m_RealTimeClock; + + /* pointer to the current instance */ + static mitk::IGTTimeStamp::Pointer s_Instance; + + /* map, in which pointer to all devices calling Start(), are saved */ + std::map m_DeviceMap; + + std::map::iterator m_MapIterator; + }; +} // namespace mitk + +#endif /* MITKTIMESTAMP_H_HEADER_INCLUDED_ */ diff --git a/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.cpp b/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.cpp index 9cec79ac24..7b883cbfef 100644 --- a/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.cpp +++ b/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.cpp @@ -1,278 +1,344 @@ /*=================================================================== 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 "mitkIGTLMessageToNavigationDataFilter.h" #include "igtlTrackingDataMessage.h" #include "igtlQuaternionTrackingDataMessage.h" +#include "igtlTransformMessage.h" #include "mitkQuaternion.h" mitk::IGTLMessageToNavigationDataFilter::IGTLMessageToNavigationDataFilter() : mitk::NavigationDataSource() { mitk::NavigationData::Pointer output = mitk::NavigationData::New(); this->SetNumberOfRequiredOutputs(1); this->SetNthOutput(0, output.GetPointer()); } mitk::IGTLMessageToNavigationDataFilter::~IGTLMessageToNavigationDataFilter() { } void mitk::IGTLMessageToNavigationDataFilter::SetInput( const IGTLMessage* msg ) { this->SetInput(0, msg); } void mitk::IGTLMessageToNavigationDataFilter::SetInput( unsigned int idx, const IGTLMessage* msg ) { if ( msg == NULL ) // if an input is set to NULL, remove it { this->RemoveInput(idx); } else { // ProcessObject is not const-correct so a const_cast is required here this->ProcessObject::SetNthInput(idx, const_cast(msg)); } this->CreateOutputsForAllInputs(); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput( void ) const { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(this->ProcessObject::GetInput(0)); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput( unsigned int idx ) const { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(this->ProcessObject::GetInput(idx)); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(std::string messageName) const { const DataObjectPointerArray& inputs = const_cast(this)->GetInputs(); for (DataObjectPointerArray::const_iterator it = inputs.begin(); it != inputs.end(); ++it) { if (std::string(messageName) == (static_cast(it->GetPointer()))->GetName()) { return static_cast(it->GetPointer()); } } return NULL; } itk::ProcessObject::DataObjectPointerArraySizeType mitk::IGTLMessageToNavigationDataFilter::GetInputIndex( std::string messageName ) { DataObjectPointerArray outputs = this->GetInputs(); for (DataObjectPointerArray::size_type i = 0; i < outputs.size(); ++i) { if (messageName == (static_cast(outputs.at(i).GetPointer()))->GetName()) { return i; } } throw std::invalid_argument("output name does not exist"); } void mitk::IGTLMessageToNavigationDataFilter::ConnectTo( mitk::IGTLMessageSource* UpstreamFilter) { for (DataObjectPointerArraySizeType i = 0; i < UpstreamFilter->GetNumberOfOutputs(); i++) { this->SetInput(i, UpstreamFilter->GetOutput(i)); } } void mitk::IGTLMessageToNavigationDataFilter::CreateOutputsForAllInputs() { // create outputs for all inputs this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs()); bool isModified = false; for (unsigned int idx = 0; idx < this->GetNumberOfIndexedOutputs(); ++idx) { if (this->GetOutput(idx) == NULL) { mitk::NavigationData::Pointer newOutput = mitk::NavigationData::New(); this->SetNthOutput(idx, newOutput); isModified = true; } } if(isModified) this->Modified(); } void mitk::IGTLMessageToNavigationDataFilter::GenerateData() { this->CreateOutputsForAllInputs(); // make sure that we have the same number of outputs as inputs /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs() ; ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); const mitk::IGTLMessage* input = this->GetInput(i); assert(input); if (input->IsDataValid() == false) { output->SetDataValid(false); continue; } -// const char* a = input->GetIGTLMessageType(); - //check if the IGTL message has the proper type if( strcmp(input->GetIGTLMessageType(), "TRANSFORM") == 0 ) { //get the tracking data message +// igtl::TransformMessage::Pointer tMsg = +// dynamic_cast(test.GetPointer()); + + igtl::TransformMessage* tMsg = + (igtl::TransformMessage*)(input->GetMessage().GetPointer()); + +// igtl::TransformMessage::Pointer tMsg = +// igtl::TransformMessage::Pointer(input->GetMessage().GetPointer()); +//// tMsg->Copy(test); + + //check if cast was successful + if ( !tMsg ) + { + mitkThrow() << "Cast from igtl::MessageBase to igtl::TransformMessage " + << "failed! Please check the message."; + continue; + } + + tMsg->Print(std::cout); + + //get the transformation matrix and convert it into an affinetransformation + igtl::Matrix4x4 transformation_; + tMsg->GetMatrix(transformation_); + mitk::AffineTransform3D::Pointer affineTransformation = + mitk::AffineTransform3D::New(); + mitk::Matrix3D transformation; + mitk::Vector3D offset; + for ( unsigned int r = 0; r < 3; r++ ) + { + for ( unsigned int c = 0; c < 3; c++ ) + { + transformation.GetVnlMatrix().set( r , c , transformation_[r][c] ); + } + offset.SetElement(r, transformation_[r][4]); + } + //convert the igtl matrix here and set it in the affine transformation + affineTransformation->SetMatrix(transformation); + affineTransformation->SetOffset(offset); + + //create a new navigation data here, there is a neat constructor for + //affine transformations that sets the orientation, position according to + //the affine transformation. The other values are initialized with standard + //values + mitk::NavigationData::Pointer nd = + mitk::NavigationData::New(affineTransformation, true); + output->Graft(nd); + + nd->Print(std::cout); + } + else if( strcmp(input->GetIGTLMessageType(), "TDATA") == 0 ) + { + //get the tracking data message igtl::TrackingDataMessage::Pointer tdMsg = dynamic_cast( input->GetMessage().GetPointer()); //get the number of tracking data elements unsigned int numTrackingDataElements = tdMsg->GetNumberOfTrackingDataElements(); - //here should be a for loop over all tracking data elements - //CHECK SIZE HERE + //one message can contain several tracking data elements, but is this + //really used? If yes, what is a tracking element? If there is one tracking + //element per connected tracker, then we definitely have check all of them. + //Moreover, we have to read the message, check how many elements there are + //and then adapt the number of outputs to the number of tracking element. + //Every tracker should have a seperate output. + //at the moment I just want the first one + //check if there is at least one tracking data element + if ( !numTrackingDataElements ) + { + output->SetDataValid(false); + MITK_ERROR("IGTLMsgToNavDataFilter") << "There are no tracking data " + "elements in this message"; + continue; + } //get the tracking data element which holds all the data igtl::TrackingDataElement::Pointer td; tdMsg->GetTrackingDataElement(0, td); //get the transformation matrix and convert it into an affinetransformation igtl::Matrix4x4 transformation_; td->GetMatrix(transformation_); mitk::AffineTransform3D::Pointer affineTransformation; mitk::Matrix3D transformation; mitk::Vector3D offset; for ( unsigned int r = 0; r < 3; r++ ) { for ( unsigned int c = 0; c < 3; c++ ) { transformation.GetVnlMatrix().set( r , c , transformation_[r][c] ); } offset.SetElement(r, transformation_[r][4]); } //convert the igtl matrix here and set it in the affine transformation affineTransformation->SetMatrix(transformation); affineTransformation->SetOffset(offset); // //get the position and set it in the affine transformation // mitk::NavigationData::PositionType position; // td->GetPosition(position.pointer); // affineTransformation->SetOffset(position); //create a new navigation data here, there is a neat constructor for //affine transformations that sets the orientation, position according to //the affine transformation. The other values are initialized with standard //values mitk::NavigationData::Pointer nd = mitk::NavigationData::New(affineTransformation, true); output->Graft(nd); } else if( strcmp(input->GetIGTLMessageType(), "QTDATA") == 0 ) { //get the tracking data message igtl::QuaternionTrackingDataMessage::Pointer tdMsg = dynamic_cast( input->GetMessage().GetPointer()); //get the tracking data element which holds all the data igtl::QuaternionTrackingDataElement::Pointer td; tdMsg->GetQuaternionTrackingDataElement(0, td); //get the quaternion and set it float quaternion_[4]; //igtl quat type td->GetQuaternion(quaternion_); mitk::Quaternion quaternion; quaternion.put(0, quaternion_[0]); quaternion.put(1, quaternion_[1]); quaternion.put(2, quaternion_[2]); output->SetOrientation(quaternion); output->SetHasOrientation(true); //get the position and set it float position_[3]; //igtl position type td->GetPosition(position_); mitk::NavigationData::PositionType position; //mitk position type position.SetElement(0, position_[0]); position.SetElement(1, position_[1]); position.SetElement(2, position_[2]); output->SetPosition(position); output->SetHasPosition(true); //the data is valid and there is no explicit covarience matrix output->SetDataValid(true); output->SetCovErrorMatrix(mitk::NavigationData::CovarianceMatrixType()); } else { //the message has another type //set the output invalid and try the next input output->SetDataValid(false); continue; } //set the time stamp output->SetIGTTimeStamp(input->GetTimeStamp()); //set the name output->SetName(input->GetName()); + + output->Print(std::cout); } } void mitk::IGTLMessageToNavigationDataFilter::GenerateOutputInformation() { // Superclass::GenerateOutputInformation(); // mitk::NavigationData* output = this->GetOutput(0); // assert(output); // const mitk::IGTLMessage* input = this->GetInput(0); // assert(input); itkDebugMacro(<<"GenerateOutputInformation()"); // output->Initialize(input->GetPixelType(), input->GetDimension(), input->GetDimensions()); // // initialize geometry // output->SetPropertyList(input->GetPropertyList()->Clone()); // mitk::TimeGeometry::Pointer clonGeometry = input->GetTimeGeometry()->Clone(); // output->SetTimeGeometry(clonGeometry.GetPointer()); } diff --git a/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.h b/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.h index dbc0ee8ed9..041f6d2324 100644 --- a/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.h +++ b/Modules/IGTLink/mitkIGTLMessageToNavigationDataFilter.h @@ -1,126 +1,129 @@ /*=================================================================== 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 MITKOPENIGTLINKMESSAGETONAVIGATIONDATAFILTER_H_HEADER_INCLUDED_ #define MITKOPENIGTLINKMESSAGETONAVIGATIONDATAFILTER_H_HEADER_INCLUDED_ #include #include "mitkIGTLMessage.h" #include "mitkIGTLMessageSource.h" #include "MitkIGTLinkExports.h" namespace mitk { /**Documentation * \brief IGTLinkMessageToNavigationDataFilter is a filter that receives * OpenIGTLink messages as input and produce NavigationDatas as output * * IGTLinkMessageToNavigationDataFilter is a filter that receives * OpenIGTLink messages as input and produce NavigationDatas as output. * If the OpenIGTLink message is not of the proper type the filter will not * do anything. * * \ingroup IGT */ class MITK_IGTLINK_EXPORT IGTLMessageToNavigationDataFilter : public NavigationDataSource { public: mitkClassMacro(IGTLMessageToNavigationDataFilter, NavigationDataSource); itkFactorylessNewMacro(Self) itkCloneMacro(Self) - //using Superclass::SetInput; + using Superclass::SetInput; /** * \brief Set the input of this filter * * \warning: this will set the number of outputs to the number of inputs, * deleting any extra outputs that might have been initialized. * Subclasses that have a different number of outputs than inputs * must overwrite the SetInput methods. */ virtual void SetInput( const IGTLMessage* msg); /** * \brief Set input with id idx of this filter * * \warning: this will set the number of outputs to the number of inputs, * deleting any extra outputs that might have been initialized. * Subclasses that have a different number of outputs than inputs * must overwrite the SetInput methods. * If the last input is set to NULL, the number of inputs will be decreased by * one (-> removing the last input). If other inputs are set to NULL, the * number of inputs will not change. */ virtual void SetInput( unsigned int idx, const IGTLMessage* msg); + /** Set an input */ +// virtual void SetInput(const DataObjectIdentifierType & key, DataObject *input); + /** * \brief Get the input of this filter */ const IGTLMessage* GetInput(void) const; /** * \brief Get the input with id idx of this filter */ const IGTLMessage* GetInput(unsigned int idx) const; /** * \brief Get the input with name messageName of this filter */ const IGTLMessage* GetInput(std::string messageName) const; /** *\brief return the index of the input with name messageName, * throw std::invalid_argument exception if that name was not found * */ DataObjectPointerArraySizeType GetInputIndex(std::string messageName); /** *\brief Connects the input of this filter to the outputs of the given * IGTLMessageSource * * This method does not support smartpointer. use FilterX.GetPointer() to * retrieve a dumbpointer. */ virtual void ConnectTo(mitk::IGTLMessageSource * UpstreamFilter); protected: IGTLMessageToNavigationDataFilter(); virtual ~IGTLMessageToNavigationDataFilter(); virtual void GenerateData(); /** * \brief Create an output for each input * * This Method sets the number of outputs to the number of inputs * and creates missing outputs objects. * \warning any additional outputs that exist before the method is called are * deleted */ void CreateOutputsForAllInputs(); /** * \brief Defines how the input will be copied into the output */ virtual void GenerateOutputInformation(); }; } // namespace mitk #endif /* MITKOPENIGTLMESSAGETONAVIGATIONDATAFILTER_H_HEADER_INCLUDED_ */ diff --git a/Modules/OpenIGTLink/files.cmake b/Modules/OpenIGTLink/files.cmake index 2028300562..eeaa6a0d50 100644 --- a/Modules/OpenIGTLink/files.cmake +++ b/Modules/OpenIGTLink/files.cmake @@ -1,7 +1,8 @@ set(CPP_FILES mitkIGTLClient.cpp mitkIGTLDevice.cpp mitkIGTLMessageSource.cpp mitkIGTLDeviceSource.cpp mitkIGTLMessage.cpp + mitkIGTLMessageCommon.cpp ) diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.cpp b/Modules/OpenIGTLink/mitkIGTLDevice.cpp index c02ebb8d77..a6ebd6f597 100644 --- a/Modules/OpenIGTLink/mitkIGTLDevice.cpp +++ b/Modules/OpenIGTLink/mitkIGTLDevice.cpp @@ -1,471 +1,486 @@ /*=================================================================== 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 "mitkIGTLDevice.h" //#include "mitkIGTTimeStamp.h" #include #include #include #include +#include typedef itk::MutexLockHolder MutexLockHolder; mitk::IGTLDevice::IGTLDevice() : // m_Data(mitk::DeviceDataUnspecified), m_State(mitk::IGTLDevice::Setup), m_StopCommunication(false), m_PortNumber(-1), m_MultiThreader(NULL), m_ThreadID(0) { m_StopCommunicationMutex = itk::FastMutexLock::New(); m_StateMutex = itk::FastMutexLock::New(); m_LatestMessageMutex = itk::FastMutexLock::New(); m_CommunicationFinishedMutex = itk::FastMutexLock::New(); // execution rights are owned by the application thread at the beginning m_CommunicationFinishedMutex->Lock(); m_MultiThreader = itk::MultiThreader::New(); // m_Data = mitk::DeviceDataUnspecified; - m_LatestMessage = igtl::MessageBase::New(); + m_LatestMessage = igtl::TransformMessage::New(); } mitk::IGTLDevice::~IGTLDevice() { /* stop communication and disconnect from igtl device */ if (GetState() == Running) { this->StopCommunication(); } if (GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if ((m_ThreadID != 0) && (m_MultiThreader.IsNotNull())) { m_MultiThreader->TerminateThread(m_ThreadID); } m_MultiThreader = NULL; } mitk::IGTLDevice::IGTLDeviceState mitk::IGTLDevice::GetState() const { MutexLockHolder lock(*m_StateMutex); return m_State; } void mitk::IGTLDevice::SetState( IGTLDeviceState 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::IGTLDeviceData mitk::IGTLDevice::GetData() const{ // return m_Data; //} //void mitk::IGTLDevice::SetData(mitk::IGTLDeviceData data){ // m_Data = data; //} bool mitk::IGTLDevice::SendMessage(igtl::MessageBase::Pointer msg) { // 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; return true; } //mitk::NDIErrorCode mitk::IGTLDevice::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; //} bool mitk::IGTLDevice::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 // { // m_SerialCommunication->CloseConnection(); // m_SerialCommunication = NULL; // mitkThrowException(mitk::IGTHardwareException) << "Hardware Reset of tracking device did not work"; // } // /* 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; return true; } ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartCommunication(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; } IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData; if (igtlDevice != NULL) { igtlDevice->RunCommunication(); } igtlDevice->m_ThreadID = 0; // erase thread id because thread will end. return ITK_THREAD_RETURN_VALUE; } void mitk::IGTLDevice::RunCommunication() { if (this->GetState() != Running) return; // keep lock until end of scope MutexLockHolder communicationFinishedLockHolder(*m_CommunicationFinishedMutex); // Because m_StopCommunication is used by two threads, access has to be guarded // by a mutex. To minimize thread locking, a local copy is used here bool localStopCommunication; // update the local copy of m_StopCommunication this->m_StopCommunicationMutex->Lock(); localStopCommunication = this->m_StopCommunication; this->m_StopCommunicationMutex->Unlock(); while ((this->GetState() == Running) && (localStopCommunication == false)) { //POLLLING // Create a message buffer to receive header igtl::MessageHeader::Pointer headerMsg; headerMsg = igtl::MessageHeader::New(); // Initialize receive buffer headerMsg->InitPack(); // Receive generic header from the socket // this->m_SocketMutex->Lock(); int r = m_Socket->Receive(headerMsg->GetPackPointer(), headerMsg->GetPackSize(),1); // this->m_SocketMutex->Unlock(); if(r == 0) { //this->StopCommunication(); // an error was received, therefor the communication must be stopped m_StopCommunicationMutex->Lock(); m_StopCommunication = true; m_StopCommunicationMutex->Unlock(); } else if (r == headerMsg->GetPackSize()) { // Deserialize the header and check the CRC int crcCheck = headerMsg->Unpack(1); if (crcCheck & igtl::MessageHeader::UNPACK_HEADER) { // Allocate a time stamp igtl::TimeStamp::Pointer ts; ts = igtl::TimeStamp::New(); // Get time stamp igtlUint32 sec; igtlUint32 nanosec; headerMsg->GetTimeStamp(ts); ts->GetTimeStamp(&sec, &nanosec); //check for invalid timestamps // if(sec != 0) { std::cerr << "Time stamp: " << sec << "." << nanosec << std::endl; std::cerr << "Dev type and name: " << headerMsg->GetDeviceType() << " " << headerMsg->GetDeviceName() << std::endl; headerMsg->Print(std::cout); //Create a message buffer to receive transform data - igtl::MessageBase::Pointer curMessage; + /*igtl::MessageBase::Pointer curMessage; curMessage = igtl::MessageBase::New(); curMessage->SetMessageHeader(headerMsg); + curMessage->AllocatePack();*/ + + //Create a message buffer to receive transform data + igtl::TransformMessage::Pointer curMessage; + curMessage = igtl::TransformMessage::New(); + curMessage->SetMessageHeader(headerMsg); curMessage->AllocatePack(); // Receive transform data from the socket int receiveCheck = 0; receiveCheck = m_Socket->Receive(curMessage->GetPackBodyPointer(), curMessage->GetPackBodySize()); if ( receiveCheck > 0 ) { + int c = curMessage->Unpack(1); + if ( !(c & igtl::MessageHeader::UNPACK_BODY) ) + { + mitkThrow() << "crc error"; + } + //copy the current message into the latest message member bool copyCheck = false; m_LatestMessageMutex->Lock(); - copyCheck = (bool)m_LatestMessage->Copy(curMessage); + copyCheck = IGTLMessageCommon::Clone(m_LatestMessage, curMessage); +// m_LatestMessage->Copy(curMessage); +// copyCheck = m_LatestMessage->Copy(curMessage); m_LatestMessageMutex->Unlock(); //check if the copying was successful, otherwise invalidate the //latest message if ( !copyCheck ) { m_LatestMessageMutex->Lock(); - m_LatestMessage = igtl::MessageBase::New(); + m_LatestMessage = igtl::TransformMessage::New(); m_LatestMessageMutex->Unlock(); MITK_ERROR("IGTLDevice") << "Could not copy the received message."; } } else { MITK_ERROR("IGTLDevice") << "Received a valid header but could not " << "read the whole message."; } // if (strcmp(headerMsg->GetDeviceType(), "TRANSFORM") == 0) // { // //ReceiveImage(socket, headerMsg); // // Create a message buffer to receive transform data // igtl::TransformMessage::Pointer transMsg; // transMsg = igtl::TransformMessage::New(); // transMsg->SetMessageHeader(headerMsg); // transMsg->AllocatePack(); // // Receive transform data from the socket // m_Socket->Receive(transMsg->GetPackBodyPointer(), // transMsg->GetPackBodySize()); // // Deserialize the transform data // // If you want to skip CRC check, call Unpack() without argument. // int c = transMsg->Unpack(1); // if (c & igtl::MessageHeader::UNPACK_BODY) // if CRC check is OK // { // // Retrive the transform data // igtl::Matrix4x4 matrix; // transMsg->GetMatrix(matrix); // igtl::PrintMatrix(matrix); // std::cerr << std::endl; // } // } } } } else { //Message size information and actual data size don't match. } // 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_StopCommunication */ this->m_StopCommunicationMutex->Lock(); localStopCommunication = m_StopCommunication; this->m_StopCommunicationMutex->Unlock(); itksys::SystemTools::Delay(1); } // StopCommunication was called, thus the mode should be changed back to Ready now // that the tracking loop has ended. this->SetState(Ready); MITK_DEBUG("IGTLDevice::RunCommunication") << "Reached end of communication."; // returning from this function (and ThreadStartCommunication()) // this will end the thread return; } bool mitk::IGTLDevice::StartCommunication() { if (this->GetState() != Ready) return false; this->SetState(Running); // go to mode Running // update the local copy of m_StopCommunication this->m_StopCommunicationMutex->Lock(); this->m_StopCommunication = false; this->m_StopCommunicationMutex->Unlock(); // transfer the execution rights to tracking thread m_CommunicationFinishedMutex->Unlock(); // start a new thread that executes the communication m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartCommunication, this); // mitk::IGTTimeStamp::GetInstance()->Start(this); return true; } bool mitk::IGTLDevice::StopCommunication() { if (this->GetState() == Running) // Only if the object is in the correct state { // m_StopCommunication is used by two threads, so we have to ensure correct // thread handling m_StopCommunicationMutex->Lock(); m_StopCommunication = true; m_StopCommunicationMutex->Unlock(); // we have to wait here that the other thread recognizes the STOP-command // and executes it m_CommunicationFinishedMutex->Lock(); // mitk::IGTTimeStamp::GetInstance()->Stop(this); // notify realtime clock // StopCommunication was called, thus the mode should be changed back // to Ready now that the tracking loop has ended. this->SetState(Ready); } return true; } bool mitk::IGTLDevice::CloseConnection() { if (this->GetState() == Setup) { return true; } else if (this->GetState() == Running) { this->StopCommunication(); } m_Socket->CloseSocket(); // //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); // m_SerialCommunication = NULL; return true; } -bool mitk::IGTLDevice::GetLatestMessage(igtl::MessageBase::Pointer msg) +igtl::MessageBase::Pointer mitk::IGTLDevice::GetLatestMessage() { - bool copySuccesful = false; //copy the latest message into the given msg m_LatestMessageMutex->Lock(); - copySuccesful = msg->Copy(m_LatestMessage); + igtl::MessageBase::Pointer msg = + mitk::IGTLMessageCommon::Clone(m_LatestMessage); m_LatestMessageMutex->Unlock(); - return copySuccesful; + return msg; } diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.h b/Modules/OpenIGTLink/mitkIGTLDevice.h index ff1115209e..b9bb40c9ee 100644 --- a/Modules/OpenIGTLink/mitkIGTLDevice.h +++ b/Modules/OpenIGTLink/mitkIGTLDevice.h @@ -1,191 +1,192 @@ /*=================================================================== 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 MITKIGTLDEVICE_H #define MITKIGTLDEVICE_H #include #include "itkObject.h" #include "mitkCommon.h" #include "itkFastMutexLock.h" #include //#include #include "igtlSocket.h" #include "igtlMessageBase.h" +#include "igtlTransformMessage.h" namespace mitk { /**Documentation * \brief Interface for all Open IGT Link Devices * * Defines the methods that are common for all devices using Open IGT Link. * */ class MITK_OPENIGTLINK_EXPORT IGTLDevice : public itk::Object { public: mitkClassMacro(IGTLDevice, itk::Object) /** * Type for state variable. The IGTLDevice is always in one of these states */ enum IGTLDeviceState {Setup, Ready, Running}; /** * \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 device is not running (e.g. object is in Ready state) */ virtual bool CloseConnection(); /** * \brief Stops the communication between the two devices * * This may only be called if there the device is in Running state */ virtual bool StopCommunication(); bool StartCommunication(); void RunCommunication(); /** * \brief Sends a message via the open IGT Link. * * 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 Running */ bool SendMessage(igtl::MessageBase::Pointer msg); /** * \brief return current object state (Setup, Ready or Running) */ IGTLDeviceState GetState() const; /** * \brief Returns the latest received message * \param msg A smartpointer to the message base where the latest message * shall be copied into * \retval true The latest message is stored in msg * \retval false The latest message could not been copied, do not use this * data */ - bool GetLatestMessage(igtl::MessageBase::Pointer msg); + igtl::MessageBase::Pointer GetLatestMessage(); /** * \brief return device data */ // igtl::MessageBase::Pointer GetData() const; /** * \brief set device data */ // void SetData(IGTLDeviceData data); /** * \brief Sets the port number of the device */ itkSetMacro(PortNumber,int); /** * \brief Returns the port number of the device */ itkGetMacro(PortNumber,int); /** * \brief Sets the ip/hostname of the device */ itkSetMacro(Hostname,std::string); /** * \brief Returns the ip/hostname of the device */ itkGetMacro(Hostname,std::string); /** * \brief static start method for the tracking thread. */ static ITK_THREAD_RETURN_TYPE ThreadStartCommunication(void* data); /** * \brief TestConnection() tries to connect to a IGTL server on the current * ip and port * * \todo check this description * * TestConnection() tries to connect to a IGTL server on the current * ip and port and returns which device it has found. * \return It returns the type of the device that answers. Throws an exception * if no device is available on that ip/port. * @throw mitk::IGTHardwareException Throws an exception if there are errors * while connecting to the device. */ virtual bool TestConnection(); protected: /** * \brief change object state */ void SetState(IGTLDeviceState state); IGTLDevice(); virtual ~IGTLDevice(); // IGTLDeviceData m_Data; ///< current device Data IGTLDeviceState m_State; ///< current object state (Setup, Ready or Running) bool m_StopCommunication; ///< signal stop to thread /** mutex to control access to m_StopThread */ itk::FastMutexLock::Pointer m_StopCommunicationMutex; /** mutex to manage control flow of StopTracking() */ itk::FastMutexLock::Pointer m_CommunicationFinishedMutex; /** mutex to control access to m_State */ itk::FastMutexLock::Pointer m_StateMutex; /** mutex to control access to m_Socket */ // itk::FastMutexLock::Pointer m_SocketMutex; /** mutex to control access to m_LatestMessage */ itk::FastMutexLock::Pointer m_LatestMessageMutex; /** the hostname or ip of the device */ std::string m_Hostname; /** the port number of the device */ int m_PortNumber; /** the socket used to communicate with other IGTL devices */ igtl::Socket::Pointer m_Socket; /** the latest received message */ - igtl::MessageBase::Pointer m_LatestMessage; + igtl::TransformMessage::Pointer m_LatestMessage; private: /** creates worker thread that continuously polls interface for new messages */ itk::MultiThreader::Pointer m_MultiThreader; int m_ThreadID; ///< ID of polling thread }; } // namespace mitk #endif /* MITKIGTLDEVICE_H */ diff --git a/Modules/OpenIGTLink/mitkIGTLDeviceSource.cpp b/Modules/OpenIGTLink/mitkIGTLDeviceSource.cpp index 09f02dd519..c60167f398 100644 --- a/Modules/OpenIGTLink/mitkIGTLDeviceSource.cpp +++ b/Modules/OpenIGTLink/mitkIGTLDeviceSource.cpp @@ -1,179 +1,179 @@ /*=================================================================== 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 "mitkIGTLDeviceSource.h" #include "mitkIGTLDevice.h" #include "mitkIGTLMessage.h" //#include "mitkIGTTimeStamp.h" //#include "mitkIGTException.h" mitk::IGTLDeviceSource::IGTLDeviceSource() : mitk::IGTLMessageSource(), m_IGTLDevice(NULL) { } mitk::IGTLDeviceSource::~IGTLDeviceSource() { if (m_IGTLDevice.IsNotNull()) { if (m_IGTLDevice->GetState() == mitk::IGTLDevice::Running) { this->StopCommunication(); } if (m_IGTLDevice->GetState() == mitk::IGTLDevice::Ready) { this->Disconnect(); } m_IGTLDevice = NULL; } } void mitk::IGTLDeviceSource::GenerateData() { if (m_IGTLDevice.IsNull()) return; /* update output with message from the device */ IGTLMessage* msgOut = this->GetOutput(); assert(msgOut); - igtl::MessageBase::Pointer msgIn = igtl::MessageBase::New(); - if ( m_IGTLDevice->GetLatestMessage(msgIn) ) + igtl::MessageBase::Pointer msgIn = m_IGTLDevice->GetLatestMessage(); + if ( msgIn.IsNotNull() ) { assert(msgIn); msgOut->SetMessage(msgIn); msgOut->SetName(msgIn->GetDeviceName()); } else { MITK_ERROR("IGTLDeviceSource") << "Could not get the latest message."; } } void mitk::IGTLDeviceSource::SetIGTLDevice( mitk::IGTLDevice* igtlDevice ) { MITK_DEBUG << "Setting IGTLDevice to " << igtlDevice; if (this->m_IGTLDevice.GetPointer() != igtlDevice) { this->m_IGTLDevice = igtlDevice; this->CreateOutputs(); std::stringstream name; // create a human readable name for the source name << "OpenIGTLink Message Source"; this->SetName(name.str()); } } void mitk::IGTLDeviceSource::CreateOutputs() { //if outputs are set then delete them if (this->GetNumberOfOutputs() > 0) { for (int numOP = this->GetNumberOfOutputs() - 1; numOP >= 0; numOP--) this->RemoveOutput(numOP); this->Modified(); } //fill the outputs if a valid OpenIGTLink device is set if (m_IGTLDevice.IsNull()) return; this->SetNumberOfIndexedOutputs(1); if (this->GetOutput(0) == NULL) { DataObjectPointer newOutput = this->MakeOutput(0); this->SetNthOutput(0, newOutput); this->Modified(); } } void mitk::IGTLDeviceSource::Connect() { if (m_IGTLDevice.IsNull()) { throw std::invalid_argument("mitk::IGTLDeviceSource: " "No OpenIGTLink device set"); } if (this->IsConnected()) { return; } try { m_IGTLDevice->OpenConnection(); } catch (mitk::Exception &e) { throw std::runtime_error(std::string("mitk::IGTLDeviceSource: Could not open" "connection to OpenIGTLink device. Error: ") + e.GetDescription()); } } void mitk::IGTLDeviceSource::StartCommunication() { if (m_IGTLDevice.IsNull()) throw std::invalid_argument("mitk::IGTLDeviceSource: " "No OpenIGTLink device set"); if (m_IGTLDevice->GetState() == mitk::IGTLDevice::Running) return; if (m_IGTLDevice->StartCommunication() == false) throw std::runtime_error("mitk::IGTLDeviceSource: " "Could not start communication"); } void mitk::IGTLDeviceSource::Disconnect() { if (m_IGTLDevice.IsNull()) throw std::invalid_argument("mitk::IGTLDeviceSource: " "No OpenIGTLink device set"); if (m_IGTLDevice->CloseConnection() == false) throw std::runtime_error("mitk::IGTLDeviceSource: Could not close connection" " to OpenIGTLink device"); } void mitk::IGTLDeviceSource::StopCommunication() { if (m_IGTLDevice.IsNull()) throw std::invalid_argument("mitk::IGTLDeviceSource: " "No OpenIGTLink device set"); if (m_IGTLDevice->StopCommunication() == false) throw std::runtime_error("mitk::IGTLDeviceSource: " "Could not stop communicating"); } void mitk::IGTLDeviceSource::UpdateOutputInformation() { this->Modified(); // make sure that we need to be updated Superclass::UpdateOutputInformation(); } bool mitk::IGTLDeviceSource::IsConnected() { if (m_IGTLDevice.IsNull()) return false; return (m_IGTLDevice->GetState() == mitk::IGTLDevice::Ready) || (m_IGTLDevice->GetState() == mitk::IGTLDevice::Running); } bool mitk::IGTLDeviceSource::IsCommunicating() { if (m_IGTLDevice.IsNull()) return false; return m_IGTLDevice->GetState() == mitk::IGTLDevice::Running; } diff --git a/Modules/OpenIGTLink/mitkIGTLMessageCommon.cpp b/Modules/OpenIGTLink/mitkIGTLMessageCommon.cpp new file mode 100644 index 0000000000..3d9de63a30 --- /dev/null +++ b/Modules/OpenIGTLink/mitkIGTLMessageCommon.cpp @@ -0,0 +1,92 @@ +/*=================================================================== + +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 "mitkIGTLMessageCommon.h" +#include "mitkException.h" + +mitk::IGTLMessageCommon::IGTLMessageCommon() : itk::DataObject() +{ +} + + +mitk::IGTLMessageCommon::~IGTLMessageCommon() +{ +} + +bool mitk::IGTLMessageCommon::Clone(igtl::TransformMessage::Pointer clone, + igtl::TransformMessage::Pointer original) +{ + bool copySuccess = false; + + //copy all meta data + copySuccess = clone->Copy(original); + + if ( !copySuccess ) + return false; + + //copy all data that is important for this class + //copy the matrix + igtl::Matrix4x4 mat; + original->GetMatrix(mat); + clone->SetMatrix(mat); + + //copy the normals + float normals[3][3]; + original->GetNormals(normals); + clone->SetNormals(normals); + + //copy the position + float position[3]; + original->GetPosition(position); + clone->SetPosition(position); + + return true; +} + +igtl::MessageBase::Pointer mitk::IGTLMessageCommon::Clone(igtl::TransformMessage::Pointer original) +{ + bool copySuccess = false; + igtl::TransformMessage::Pointer clone_ = igtl::TransformMessage::New(); + + //initialize the clone +// clone = igtl::MessageBase::New(); + + //copy all meta data + copySuccess = clone_->Copy(original); + + if ( !copySuccess ) + return false; + + //copy all data that is important for this class + //copy the matrix + igtl::Matrix4x4 mat; + original->GetMatrix(mat); + clone_->SetMatrix(mat); + + //copy the normals + float normals[3][3]; + original->GetNormals(normals); + clone_->SetNormals(normals); + + //copy the position + float position[3]; + original->GetPosition(position); + clone_->SetPosition(position); + + return igtl::MessageBase::Pointer(clone_.GetPointer()); + +// return true; +} diff --git a/Modules/OpenIGTLink/mitkIGTLMessageCommon.h b/Modules/OpenIGTLink/mitkIGTLMessageCommon.h new file mode 100644 index 0000000000..30dc31e3ce --- /dev/null +++ b/Modules/OpenIGTLink/mitkIGTLMessageCommon.h @@ -0,0 +1,73 @@ +/*=================================================================== + +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 MITKIGTLMESSAGECOMMONH_HEADER_INCLUDED_ +#define MITKIGTLMESSAGECOMMONH_HEADER_INCLUDED_ + +#include +#include "MitkOpenIGTLinkExports.h" +#include +#include + +#include "igtlMessageBase.h" +#include "igtlTransformMessage.h" + +namespace mitk { + + /**Documentation + * \brief Helper class for copying OpenIGTLink messages. + * + * This class is a helper class for copying OpenIGTLink messages. + * + */ + class MITK_OPENIGTLINK_EXPORT IGTLMessageCommon : public itk::DataObject + { + public: + mitkClassMacro(IGTLMessageCommon, itk::DataObject); + itkFactorylessNewMacro(Self); + itkCloneMacro(Self); + + /** + * \brief Copies the content of a transform message original into another one + * (clone) + */ + static bool Clone(igtl::TransformMessage::Pointer clone, igtl::TransformMessage::Pointer original); + + /** + * \brief Copies the content of a transform message original into another one + * (clone) + */ + static igtl::MessageBase::Pointer Clone(igtl::TransformMessage::Pointer original); + + + protected: + IGTLMessageCommon(); + + /** + * Copy constructor internally used. + */ +// IGTLMessageCommon(const mitk::IGTLMessageCommon& toCopy); + + virtual ~IGTLMessageCommon(); + + private: + + }; + + +} // namespace mitk +#endif /* MITKIGTLMESSAGECOMMONH_HEADER_INCLUDED_ */