diff --git a/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp b/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp index 568c1a3691..a6ba169873 100644 --- a/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp +++ b/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp @@ -1,344 +1,345 @@ /*=================================================================== 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; } //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()); - + //cast the input message into the proper type 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; } + //debug output 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][3]); } //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()); + //cast the input message into the proper type + igtl::TrackingDataMessage* tdMsg = + (igtl::TrackingDataMessage*)(input->GetMessage().GetPointer()); + + //check if cast was successful + if ( !tdMsg ) + { + mitkThrow() << "Cast from igtl::MessageBase to igtl::TransformMessage " + << "failed! Please check the message."; + continue; + } //get the number of tracking data elements unsigned int numTrackingDataElements = tdMsg->GetNumberOfTrackingDataElements(); //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/OpenIGTLink/files.cmake b/Modules/OpenIGTLink/files.cmake index eeaa6a0d50..2ea75a3a97 100644 --- a/Modules/OpenIGTLink/files.cmake +++ b/Modules/OpenIGTLink/files.cmake @@ -1,8 +1,10 @@ set(CPP_FILES mitkIGTLClient.cpp mitkIGTLDevice.cpp mitkIGTLMessageSource.cpp mitkIGTLDeviceSource.cpp mitkIGTLMessage.cpp mitkIGTLMessageCommon.cpp + mitkIGTLMessageFactory.cpp + mitkIGTLMessageCloneHandler.h ) diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.cpp b/Modules/OpenIGTLink/mitkIGTLDevice.cpp index a6ebd6f597..a0623ec139 100644 --- a/Modules/OpenIGTLink/mitkIGTLDevice.cpp +++ b/Modules/OpenIGTLink/mitkIGTLDevice.cpp @@ -1,486 +1,475 @@ /*=================================================================== 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::TransformMessage::New(); + m_LatestMessage = igtl::MessageBase::New(); + + m_MessageFactory = mitk::IGTLMessageFactory::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; 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(); + igtl::MessageBase::Pointer curMessage; + curMessage = m_MessageFactory->CreateInstance(headerMsg); 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 = IGTLMessageCommon::Clone(m_LatestMessage, curMessage); -// m_LatestMessage->Copy(curMessage); -// copyCheck = m_LatestMessage->Copy(curMessage); + m_LatestMessage = m_MessageFactory->Clone(curMessage); m_LatestMessageMutex->Unlock(); - - //check if the copying was successful, otherwise invalidate the - //latest message - if ( !copyCheck ) - { - m_LatestMessageMutex->Lock(); - 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; } igtl::MessageBase::Pointer mitk::IGTLDevice::GetLatestMessage() { //copy the latest message into the given msg m_LatestMessageMutex->Lock(); igtl::MessageBase::Pointer msg = - mitk::IGTLMessageCommon::Clone(m_LatestMessage); + this->m_MessageFactory->Clone(m_LatestMessage); m_LatestMessageMutex->Unlock(); return msg; } diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.h b/Modules/OpenIGTLink/mitkIGTLDevice.h index b9bb40c9ee..6067d9e2e6 100644 --- a/Modules/OpenIGTLink/mitkIGTLDevice.h +++ b/Modules/OpenIGTLink/mitkIGTLDevice.h @@ -1,192 +1,195 @@ /*=================================================================== 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" +#include "mitkIGTLMessageFactory.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 */ 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::TransformMessage::Pointer m_LatestMessage; + igtl::MessageBase::Pointer m_LatestMessage; + /** the latest received message */ + mitk::IGTLMessageFactory::Pointer m_MessageFactory; 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/mitkIGTLMessageCloneHandler.h b/Modules/OpenIGTLink/mitkIGTLMessageCloneHandler.h new file mode 100644 index 0000000000..80432ee542 --- /dev/null +++ b/Modules/OpenIGTLink/mitkIGTLMessageCloneHandler.h @@ -0,0 +1,87 @@ +/*=================================================================== + +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 MITKIGTLMESSAGECLONE_H_ +#define MITKIGTLMESSAGECLONE_H_ + +#include "itkObject.h" +#include "mitkCommon.h" + +#include "igtlObject.h" +#include "igtlMacro.h" +#include "igtlSocket.h" +#include "igtlMessageBase.h" + +#include "MitkOpenIGTLinkExports.h" + +namespace mitk +{ + /**Documentation + * \brief Base class for clone handlers for igtl::MessageBase derived message + * types + * + * To enable new message types a clone function must be defined and added to the + * message factory. + * + */ + class MITK_OPENIGTLINK_EXPORT IGTLMessageCloneHandler: public itk::Object + { + public: + mitkClassMacro(IGTLMessageCloneHandler, itk::Object); + itkFactorylessNewMacro(Self); + itkCloneMacro(Self); + + public: + virtual igtl::MessageBase::Pointer Clone(igtl::MessageBase*) { return 0; } + + protected: + IGTLMessageCloneHandler() {} + ~IGTLMessageCloneHandler() {} +}; + +/** + * Description: + * The mitkIGTMessageCloneClassMacro() macro is to help developers to + * define message clone handler classes. It generates a chlid class of + * mitk::IGTLMessageCloneHandler. + * The developer only needs to implement Clone() after calling this macro. + * The following code shows how to define a handler that processes IMAGE message: + * + * mitkIGTMessageCloneClassMacro(igtl::ImageMessage, TestImageMessageHandler); + * igtl::MessageBase::Pointer + * TestImageMessageHandler::Clone(igtl::MessageBase * message) + * { + * // do something + * } + **/ +#define mitkIGTMessageCloneClassMacro(messagetype, classname) \ + class classname : public ::mitk::IGTLMessageCloneHandler \ + { \ + public: \ + mitkClassMacro(classname, mitk::IGTLMessageCloneHandler); \ + itkFactorylessNewMacro(Self); \ + itkCloneMacro(Self); \ + public: \ + virtual igtl::MessageBase::Pointer Clone(igtl::MessageBase*); \ + protected: \ + classname(){} \ + ~classname() {} \ + }; + + +} // namespace mitk + +#endif // MITKIGTLMESSAGECLONE_H_ diff --git a/Modules/OpenIGTLink/mitkIGTLMessageCommon.cpp b/Modules/OpenIGTLink/mitkIGTLMessageCommon.cpp index 3d9de63a30..238da87839 100644 --- a/Modules/OpenIGTLink/mitkIGTLMessageCommon.cpp +++ b/Modules/OpenIGTLink/mitkIGTLMessageCommon.cpp @@ -1,92 +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; +//bool mitk::IGTLMessageCommon::Clone(igtl::TransformMessage::Pointer clone, +// igtl::TransformMessage::Pointer original) +//{ +// bool copySuccess = false; - //copy all meta data - copySuccess = clone->Copy(original); +// //copy all meta data +// copySuccess = clone->Copy(original); - if ( !copySuccess ) - return false; +// 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 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 normals +// float normals[3][3]; +// original->GetNormals(normals); +// clone->SetNormals(normals); - //copy the position - float position[3]; - original->GetPosition(position); - clone->SetPosition(position); +// //copy the position +// float position[3]; +// original->GetPosition(position); +// clone->SetPosition(position); - return true; -} +// return true; +//} -igtl::MessageBase::Pointer mitk::IGTLMessageCommon::Clone(igtl::TransformMessage::Pointer original) -{ - bool copySuccess = false; - igtl::TransformMessage::Pointer clone_ = igtl::TransformMessage::New(); +//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(); +// //initialize the clone +//// clone = igtl::MessageBase::New(); - //copy all meta data - copySuccess = clone_->Copy(original); +// //copy all meta data +// copySuccess = clone_->Copy(original); - if ( !copySuccess ) - return false; +// 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 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 normals +// float normals[3][3]; +// original->GetNormals(normals); +// clone_->SetNormals(normals); - //copy the position - float position[3]; - original->GetPosition(position); - clone_->SetPosition(position); +// //copy the position +// float position[3]; +// original->GetPosition(position); +// clone_->SetPosition(position); - return igtl::MessageBase::Pointer(clone_.GetPointer()); +// return igtl::MessageBase::Pointer(clone_.GetPointer()); -// return true; -} +//// return true; +//} diff --git a/Modules/OpenIGTLink/mitkIGTLMessageCommon.h b/Modules/OpenIGTLink/mitkIGTLMessageCommon.h index 30dc31e3ce..baec32a263 100644 --- a/Modules/OpenIGTLink/mitkIGTLMessageCommon.h +++ b/Modules/OpenIGTLink/mitkIGTLMessageCommon.h @@ -1,73 +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) + * \brief Clones the input message and returns a igtl::MessageBase::Pointer + * to the clone */ - static bool Clone(igtl::TransformMessage::Pointer clone, igtl::TransformMessage::Pointer original); +// static igtl::MessageBase::Pointer Clone(igtl::TransformMessage::Pointer original); /** - * \brief Copies the content of a transform message original into another one - * (clone) + * \brief Clones the input message and returns a igtl::MessageBase::Pointer + * to the clone */ - static igtl::MessageBase::Pointer Clone(igtl::TransformMessage::Pointer original); +// static igtl::MessageBase::Pointer Clone(igtl::MessageBase::Pointer original); protected: IGTLMessageCommon(); /** * Copy constructor internally used. */ // IGTLMessageCommon(const mitk::IGTLMessageCommon& toCopy); virtual ~IGTLMessageCommon(); private: }; } // namespace mitk #endif /* MITKIGTLMESSAGECOMMONH_HEADER_INCLUDED_ */ diff --git a/Modules/OpenIGTLink/mitkIGTLMessageFactory.h b/Modules/OpenIGTLink/mitkIGTLMessageFactory.h new file mode 100644 index 0000000000..2d4e36c3a2 --- /dev/null +++ b/Modules/OpenIGTLink/mitkIGTLMessageFactory.h @@ -0,0 +1,159 @@ +/*=================================================================== + +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 MITKIGTLMESSAGEFACTORYH_HEADER_INCLUDED_ +#define MITKIGTLMESSAGEFACTORYH_HEADER_INCLUDED_ + +#include "MitkOpenIGTLinkExports.h" +#include "mitkCommon.h" + +#include "igtlMessageBase.h" +#include "igtlMessageHeader.h" + +#include "mitkIGTLMessageCloneHandler.h" + +namespace mitk { + /**Documentation + * \brief Factory class of supported OpenIGTLink message types + * + * This class is a factory class of supported OpenIGTLink message types to + * localize the message creation code. + * + */ + class MITK_OPENIGTLINK_EXPORT IGTLMessageFactory : public itk::Object + { + public: + mitkClassMacro(IGTLMessageFactory, itk::Object) + itkFactorylessNewMacro(Self); + itkCloneMacro(Self); + + +// virtual void PrintSelf(ostream& os); + + /** + * \brief Function pointer for storing New() static methods of + * igtl::MessageBase classes + */ + typedef igtl::MessageBase::Pointer (*PointerToMessageBaseNew)(); + + /** + * \brief Add message type name and pointer to IGTL message new function and + * the clone handler + * \param messageTypeName The name of the message type + * \param messageTypeNewPointer Function pointer to the message type new + * function (e.g. (PointerToMessageBaseNew)&igtl::ImageMessage::New ) + * \param msgClonePointer Function pointer to the message clone function + * (e.g. TBD ) + */ + void AddMessageType(std::string messageTypeName, + IGTLMessageFactory::PointerToMessageBaseNew messageTypeNewPointer, + mitk::IGTLMessageCloneHandler::Pointer cloneHandler); + + /** + * \brief Add message type name and pointer to IGTL message new function + * Usage: + * AddMessageType("IMAGE", (PointerToMessageBaseNew)&igtl::ImageMessage::New); + * \param messageTypeName The name of the message type + * \param messageTypeNewPointer Function pointer to the message type new + * function (e.g. (PointerToMessageBaseNew)&igtl::ImageMessage::New ) + */ + virtual void AddMessageNewMethod(std::string messageTypeName, + IGTLMessageFactory::PointerToMessageBaseNew messageTypeNewPointer); + + /** + * \brief Get pointer to message type new function, or NULL if the message + * type not registered + * Usage: + * igtl::MessageBase::Pointer message = GetMessageTypeNewPointer("IMAGE")(); + */ + virtual IGTLMessageFactory::PointerToMessageBaseNew GetMessageTypeNewPointer( + std::string messageTypeName); + + /** + * \brief Creates a new message according to the given header + * \param msg The message header that defines the type of the message + * This method checks the data type and creates a new message according to the + * type. + */ + igtl::MessageBase::Pointer CreateInstance(igtl::MessageHeader::Pointer msg); + + /** + * \brief Add a clone function for the specified message type + * \param msgTypeName The name of the message type + * \param msgClonePointer Function pointer to the message clone function + * (e.g. TBD ) + */ + virtual void AddMessageCloneHandler(std::string msgTypeName, + mitk::IGTLMessageCloneHandler::Pointer msgCloneHandler); + + /** + * \brief Get pointer to message type clone function, or NULL if the message type is + * not registered + * Usage: igtl::TransformMessage::Pointer original = igtl::TransformMessage::New(); + * igtl::MessageBase::Pointer message = GetMessageTypeClonePointer("IMAGE")(original); + */ + virtual mitk::IGTLMessageCloneHandler::Pointer GetCloneHandler(std::string messageTypeName); + + /** + * \brief Clones the given message according to the available clone methods + * \param msg The message that has to be cloned + * This method checks the data type and clones the message according to this + * type. + */ + igtl::MessageBase::Pointer Clone(igtl::MessageBase::Pointer msg); + + /*! Print all supported OpenIGTLink message types */ + // virtual void PrintAvailableMessageTypes(ostream& os, vtkIndent indent); + + /*! Create a new igtl::MessageBase instance from message type, delete previous igtl::MessageBase if's not NULL */ + // PlusStatus CreateInstance(const char* aIgtlMessageType, igtl::MessageBase::Pointer& aMessageBase); + + /*! + Generate and pack IGTL messages from tracked frame + \param packValidTransformsOnly Control whether or not to pack transform messages if they contain invalid transforms + \param igtlMessageTypes List of message types to generate for a client + \param igtMessages Output list for the generated IGTL messages + \param trackedFrame Input tracked frame data used for IGTL message generation + \param transformNames List of transform names to send + \param imageTransformName Image transform name used in the IGTL image message + \param transformRepository Transform repository used for computing the selected transforms + */ + // PlusStatus PackMessages(const std::vector& igtlMessageTypes, std::vector& igtMessages, TrackedFrame& trackedFrame, + // std::vector& transformNames, std::vector& imageStreams, bool packValidTransformsOnly, vtkTransformRepository* transformRepository=NULL); + + protected: + IGTLMessageFactory(); + virtual ~IGTLMessageFactory(); + + /** + * Map igt message types and the Clone() methods + */ + std::map m_CloneHandlers; + + /** + * Map igt message types and the New() static methods of igtl::MessageBase + * classes + */ + std::map m_NewMethods; + + private: + IGTLMessageFactory(const IGTLMessageFactory&); + // void operator=(const IGTLMessageFactory&); + + }; +} + +#endif