diff --git a/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp b/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp index 5cc584f098..e1fa269669 100644 --- a/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp +++ b/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp @@ -1,439 +1,436 @@ /*=================================================================== 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" #include 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 == nullptr) // if an input is set to nullptr, 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 nullptr; return static_cast(this->ProcessObject::GetInput(0)); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(unsigned int idx) const { if (this->GetNumberOfInputs() < 1) return nullptr; 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 nullptr; } 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::SetNumberOfExpectedOutputs( unsigned int numOutputs) { this->SetNumberOfIndexedOutputs(numOutputs); this->CreateOutputsForAllInputs(); } 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) == nullptr) { mitk::NavigationData::Pointer newOutput = mitk::NavigationData::New(); this->SetNthOutput(idx, newOutput); isModified = true; } } if (isModified) this->Modified(); } void mitk::IGTLMessageToNavigationDataFilter::GenerateTransformData() { const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //cast the input message into the proper type igtl::TransformMessage* tMsg = (igtl::TransformMessage*)(input->GetMessage().GetPointer()); //check if cast was successful if (!tMsg) { mitkThrow() << "Cast from igtl::MessageBase to igtl::TransformMessage " << "failed! Please check the message."; } /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); if (input->IsDataValid() == false) { output->SetDataValid(false); continue; } //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); //set the time stamp nd->SetIGTTimeStamp(input->GetTimeStamp()); //set the name nd->SetName(input->GetName()); output->Graft(nd); } } void mitk::IGTLMessageToNavigationDataFilter::GenerateTrackingDataData() { const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //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::TrackingDataMessage " << "failed! Please check the message."; } //get the number of tracking data elements unsigned int numTrackingDataElements = tdMsg->GetNumberOfTrackingDataElements(); if (!numTrackingDataElements) { MITK_ERROR("IGTLMsgToNavDataFilter") << "There are no tracking data " "elements in this message"; } /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); //invalidate the output output->SetDataValid(false); //check if the current index, all outputs that have no corresponding input //tracking element stay invalidated, the others are validated according to //the tracking element - if (input->IsDataValid() == false || i >= numTrackingDataElements) - { - continue; - } + if (input->IsDataValid() == false) { continue; } output->SetDataValid(true); //get the tracking data element which holds all the data igtl::TrackingDataElement::Pointer td; tdMsg->GetTrackingDataElement(i, td); //get the transformation matrix and convert it into an affinetransformation igtl::Matrix4x4 transformation_; td->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); mitk::NavigationData::Pointer nd; //check the rotation matrix vnl_matrix_fixed rotationMatrix = affineTransformation->GetMatrix().GetVnlMatrix(); vnl_matrix_fixed rotationMatrixTransposed = rotationMatrix.transpose(); // a quadratic matrix is a rotation matrix exactly when determinant is 1 // and transposed is inverse if (!Equal(1.0, vnl_det(rotationMatrix), 0.1) || !((rotationMatrix*rotationMatrixTransposed).is_identity(0.1))) { MITK_ERROR("IGTLMsgToNavDataFilter") << "tried to initialize NavData " << "with non-rotation matrix :" << rotationMatrix << " (Does your " "AffineTransform3D object include spacing? This is not " "supported by NavigationData objects!)"; nd = mitk::NavigationData::New(); } else { //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 nd = mitk::NavigationData::New(affineTransformation, true); } //set the time stamp nd->SetIGTTimeStamp(input->GetIGTTimeStamp()); //set the name - nd->SetName(input->GetName()); + nd->SetName(td->GetName()); output->Graft(nd); } } void mitk::IGTLMessageToNavigationDataFilter::GenerateQuaternionTrackingDataData() { const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //cast the input message into the proper type igtl::QuaternionTrackingDataMessage* tdMsg = (igtl::QuaternionTrackingDataMessage*)(input->GetMessage().GetPointer()); //check if cast was successful if (!tdMsg) { mitkThrow() << "Cast from igtl::MessageBase to igtl::TrackingDataMessage " << "failed! Please check the message."; } //get the number of tracking data elements unsigned int numTrackingDataElements = tdMsg->GetNumberOfQuaternionTrackingDataElements(); if (!numTrackingDataElements) { MITK_ERROR("IGTLMsgToNavDataFilter") << "There are no tracking data " "elements in this message"; } /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); //invalidate the output output->SetDataValid(false); //check if the current index, all outputs that have no corresponding input //tracking element stay invalidated, the others are validated according to //the tracking element if (input->IsDataValid() == false || i >= numTrackingDataElements) { continue; } output->SetDataValid(true); //get the tracking data element which holds all the data igtl::QuaternionTrackingDataElement::Pointer td; tdMsg->GetQuaternionTrackingDataElement(i, 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]); quaternion.put(3, quaternion_[3]); 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); //set the time stamp output->SetIGTTimeStamp(input->GetTimeStamp()); //set the name output->SetName(td->GetName()); //there is no explicit covarience matrix output->SetCovErrorMatrix(mitk::NavigationData::CovarianceMatrixType()); } } void mitk::IGTLMessageToNavigationDataFilter::GenerateData() { //get the IGTLMessage from the previous filter const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //check if the message is valid, if it is not valid we do not generate new //outputs if (!input->IsDataValid()) { MITK_DEBUG("IGTLMessageToNavigationDataFilter") << "Input data is invalid."; return; } //get the message type const char* msgType = input->GetIGTLMessageType(); //check if the IGTL message has the proper type if (strcmp(msgType, "TRANSFORM") == 0) { this->GenerateTransformData(); } else if (strcmp(msgType, "TDATA") == 0) { this->GenerateTrackingDataData(); } else if (strcmp(msgType, "QTDATA") == 0) { this->GenerateQuaternionTrackingDataData(); } else { //the message has another type //ignore MITK_INFO("IGTLMessageToNavigationDataFilter") << "The input has a unknown " << "message type: " << msgType; } } 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/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp index c940bf8a72..bed9cf1f82 100644 --- a/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp +++ b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp @@ -1,348 +1,323 @@ /*=================================================================== 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 "mitkNavigationDataToIGTLMessageFilter.h" #include "igtlQuaternionTrackingDataMessage.h" #include "igtlTrackingDataMessage.h" #include "igtlTransformMessage.h" #include "igtlPositionMessage.h" #include #include mitk::NavigationDataToIGTLMessageFilter::NavigationDataToIGTLMessageFilter() { mitk::IGTLMessage::Pointer output = mitk::IGTLMessage::New(); this->SetNumberOfRequiredOutputs(1); this->SetNthOutput(0, output.GetPointer()); this->SetNumberOfRequiredInputs(1); // m_OperationMode = Mode3D; m_CurrentTimeStep = 0; // m_RingBufferSize = 50; //the default ring buffer size // m_NumberForMean = 100; } mitk::NavigationDataToIGTLMessageFilter::~NavigationDataToIGTLMessageFilter() { } void mitk::NavigationDataToIGTLMessageFilter::GenerateData() { switch (m_OperationMode) { case ModeSendQTDataMsg: this->GenerateDataModeSendQTDataMsg(); break; case ModeSendTDataMsg: this->GenerateDataModeSendTDataMsg(); break; case ModeSendQTransMsg: this->GenerateDataModeSendQTransMsg(); break; case ModeSendTransMsg: this->GenerateDataModeSendTransMsg(); break; default: break; } } void mitk::NavigationDataToIGTLMessageFilter::SetInput(const NavigationData* nd) { // Process object is not const-correct so the const_cast is required here this->ProcessObject::SetNthInput(0, const_cast(nd)); this->CreateOutputsForAllInputs(); } void mitk::NavigationDataToIGTLMessageFilter::SetInput(unsigned int idx, const NavigationData* nd) { // Process object is not const-correct so the const_cast is required here this->ProcessObject::SetNthInput(idx, const_cast(nd)); this->CreateOutputsForAllInputs(); } const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(void) { if (this->GetNumberOfInputs() < 1) return nullptr; return static_cast(this->ProcessObject::GetInput(0)); } const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(unsigned int idx) { if (this->GetNumberOfInputs() < 1) return nullptr; return static_cast(this->ProcessObject::GetInput(idx)); } void mitk::NavigationDataToIGTLMessageFilter::CreateOutputsForAllInputs() { switch (m_OperationMode) { case ModeSendQTDataMsg: // create one message output for all navigation data inputs - this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs()); + this->SetNumberOfIndexedOutputs(1); // set the type for this filter this->SetType("QTDATA"); break; case ModeSendTDataMsg: // create one message output for all navigation data inputs - this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs()); + this->SetNumberOfIndexedOutputs(1); // set the type for this filter this->SetType("TDATA"); break; case ModeSendQTransMsg: // create one message output for all navigation data input together - this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs()); + this->SetNumberOfIndexedOutputs(1); // set the type for this filter this->SetType("POSITION"); break; case ModeSendTransMsg: - // create one message output for all navigation data input together + // create one message output for each navigation data input this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs()); // set the type for this filter this->SetType("TRANS"); break; default: break; } for (unsigned int idx = 0; idx < this->GetNumberOfIndexedOutputs(); ++idx) { if (this->GetOutput(idx) == nullptr) { DataObjectPointer newOutput = this->MakeOutput(idx); this->SetNthOutput(idx, newOutput); } this->Modified(); } } void ConvertAffineTransformationIntoIGTLMatrix(mitk::AffineTransform3D* trans, igtl::Matrix4x4 igtlTransform) { const mitk::AffineTransform3D::MatrixType& matrix = trans->GetMatrix(); mitk::Vector3D position = trans->GetOffset(); //copy the data into a matrix type that igtl understands for (unsigned int r = 0; r < 3; r++) { for (unsigned int c = 0; c < 3; c++) { igtlTransform[r][c] = matrix(r, c); } igtlTransform[r][3] = position[r]; } for (unsigned int c = 0; c < 3; c++) { igtlTransform[3][c] = 0.0; } igtlTransform[3][3] = 1.0; } void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendQTransMsg() { // for each output message for (unsigned int i = 0; i < this->GetNumberOfIndexedInputs(); ++i) { mitk::IGTLMessage* output = this->GetOutput(i); assert(output); const mitk::NavigationData* input = this->GetInput(i); assert(input); // do not add navigation data to message if input is invalid if (input->IsDataValid() == false) continue; //get the navigation data components mitk::NavigationData::PositionType pos = input->GetPosition(); mitk::NavigationData::OrientationType ori = input->GetOrientation(); //insert this information into the message igtl::PositionMessage::Pointer posMsg = igtl::PositionMessage::New(); posMsg->SetPosition(pos[0], pos[1], pos[2]); posMsg->SetQuaternion(ori[0], ori[1], ori[2], ori[3]); igtl::TimeStamp::Pointer timestamp = ConvertToIGTLTimeStamp(input->GetIGTTimeStamp()); posMsg->SetTimeStamp(timestamp); posMsg->SetDeviceName(input->GetName()); posMsg->Pack(); //add the igtl message to the mitk::IGTLMessage output->SetMessage(posMsg.GetPointer()); } } void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendTransMsg() { // for each output message for (unsigned int i = 0; i < this->GetNumberOfIndexedInputs(); ++i) { mitk::IGTLMessage* output = this->GetOutput(i); assert(output); const mitk::NavigationData* input = this->GetInput(i); assert(input); // do not add navigation data to message if input is invalid if (input->IsDataValid() == false) continue; //get the navigation data components mitk::AffineTransform3D::Pointer transform = input->GetAffineTransform3D(); mitk::NavigationData::PositionType position = transform->GetOffset(); //convert the transform into a igtl type igtl::Matrix4x4 igtlTransform; ConvertAffineTransformationIntoIGTLMatrix(transform, igtlTransform); //insert this information into the message igtl::TransformMessage::Pointer transMsg = igtl::TransformMessage::New(); transMsg->SetMatrix(igtlTransform); transMsg->SetPosition(position[0], position[1], position[2]); igtl::TimeStamp::Pointer timestamp = ConvertToIGTLTimeStamp(input->GetIGTTimeStamp()); transMsg->SetTimeStamp(timestamp); transMsg->SetDeviceName(input->GetName()); transMsg->Pack(); //add the igtl message to the mitk::IGTLMessage output->SetMessage(transMsg.GetPointer()); } } igtl::TimeStamp::Pointer mitk::NavigationDataToIGTLMessageFilter::ConvertToIGTLTimeStamp(double IGTTimeStamp) { igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New(); timestamp->SetTime(IGTTimeStamp / 1000, (int)(IGTTimeStamp) % 1000); return timestamp; } void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendQTDataMsg() { mitk::IGTLMessage* output = this->GetOutput(); assert(output); //create a output igtl message igtl::QuaternionTrackingDataMessage::Pointer qtdMsg = igtl::QuaternionTrackingDataMessage::New(); mitk::NavigationData::PositionType pos; mitk::NavigationData::OrientationType ori; for (unsigned int index = 0; index < this->GetNumberOfIndexedInputs(); index++) { const mitk::NavigationData* nd = GetInput(index); assert(nd); //get the navigation data components pos = nd->GetPosition(); ori = nd->GetOrientation(); //insert the information into the tracking element igtl::QuaternionTrackingDataElement::Pointer tde = igtl::QuaternionTrackingDataElement::New(); tde->SetPosition(pos[0], pos[1], pos[2]); tde->SetQuaternion(ori[0], ori[1], ori[2], ori[3]); tde->SetName(nd->GetName()); //insert this element into the tracking data message qtdMsg->AddQuaternionTrackingDataElement(tde); MITK_INFO << ConvertToIGTLTimeStamp(nd->GetIGTTimeStamp()); } qtdMsg->Pack(); //add the igtl message to the mitk::IGTLMessage output->SetMessage(qtdMsg.GetPointer()); } void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendTDataMsg() { - bool isValidData = true; - mitk::IGTLMessage* output = this->GetOutput(); - assert(output); - - //create a output igtl message igtl::TrackingDataMessage::Pointer tdMsg = igtl::TrackingDataMessage::New(); + mitk::IGTLMessage* output = this->GetOutput(0); + assert(output); - mitk::AffineTransform3D::Pointer transform; - Vector3D position; - igtl::Matrix4x4 igtlTransform; - vnl_matrix_fixed rotationMatrix; - vnl_matrix_fixed rotationMatrixTransposed; - - for (unsigned int index = 0; index < this->GetNumberOfIndexedInputs(); index++) + // for each output message + for (unsigned int i = 0; i < this->GetNumberOfIndexedInputs(); ++i) { - const mitk::NavigationData* nd = GetInput(index); - assert(nd); - - //create a new tracking element - igtl::TrackingDataElement::Pointer tde = igtl::TrackingDataElement::New(); + const mitk::NavigationData* input = this->GetInput(i); + assert(input); + // do not add navigation data to message if input is invalid + if (input->IsDataValid() == false) + continue; //get the navigation data components - transform = nd->GetAffineTransform3D(); - position = transform->GetOffset(); - - //check the rotation matrix - rotationMatrix = transform->GetMatrix().GetVnlMatrix(); - rotationMatrixTransposed = rotationMatrix.transpose(); - // a quadratic matrix is a rotation matrix exactly when determinant is 1 - // and transposed is inverse - if (!Equal(1.0, vnl_det(rotationMatrix), 0.1) - || !((rotationMatrix*rotationMatrixTransposed).is_identity(0.1))) - { - //the rotation matrix is not valid! => invalidate the current element - isValidData = false; - } + mitk::AffineTransform3D::Pointer transform = input->GetAffineTransform3D(); + mitk::NavigationData::PositionType position = transform->GetOffset(); //convert the transform into a igtl type + igtl::Matrix4x4 igtlTransform; ConvertAffineTransformationIntoIGTLMatrix(transform, igtlTransform); - //fill the tracking element with life + //insert this information into the message + igtl::TrackingDataElement::Pointer tde = igtl::TrackingDataElement::New(); tde->SetMatrix(igtlTransform); tde->SetPosition(position[0], position[1], position[2]); - std::stringstream name; - name << nd->GetName(); - if (name.rdbuf()->in_avail() == 0) - { - name << "TrackingTool" << index; - } - tde->SetName(name.str().c_str()); - - //insert this element into the tracking data message + tde->SetName(input->GetName()); + tde->SetType(igtl::TrackingDataElement::TYPE_6D); tdMsg->AddTrackingDataElement(tde); - - //copy the time stamp - igtl::TimeStamp::Pointer timestamp = ConvertToIGTLTimeStamp(nd->GetIGTTimeStamp()); - tdMsg->SetTimeStamp(timestamp); } + + //use time stamp from first input + igtl::TimeStamp::Pointer timestamp = ConvertToIGTLTimeStamp(this->GetInput(0)->GetIGTTimeStamp()); + tdMsg->SetTimeStamp(timestamp); + //tdMsg->SetDeviceName("MITK OpenIGTLink Connection"); tdMsg->Pack(); - //add the igtl message to the mitk::IGTLMessage + tdMsg->SetDeviceName("MITK OpenIGTLink Source"); output->SetMessage(tdMsg.GetPointer()); - output->SetDataValid(isValidData); } void mitk::NavigationDataToIGTLMessageFilter::SetOperationMode(OperationMode mode) { m_OperationMode = mode; this->Modified(); } void mitk::NavigationDataToIGTLMessageFilter::ConnectTo( mitk::NavigationDataSource* UpstreamFilter) { for (DataObjectPointerArraySizeType i = 0; i < UpstreamFilter->GetNumberOfOutputs(); i++) { this->SetInput(i, UpstreamFilter->GetOutput(i)); } } diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp index 3c26e8fcbf..d85724a106 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp @@ -1,638 +1,524 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkOpenIGTLinkTrackingDevice.h" #include "mitkOpenIGTLinkTrackingTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include "mitkTrackingTypes.h" #include #include #include #include #include #include //sleep headers #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::OpenIGTLinkTrackingDevice::OpenIGTLinkTrackingDevice() : mitk::TrackingDevice(), m_UpdateRate(60) { //set the type of this tracking device this->m_Data = mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection(); m_OpenIGTLinkClient = mitk::IGTLClient::New(true); m_OpenIGTLinkClient->SetName("OpenIGTLink Tracking Device"); m_OpenIGTLinkClient->EnableNoBufferingMode(false); - m_IGTLDeviceSource = mitk::IGTLTransformDeviceSource::New(); + m_IGTLDeviceSource = mitk::IGTLTrackingDataDeviceSource::New(); m_IGTLDeviceSource->SetIGTLDevice(m_OpenIGTLinkClient); } mitk::OpenIGTLinkTrackingDevice::~OpenIGTLinkTrackingDevice() { } int mitk::OpenIGTLinkTrackingDevice::GetPortNumber() { return m_OpenIGTLinkClient->GetPortNumber(); } bool mitk::OpenIGTLinkTrackingDevice::AutoDetectToolsAvailable() { return true; } mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::AutoDetectTools() { mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); if (m_OpenIGTLinkClient->GetPortNumber() == -1) { MITK_WARN << "Connection not initialized, aborting (invalid port number)."; return mitk::NavigationToolStorage::New(); } //open connection try { m_IGTLDeviceSource->Connect(); m_IGTLDeviceSource->StartCommunication(); } catch (std::runtime_error &e) { MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to connect: " << e.what(); return mitk::NavigationToolStorage::New(); } //get a message to find out type + m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::UNKNOWN); mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(100); const char* msgType = receivedMessage->GetIGTLMessageType(); if (std::string(msgType).empty()) { MITK_INFO << "Did not receive a message. Do you have to start the stream manually at the server?"; MITK_INFO << "Waiting for 10 seconds ..."; receivedMessage = ReceiveMessage(10000); msgType = receivedMessage->GetIGTLMessageType(); } - + MITK_INFO << "################# got message type: " << msgType; mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type = GetMessageTypeFromString(msgType); - - returnValue = DiscoverToolsAndConvertToNavigationTools(type); - /* switch (type) { + case UNKNOWN: + m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::UNKNOWN); + break; case TDATA: - returnValue = DiscoverToolsFromTData(dynamic_cast(receivedMessage->GetMessage().GetPointer())); + m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::TDATA); break; case QTDATA: - returnValue = DiscoverToolsFromQTData(dynamic_cast(receivedMessage->GetMessage().GetPointer())); + m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::QTDATA); break; case TRANSFORM: - returnValue = DiscoverToolsFromTransform(); + m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::TRANSFORM); break; - default: - MITK_INFO << "Server does not send tracking data or received data is not of a compatible type. (Received type: " << msgType << ")"; } - */ + returnValue = DiscoverToolsAndConvertToNavigationTools(type); //close connection try { m_IGTLDeviceSource->StopCommunication(); m_IGTLDeviceSource->Disconnect(); } catch (std::runtime_error &e) { MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to disconnect: " << e.what(); return mitk::NavigationToolStorage::New(); } return returnValue; } mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsAndConvertToNavigationTools(mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type, int NumberOfMessagesToWait) { MITK_INFO << "Start discovering tools by " << type << " messages"; mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); std::map toolNameMap; for (int j = 0; jUpdate(); - igtl::TransformMessage::Pointer msg = dynamic_cast(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer()); - if (msg == nullptr || msg.IsNull()) - { - MITK_INFO << "Received message could not be casted to TransformMessage. Skipping.."; - continue; - } - - int count = toolNameMap[msg->GetDeviceName()]; - if (count == 0) - { - //MITK_WARN << "ADDED NEW TOOL TO TOOLCHAIN: " << msg->GetDeviceName() << " - 1"; - toolNameMap[msg->GetDeviceName()] = 1; - } - else + switch (type) { - toolNameMap[msg->GetDeviceName()]++; - //MITK_WARN << "INCREMENTED TOOL COUNT IN TOOLCHAIN: " << msg->GetDeviceName() << " - " << toolNameMap[msg->GetDeviceName()]; + case TRANSFORM: + { + igtl::TransformMessage::Pointer msg = dynamic_cast(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer()); + if (msg == nullptr || msg.IsNull()) { MITK_INFO << "Received message is invalid / null. Skipping.."; continue; } + int count = toolNameMap[msg->GetDeviceName()]; + if (count == 0) { toolNameMap[msg->GetDeviceName()] = 1; } + else { toolNameMap[msg->GetDeviceName()]++; } + } + break; + case TDATA: + { + igtl::TrackingDataMessage::Pointer msg = dynamic_cast(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer()); + if (msg == nullptr || msg.IsNull()) { MITK_INFO << "Received message is invalid / null. Skipping.."; continue; } + for (int k = 0; k < msg->GetNumberOfTrackingDataElements(); k++) + { + igtl::TrackingDataElement::Pointer tde; + msg->GetTrackingDataElement(k, tde); + if (tde.IsNotNull()) + { + int count = toolNameMap[tde->GetName()]; + if (count == 0) { toolNameMap[tde->GetName()] = 1; } + else { toolNameMap[tde->GetName()]++; } + } + } + } + break; + default: + MITK_WARN << "Only TRANSFORM and TDATA is currently supported, skipping!"; + break; } } int i = 0; for (std::map::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it) { MITK_INFO << "Found tool: " << it->first; std::stringstream name; name << it->first; std::stringstream identifier; identifier << "AutoDetectedTool-" << i; i++; mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name.str(), identifier.str()); returnValue->AddTool(newTool); } return returnValue; } std::string mitk::OpenIGTLinkTrackingDevice::GetHostname() { return m_OpenIGTLinkClient->GetHostname(); } void mitk::OpenIGTLinkTrackingDevice::SetPortNumber(int portNumber) { m_OpenIGTLinkClient->SetPortNumber(portNumber); } void mitk::OpenIGTLinkTrackingDevice::SetHostname(std::string hostname) { m_OpenIGTLinkClient->SetHostname(hostname); } bool mitk::OpenIGTLinkTrackingDevice::IsDeviceInstalled() { return true; } mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::AddTool(const char* toolName, const char* fileName) { mitk::OpenIGTLinkTrackingTool::Pointer t;// = mitk::OpenIGTLinkTrackingTool::New(); //TODO: Implement if (this->InternalAddTool(t) == false) return nullptr; return t.GetPointer(); } bool mitk::OpenIGTLinkTrackingDevice::InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::OpenIGTLinkTrackingDevice::DiscoverTools(int waitingTime) { if (m_OpenIGTLinkClient->GetPortNumber() == -1) { MITK_WARN << "Connection not initialized, aborting (invalid port number)."; return false; } try { m_IGTLDeviceSource->Connect(); m_IGTLDeviceSource->StartCommunication(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(waitingTime); //check the tracking stream for the number and type of tools //igtl::MessageBase::Pointer receivedMessage = m_OpenIGTLinkClient->GetNextMessage(); if (receivedMessage.IsNull()) { MITK_WARN << "No message was received. Is there really a server?"; return false; } else if (!receivedMessage->IsDataValid()) { MITK_WARN << "Received invalid message."; return false; } const char* msgType = receivedMessage->GetIGTLMessageType(); mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type = GetMessageTypeFromString(msgType); - mitk::NavigationToolStorage::Pointer foundTools; - switch (type) - { - case TDATA: - foundTools = DiscoverToolsFromTData(dynamic_cast(receivedMessage->GetMessage().GetPointer())); - break; - case QTDATA: - foundTools = DiscoverToolsFromQTData(dynamic_cast(receivedMessage->GetMessage().GetPointer())); - break; - case TRANSFORM: - foundTools = DiscoverToolsFromTransform(); - break; - default: - MITK_INFO << "Server does not send tracking data. Received data is not of a compatible type. Received type: " << msgType; - return false; - } + mitk::NavigationToolStorage::Pointer foundTools = this->DiscoverToolsAndConvertToNavigationTools(type); if (foundTools.IsNull() || (foundTools->GetToolCount() == 0)) { return false; } for (int i = 0; i < foundTools->GetToolCount(); i++) { AddNewToolForName(foundTools->GetTool(i)->GetToolName(), i); } MITK_INFO << "Found tools: " << foundTools->GetToolCount(); return true; } mitk::IGTLMessage::Pointer mitk::OpenIGTLinkTrackingDevice::ReceiveMessage(int waitingTime) { mitk::IGTLMessage::Pointer receivedMessage; //send a message to the server: start tracking stream mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory(); std::string message[2] = {"STT_QTDATA","STT_TDATA"}; for (int i = 0; i < 2; i++) { igtl::MessageBase::Pointer sttMsg = msgFactory->CreateInstance(message[i]); //TODO: Fix this to dynamically get this from GUI ((igtl::StartTrackingDataMessage*)sttMsg.GetPointer())->SetResolution(m_UpdateRate); m_OpenIGTLinkClient->SendMessage(mitk::IGTLMessage::New(sttMsg)); } std::chrono::high_resolution_clock::time_point time = std::chrono::high_resolution_clock::now(); std::chrono::milliseconds d = std::chrono::milliseconds(waitingTime); while (!(receivedMessage.IsNotNull() && receivedMessage->IsDataValid())) { m_IGTLDeviceSource->Update(); receivedMessage = m_IGTLDeviceSource->GetOutput(); if ((time + d) < std::chrono::high_resolution_clock::now()) break; std::this_thread::sleep_for(std::chrono::milliseconds(100)); } return receivedMessage; - -} - -mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTData(igtl::TrackingDataMessage::Pointer tdMsg) -{ - mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); - MITK_INFO << "Start discovering tools by TDATA messages"; - if (tdMsg == nullptr) - { - MITK_WARN << "Message was not a TrackingDataMessage, aborting!"; - return returnValue; - } - - int numberOfTools = tdMsg->GetNumberOfTrackingDataElements(); - MITK_INFO << "Found " << numberOfTools << " tools"; - for (int i = 0; i < numberOfTools; i++) - { - igtl::TrackingDataElement::Pointer currentTrackingData; - tdMsg->GetTrackingDataElement(i, currentTrackingData); - std::string name = currentTrackingData->GetName(); - - std::stringstream identifier; - identifier << "AutoDetectedTool-" << i; - i++; - - mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name, identifier.str()); - - returnValue->AddTool(newTool); - } - - m_IGTLDeviceSource->StopCommunication(); - SetState(Ready); - return returnValue; -} - -mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg) -{ - mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); - MITK_INFO << "Start discovering tools by QTDATA messages"; - if (msg == nullptr) - { - MITK_WARN << "Message was not a QuaternionTrackingDataMessage, aborting!"; - return returnValue; - } - int numberOfTools = msg->GetNumberOfQuaternionTrackingDataElements(); - MITK_INFO << "Found " << numberOfTools << " tools"; - for (int i = 0; i < numberOfTools; i++) - { - igtl::QuaternionTrackingDataElement::Pointer currentTrackingData; - msg->GetQuaternionTrackingDataElement(i, currentTrackingData); - std::string name = currentTrackingData->GetName(); - - std::stringstream identifier; - identifier << "AutoDetectedTool-" << i; - i++; - - mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name, identifier.str()); - - returnValue->AddTool(newTool); - } - m_IGTLDeviceSource->StopCommunication(); - SetState(Ready); - return returnValue; } void mitk::OpenIGTLinkTrackingDevice::AddNewToolForName(std::string name, int i) { mitk::OpenIGTLinkTrackingTool::Pointer newTool = mitk::OpenIGTLinkTrackingTool::New(); if (name == "") //if no name was given create a default name { std::stringstream defaultName; defaultName << "OpenIGTLinkTool#" << i; name = defaultName.str(); } MITK_INFO << "Added tool " << name << " to tracking device."; newTool->SetToolName(name); InternalAddTool(newTool); } -mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTransform(int NumberOfMessagesToWait) -{ - MITK_INFO << "Start discovering tools by TRANSFORM messages"; - mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); - std::map toolNameMap; - - for (int j=0; jUpdate(); - igtl::TransformMessage::Pointer msg = dynamic_cast(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer()); - if (msg == nullptr || msg.IsNull()) - { - MITK_INFO << "Received message could not be casted to TransformMessage. Skipping.."; - continue; - } - - int count = toolNameMap[msg->GetDeviceName()]; - if (count == 0) - { - //MITK_WARN << "ADDED NEW TOOL TO TOOLCHAIN: " << msg->GetDeviceName() << " - 1"; - toolNameMap[msg->GetDeviceName()] = 1; - } - else - { - toolNameMap[msg->GetDeviceName()]++; - //MITK_WARN << "INCREMENTED TOOL COUNT IN TOOLCHAIN: " << msg->GetDeviceName() << " - " << toolNameMap[msg->GetDeviceName()]; - } - } - - int i = 0; - for (std::map::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it) - { - MITK_INFO << "Found tool: " << it->first; - - std::stringstream name; - name << it->first; - - std::stringstream identifier; - identifier << "AutoDetectedTool-" << i; - i++; - - mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name.str(), identifier.str()); - - returnValue->AddTool(newTool); - } - - return returnValue; -} - mitk::NavigationTool::Pointer mitk::OpenIGTLinkTrackingDevice::ConstructDefaultOpenIGTLinkTool(std::string name, std::string identifier) { mitk::DataNode::Pointer newNode = mitk::DataNode::New(); newNode->SetName(name); mitk::Surface::Pointer myCone = mitk::Surface::New(); vtkConeSource *vtkData = vtkConeSource::New(); vtkData->SetAngle(5.0); vtkData->SetResolution(50); vtkData->SetHeight(6.0f); vtkData->SetRadius(2.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); myCone->SetVtkPolyData(vtkData->GetOutput()); vtkData->Delete(); newNode->SetData(myCone); mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); newTool->SetDataNode(newNode); newTool->SetIdentifier(identifier); newTool->SetTrackingDeviceType(mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection().Line); return newTool; } void mitk::OpenIGTLinkTrackingDevice::UpdateTools() { if (this->GetState() != Tracking) { MITK_ERROR << "Method was called in the wrong state, something went wrong!"; return; } m_IGTLMsgToNavDataFilter->Update(); - mitk::NavigationData::Pointer currentNavData = m_IGTLMsgToNavDataFilter->GetOutput(); - const char* name = currentNavData->GetName(); - //MITK_WARN << name; - - for (int i = 0; i < m_AllTools.size(); i++) + for (int j = 0; j < m_IGTLMsgToNavDataFilter->GetNumberOfIndexedOutputs(); j++) { - if (strcmp(m_AllTools.at(i)->GetToolName(), name) == 0) + mitk::NavigationData::Pointer currentNavData = m_IGTLMsgToNavDataFilter->GetOutput(j); + const char* name = currentNavData->GetName(); + for (int i = 0; i < m_AllTools.size(); i++) { - m_AllTools.at(i)->SetDataValid(currentNavData->IsDataValid()); - m_AllTools.at(i)->SetPosition(currentNavData->GetPosition()); - m_AllTools.at(i)->SetOrientation(currentNavData->GetOrientation()); - m_AllTools.at(i)->SetIGTTimeStamp(currentNavData->GetIGTTimeStamp()); + if (strcmp(m_AllTools.at(i)->GetToolName(), name) == 0) + { + m_AllTools.at(i)->SetDataValid(currentNavData->IsDataValid()); + m_AllTools.at(i)->SetPosition(currentNavData->GetPosition()); + m_AllTools.at(i)->SetOrientation(currentNavData->GetOrientation()); + m_AllTools.at(i)->SetIGTTimeStamp(currentNavData->GetIGTTimeStamp()); + } } } } bool mitk::OpenIGTLinkTrackingDevice::StartTracking() { //check tracking state if (this->GetState() != Ready) { MITK_WARN << "Cannot start tracking, device is not ready!"; return false; } try { m_IGTLDeviceSource->StartCommunication(); //send a message to the server: start tracking stream mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory(); std::string message = "STT_TDATA"; //m_OpenIGTLinkClient->SendMessage(msgFactory->CreateInstance(message)); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while starting communication: " << e.what(); return false; } //create internal igtl pipeline m_IGTLMsgToNavDataFilter = mitk::IGTLMessageToNavigationDataFilter::New(); m_IGTLMsgToNavDataFilter->SetNumberOfExpectedOutputs(this->GetToolCount()); m_IGTLMsgToNavDataFilter->ConnectTo(m_IGTLDeviceSource); //connect itk events typedef itk::SimpleMemberCommand< mitk::OpenIGTLinkTrackingDevice > CurCommandType; CurCommandType::Pointer messageReceivedCommand = CurCommandType::New(); messageReceivedCommand->SetCallbackFunction(this, &mitk::OpenIGTLinkTrackingDevice::UpdateTools); m_MessageReceivedObserverTag = m_OpenIGTLinkClient->AddObserver(mitk::MessageReceivedEvent(), messageReceivedCommand); m_OpenIGTLinkClient->EnableNoBufferingMode(true); this->SetState(Tracking); return true; } bool mitk::OpenIGTLinkTrackingDevice::StopTracking() { //check tracking state if (this->GetState() != Tracking) { MITK_WARN << "Cannot open connection, device is already connected!"; return false; } m_OpenIGTLinkClient->RemoveObserver(m_MessageReceivedObserverTag); //disconnect itk events try { m_IGTLDeviceSource->StopCommunication(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while stopping communication: " << e.what(); return false; } m_OpenIGTLinkClient->EnableNoBufferingMode(false); this->SetState(Ready); return true; } unsigned int mitk::OpenIGTLinkTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::GetTool(unsigned int toolNumber) const { if (toolNumber >= this->GetToolCount()) return nullptr; else return this->m_AllTools[toolNumber]; } bool mitk::OpenIGTLinkTrackingDevice::OpenConnection() { //check tracking state if (this->GetState() != Setup) { MITK_WARN << "Cannot open connection, device is already connected!"; return false; } try { m_IGTLDeviceSource->Connect(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } this->SetState(Ready); return true; } bool mitk::OpenIGTLinkTrackingDevice::CloseConnection() { //check tracking state if (this->GetState() != Ready) { MITK_WARN << "Cannot close connection, device is in the wrong state!"; return false; } try { m_IGTLDeviceSource->Disconnect(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to disconnect: " << e.what(); return false; } this->SetState(Setup); return true; } std::vector mitk::OpenIGTLinkTrackingDevice::GetAllTools() { return this->m_AllTools; } mitk::OpenIGTLinkTrackingDevice::TrackingMessageType mitk::OpenIGTLinkTrackingDevice::GetMessageTypeFromString(const char* messageTypeString) { if (strcmp(messageTypeString, "TDATA") == 0) { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TDATA; } else if (strcmp(messageTypeString, "QTDATA") == 0) { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::QTDATA; } else if (strcmp(messageTypeString, "TRANSFORM") == 0) { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TRANSFORM; } else { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::UNKNOWN; } } diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h index e040ce5ee5..b73b3ed341 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h @@ -1,188 +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. ===================================================================*/ #ifndef MITKOPENIGTLINKTRACKINGDEVICE_H_HEADER_INCLUDED_ #define MITKOPENIGTLINKTRACKINGDEVICE_H_HEADER_INCLUDED_ #include #include #include #include #include #include #include #include #include #include -#include "mitkIGTLTransformDeviceSource.h" +#include "mitkIGTLTrackingDataDeviceSource.h" namespace mitk { /** Documentation: * \brief An object of this class represents the MicronTracker device. You can add tools to this * device, then open the connection and start tracking. The tracking device will then * continuously update the tool coordinates. * \ingroup IGT */ class MITKIGT_EXPORT OpenIGTLinkTrackingDevice : public TrackingDevice { public: mitkClassMacro(OpenIGTLinkTrackingDevice, TrackingDevice); - itkFactorylessNewMacro(Self) - itkCloneMacro(Self) + itkFactorylessNewMacro(Self); + itkCloneMacro(Self); - /** Sets the port number for the Open IGT Link connection. Default value is -1 (invalid). */ - void SetPortNumber(int portNumber); + /** Sets the port number for the Open IGT Link connection. Default value is -1 (invalid). */ + void SetPortNumber(int portNumber); /** Sets the hostname for the Open IGT Link connection. Default value is 127.0.0.1 (localhost). */ void SetHostname(std::string hostname); int GetPortNumber(); std::string GetHostname(); /** * \brief Starts the tracking. * \return Returns true if the tracking is started. Throws an exception if an error occures. * @throw mitk::IGTHardwareException Throws an exception if there is an error during start tracking. */ virtual bool StartTracking(); /** * \brief Stops the tracking. * \return Returns true if the tracking is stopped. */ virtual bool StopTracking(); /** * \brief Opens the connection to the device. This have to be done before the tracking is started. * @throw mitk::IGTHardwareException Throws an exception if there is an error during open connection. */ virtual bool OpenConnection(); /** * \brief Closes the connection and clears all resources. */ virtual bool CloseConnection(); /** * \return Returns the number of tools which have been added to the device. */ virtual unsigned int GetToolCount() const; /** * \param toolNumber The number of the tool which should be given back. * \return Returns the tool which the number "toolNumber". Returns nullptr, if there is * no tool with this number. */ TrackingTool* GetTool(unsigned int toolNumber) const; /** * \brief Discover the tools available from the connected OpenIGTLink device and adds these tools to this tracking device. Therefore, a connection * is opened, the tools are discovered and added. * \param WaitingTime Defines how long the method waits for an answer from the server (in milliseconds). Default value is 10000 (10 seconds). * \return Returns true if the connection was established and the tools were discovered successfully and - if at least one tool was found - were added to this device. * Retruns false if no valid connection is available. */ bool DiscoverTools(int WaitingTime = 10000); /** * \brief Create a new OpenIGTLink tool with toolName and fileName and add it to the list of tools * * Note that tools are usually provided by the OpenIGTLink connection. In most cases, the method DiscoverTools() should be used * instead which automatically finds the provided tools. If you use this method to manually add tools be sure that you add the * same number and type of tools that are provided by the connected device. Otherwise problems might occur when you try to start * tracking. */ mitk::TrackingTool* AddTool(const char* toolName, const char* fileName); /** @return Returns true if this device can autodetects its tools. */ virtual bool AutoDetectToolsAvailable(); /** Autodetects tools from the current OpenIGTLink connection and returns them as a navigation tool storage. * @return Returns the detected tools. Returns an empty storage if no tools are present * or if OpenIGTLink Connection is not possible */ virtual mitk::NavigationToolStorage::Pointer AutoDetectTools(); bool IsDeviceInstalled(); itkSetMacro(UpdateRate, int); ///< Sets the update rate of the device in fps. Default value is 60 fps. itkGetConstMacro(UpdateRate, int); ///< Returns the update rate of the device in fps protected: OpenIGTLinkTrackingDevice(); ~OpenIGTLinkTrackingDevice(); /** * \brief Adds a tool to the tracking device. * * \param tool The tool which will be added. * \return Returns true if the tool has been added, false otherwise. */ bool InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool); /** Updates the tools from the open IGT link connection. Is called every time a message received event is invoked.*/ void UpdateTools(); unsigned long m_MessageReceivedObserverTag; /** Receives one message from the OpenIGTLink connection. Starts the tracking stream if required. */ mitk::IGTLMessage::Pointer ReceiveMessage(int waitingTime); /** * \return Returns all tools of the tracking device. */ std::vector GetAllTools(); //OpenIGTLink connection class mitk::IGTLClient::Pointer m_OpenIGTLinkClient; //OpenIGTLink pipeline - mitk::IGTLTransformDeviceSource::Pointer m_IGTLDeviceSource; + mitk::IGTLTrackingDataDeviceSource::Pointer m_IGTLDeviceSource; mitk::IGTLMessageToNavigationDataFilter::Pointer m_IGTLMsgToNavDataFilter; std::vector m_AllTools; ///< vector holding all tools int m_UpdateRate; ///< holds the update rate in FPS (will be set automatically when the OpenIGTLink connection is established) private: enum TrackingMessageType { TDATA, TRANSFORM, QTDATA, UNKNOWN }; mitk::OpenIGTLinkTrackingDevice::TrackingMessageType GetMessageTypeFromString(const char* messageTypeString); /** Discovers tools from the OpenIGTLink connection and converts them to MITK navigation tool objects. @return Returns a navigation tool storage holding all found tools. Returns an empty storage if no tools were found or if there was an error.*/ mitk::NavigationToolStorage::Pointer DiscoverToolsAndConvertToNavigationTools(mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type, int NumberOfMessagesToWait = 50); - /** Discovers tools from the input (type TDATA) */ - mitk::NavigationToolStorage::Pointer DiscoverToolsFromTData(igtl::TrackingDataMessage::Pointer msg); - - /** Discovers tools from the input (type QTDATA) */ - mitk::NavigationToolStorage::Pointer DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg); - - /** Discovers tools from the input (type TRANSFORM) and waits for the given number of messages */ - mitk::NavigationToolStorage::Pointer DiscoverToolsFromTransform(int NumberOfMessagesToWait = 50); - void AddNewToolForName(std::string name, int i); mitk::NavigationTool::Pointer ConstructDefaultOpenIGTLinkTool(std::string name, std::string identifier); }; }//mitk #endif /* MITKOpenIGTLinkTRACKINGDEVICE_H_HEADER_INCLUDED_ */ diff --git a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTrackingDataDeviceSource.cpp b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTrackingDataDeviceSource.cpp new file mode 100644 index 0000000000..f58741555c --- /dev/null +++ b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTrackingDataDeviceSource.cpp @@ -0,0 +1,80 @@ +/*=================================================================== + +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 "mitkIGTLTrackingDataDeviceSource.h" + +#include "mitkIGTLMessage.h" + +//Microservices +#include +#include +#include +#include + +//itk +#include + +mitk::IGTLTrackingDataDeviceSource::IGTLTrackingDataDeviceSource() + : mitk::IGTLDeviceSource() , m_trackingDataType(UNKNOWN) +{ + this->SetName("IGTLDeviceSource (tracking data)"); +} + +mitk::IGTLTrackingDataDeviceSource::~IGTLTrackingDataDeviceSource() +{ +} + +void mitk::IGTLTrackingDataDeviceSource::GenerateData() +{ + if (m_IGTLDevice.IsNull()) + return; + + /* update output with message from the device */ + IGTLMessage* msgOut = this->GetOutput(); + assert(msgOut); + igtl::MessageBase::Pointer msgIn; + switch (m_trackingDataType) + { + case UNKNOWN: + { + igtl::MessageBase::Pointer msgInTDATA = dynamic_cast(m_IGTLDevice->GetNextTrackingDataMessage().GetPointer()); + igtl::MessageBase::Pointer msgInTRANSFORM = dynamic_cast(m_IGTLDevice->GetNextTransformMessage().GetPointer()); + if (msgInTDATA.IsNull() && msgInTRANSFORM.IsNotNull()) { msgIn = msgInTRANSFORM; } + else if (msgInTDATA.IsNotNull() && msgInTRANSFORM.IsNull()) { msgIn = msgInTDATA; } + else if (msgInTDATA.IsNotNull() && msgInTRANSFORM.IsNotNull()) + { + MITK_INFO << "Found both: TDATA and TRANSFORM messages. Using TRANSFORM as default."; + } + } + break; + case TDATA: + msgIn = dynamic_cast(m_IGTLDevice->GetNextTrackingDataMessage().GetPointer()); + break; + case QTDATA: + MITK_WARN << "Receiving QTDATA is not implemented yet!"; + break; + case TRANSFORM: + msgIn = dynamic_cast(m_IGTLDevice->GetNextTransformMessage().GetPointer()); + break; + } + + if (msgIn.IsNotNull()) + { + assert(msgIn); + msgOut->SetMessage(msgIn); + msgOut->SetName(msgIn->GetDeviceName()); + } +} diff --git a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTrackingDataDeviceSource.h b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTrackingDataDeviceSource.h new file mode 100644 index 0000000000..6be7603bf3 --- /dev/null +++ b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTrackingDataDeviceSource.h @@ -0,0 +1,70 @@ +/*=================================================================== + +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 IGTLTrackingDataDeviceSource_H_HEADER_INCLUDED_ +#define IGTLTrackingDataDeviceSource_H_HEADER_INCLUDED_ + +#include "mitkIGTLDeviceSource.h" + +namespace mitk { + /** + * \brief Connects a mitk::IGTLDevice to a + * MITK-OpenIGTLink-Message-Filter-Pipeline + * + * This class is the source of most OpenIGTLink tracking data pipelines. + * Deriving from IGTLDeviceSource it encapsulates a mitk::IGTLDevice and + * provides the tracking data messages of the connected + * OpenIGTLink devices as igtl::MessageBase objects. + * This means it filters for TDATA, QTDATA and TRANSFORM messages. It can + * be configured to listen to one of these message types or to all of them. + * + * Note, that there is just one single output. + * + */ + class MITKOPENIGTLINK_EXPORT IGTLTrackingDataDeviceSource : public IGTLDeviceSource + { + public: + /**OpenIGTLink tracking data message types*/ + enum TrackingMessageType + { + TDATA, TRANSFORM, QTDATA, UNKNOWN + }; + + mitkClassMacro(IGTLTrackingDataDeviceSource, IGTLDeviceSource); + itkFactorylessNewMacro(Self); + itkCloneMacro(Self); + /** Sets the tracking data type the source should listen to. If it is set to unknown (default) it listens to all tracking data message types.*/ + itkSetMacro(trackingDataType, TrackingMessageType); + itkGetConstMacro(trackingDataType, TrackingMessageType); + + protected: + IGTLTrackingDataDeviceSource(); + virtual ~IGTLTrackingDataDeviceSource(); + + TrackingMessageType m_trackingDataType; + + /** + * \brief filter execute method + * + * queries the OpenIGTLink device for new messages and updates its output + * igtl::MessageBase objects with it. + * \warning Will raise a std::out_of_range exception, if tools were added to + * the OpenIGTLink device after it was set as input for this filter + */ + virtual void GenerateData() override; + }; +} // namespace mitk +#endif /* MITKIGTLDeviceSource_H_HEADER_INCLUDED_ */ diff --git a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.cpp b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.cpp deleted file mode 100644 index 26483d2f4c..0000000000 --- a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/*=================================================================== - -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 "mitkIGTLTransformDeviceSource.h" - -#include "mitkIGTLMessage.h" - -//Microservices -#include -#include -#include -#include - -//itk -#include - -mitk::IGTLTransformDeviceSource::IGTLTransformDeviceSource() - : mitk::IGTLDeviceSource() -{ - this->SetName("IGTLDeviceSource (Transforms)"); -} - -mitk::IGTLTransformDeviceSource::~IGTLTransformDeviceSource() -{ -} - -void mitk::IGTLTransformDeviceSource::GenerateData() -{ - if (m_IGTLDevice.IsNull()) - return; - - /* update output with message from the device */ - IGTLMessage* msgOut = this->GetOutput(); - assert(msgOut); - igtl::MessageBase::Pointer msgIn = dynamic_cast(m_IGTLDevice->GetNextTransformMessage().GetPointer()); - if (msgIn.IsNotNull()) - { - assert(msgIn); - msgOut->SetMessage(msgIn); - msgOut->SetName(msgIn->GetDeviceName()); - } -} diff --git a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.h b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.h deleted file mode 100644 index e513c828a5..0000000000 --- a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.h +++ /dev/null @@ -1,55 +0,0 @@ -/*=================================================================== - -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 IGTLTRANSFORMDEVICESOURCE_H_HEADER_INCLUDED_ -#define IGTLTRANSFORMDEVICESOURCE_H_HEADER_INCLUDED_ - -#include "mitkIGTLDeviceSource.h" - -namespace mitk { - /** - * \brief Connects a mitk::IGTLDevice to a - * MITK-OpenIGTLink-Message-Filter-Pipeline - * - * This class is the source of most OpenIGTLink pipelines. It encapsulates a - * mitk::IGTLDevice and provides the information/messages of the connected - * OpenIGTLink devices as igtl::MessageBase objects. Note, that there is just - * one single output. - * - */ - class MITKOPENIGTLINK_EXPORT IGTLTransformDeviceSource : public IGTLDeviceSource - { - public: - mitkClassMacro(IGTLTransformDeviceSource, IGTLDeviceSource); - itkFactorylessNewMacro(Self) - itkCloneMacro(Self) - - protected: - IGTLTransformDeviceSource(); - virtual ~IGTLTransformDeviceSource(); - - /** - * \brief filter execute method - * - * queries the OpenIGTLink device for new messages and updates its output - * igtl::MessageBase objects with it. - * \warning Will raise a std::out_of_range exception, if tools were added to - * the OpenIGTLink device after it was set as input for this filter - */ - virtual void GenerateData() override; - }; -} // namespace mitk -#endif /* MITKIGTLDeviceSource_H_HEADER_INCLUDED_ */ diff --git a/Modules/OpenIGTLink/files.cmake b/Modules/OpenIGTLink/files.cmake index f3a7eaa297..5116602842 100644 --- a/Modules/OpenIGTLink/files.cmake +++ b/Modules/OpenIGTLink/files.cmake @@ -1,21 +1,21 @@ set(CPP_FILES mitkIGTLClient.cpp mitkIGTLServer.cpp mitkIGTLDevice.cpp mitkIGTLMessageSource.cpp mitkIGTLMessageCommon.cpp mitkIGTLDeviceSource.cpp mitkIGTLMessage.cpp mitkIGTLMessageFactory.cpp mitkIGTLMessageCloneHandler.h mitkIGTLDummyMessage.cpp mitkIGTLMessageQueue.cpp mitkIGTLMessageProvider.cpp mitkIGTLMeasurements.cpp mitkIGTLModuleActivator.cpp Filters/mitkImageToIGTLMessageFilter.cpp DeviceSources/mitkIGTL2DImageDeviceSource.cpp DeviceSources/mitkIGTL3DImageDeviceSource.cpp - DeviceSources/mitkIGTLTransformDeviceSource.cpp + DeviceSources/mitkIGTLTrackingDataDeviceSource.cpp ) diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.cpp b/Modules/OpenIGTLink/mitkIGTLDevice.cpp index 9b2ec6b70c..e8e38e7119 100644 --- a/Modules/OpenIGTLink/mitkIGTLDevice.cpp +++ b/Modules/OpenIGTLink/mitkIGTLDevice.cpp @@ -1,564 +1,562 @@ /*=================================================================== 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 "mitkIGTException.h" //#include "mitkIGTTimeStamp.h" #include #include #include #include #include #include //remove later #include //TODO: Which timeout is acceptable and also needed to transmit image data? Is there a maximum data limit? static const int SOCKET_SEND_RECEIVE_TIMEOUT_MSEC = 100; typedef itk::MutexLockHolder MutexLockHolder; mitk::IGTLDevice::IGTLDevice(bool ReadFully) : // m_Data(mitk::DeviceDataUnspecified), m_State(mitk::IGTLDevice::Setup), m_Name("Unspecified Device"), m_StopCommunication(false), m_Hostname("127.0.0.1"), m_PortNumber(-1), m_MultiThreader(nullptr), m_SendThreadID(0), m_ReceiveThreadID(0), m_ConnectThreadID(0) { m_ReadFully = ReadFully; m_StopCommunicationMutex = itk::FastMutexLock::New(); m_StateMutex = itk::FastMutexLock::New(); // m_LatestMessageMutex = itk::FastMutexLock::New(); m_SendingFinishedMutex = itk::FastMutexLock::New(); m_ReceivingFinishedMutex = itk::FastMutexLock::New(); m_ConnectingFinishedMutex = itk::FastMutexLock::New(); // execution rights are owned by the application thread at the beginning m_SendingFinishedMutex->Lock(); m_ReceivingFinishedMutex->Lock(); m_ConnectingFinishedMutex->Lock(); m_MultiThreader = itk::MultiThreader::New(); // m_Data = mitk::DeviceDataUnspecified; // m_LatestMessage = igtl::MessageBase::New(); m_MessageFactory = mitk::IGTLMessageFactory::New(); m_MessageQueue = mitk::IGTLMessageQueue::New(); } mitk::IGTLDevice::~IGTLDevice() { /* stop communication and disconnect from igtl device */ if (GetState() == Running) { this->StopCommunication(); this->CloseConnection(); } else if (GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if (m_MultiThreader.IsNotNull()) { if ((m_SendThreadID != 0)) { m_MultiThreader->TerminateThread(m_SendThreadID); } if ((m_ReceiveThreadID != 0)) { m_MultiThreader->TerminateThread(m_ReceiveThreadID); } if ((m_ConnectThreadID != 0)) { m_MultiThreader->TerminateThread(m_ConnectThreadID); } } m_MultiThreader = nullptr; } 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); m_StateMutex->Lock(); // MutexLockHolder lock(*m_StateMutex); // lock and unlock the mutex if (m_State == state) { m_StateMutex->Unlock(); return; } m_State = state; m_StateMutex->Unlock(); this->Modified(); } bool mitk::IGTLDevice::TestConnection() { return true; } unsigned int mitk::IGTLDevice::ReceivePrivate(igtl::Socket* socket) { // 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 int r = socket->Receive(headerMsg->GetPackPointer(), headerMsg->GetPackSize(), 0); //MITK_INFO << "Server received r = " << r; //MITK_INFO << "Received r = " << r; if (r == 0) //connection error { // an error was received, therefore the communication with this socket // must be stoppedy return IGTL_STATUS_NOT_PRESENT; } else if (r == -1) //timeout { // a timeout was received, this is no error state, thus, do nothing return IGTL_STATUS_TIME_OUT; } else if (r == headerMsg->GetPackSize()) { // Deserialize the header and check the CRC // ERROR HERE: This probably means the header data is corrupted... 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); // std::cerr << "Time stamp: " // << sec << "." // << nanosec << std::endl; // std::cerr << "Dev type and name: " << headerMsg->GetDeviceType() << " " // << headerMsg->GetDeviceName() << std::endl; - // headerMsg->Print(std::cout); + // headerMsg->Print(std::cout); //check the type of the received message //if it is a GET_, STP_ or RTS_ command push it into the command queue //otherwise continue reading the whole message from the socket const char* curDevType = headerMsg->GetDeviceType(); if (std::strstr(curDevType, "GET_") != nullptr || std::strstr(curDevType, "STP_") != nullptr || std::strstr(curDevType, "RTS_") != nullptr) { this->m_MessageQueue->PushCommandMessage(headerMsg); this->InvokeEvent(CommandReceivedEvent()); return IGTL_STATUS_OK; } //Create a message according to the header message igtl::MessageBase::Pointer curMessage; curMessage = m_MessageFactory->CreateInstance(headerMsg); //check if the curMessage is created properly, if not the message type is //not supported and the message has to be skipped if (curMessage.IsNull()) { socket->Skip(headerMsg->GetBodySizeToRead(), 0); // MITK_ERROR("IGTLDevice") << "The received type is not supported. Please " // "add it to the message factory."; return IGTL_STATUS_NOT_FOUND; } //insert the header to the message and allocate the pack curMessage->SetMessageHeader(headerMsg); curMessage->AllocatePack(); // Receive transform data from the socket int receiveCheck = 0; receiveCheck = socket->Receive(curMessage->GetPackBodyPointer(), curMessage->GetPackBodySize(), m_ReadFully); if (receiveCheck > 0) { int c = curMessage->Unpack(1); if (!(c & igtl::MessageHeader::UNPACK_BODY)) { return IGTL_STATUS_CHECKSUM_ERROR; } //check the type of the received message //if it is a command push it into the command queue //otherwise into the normal receive queue //STP_ commands are handled here because they implemented additional //member variables that are not stored in the header message if (std::strstr(curDevType, "STT_") != nullptr) { this->m_MessageQueue->PushCommandMessage(curMessage); this->InvokeEvent(CommandReceivedEvent()); } else { if(m_LogMessages) MITK_INFO << "Received Message: " << mitk::IGTLMessage::New(curMessage)->ToString(); this->m_MessageQueue->PushMessage(curMessage); this->InvokeEvent(MessageReceivedEvent()); } return IGTL_STATUS_OK; } else { - MITK_ERROR("IGTLDevice") << "Received a valid header but could not " + MITK_WARN("IGTLDevice") << "Received a valid header but could not " << "read the whole message."; return IGTL_STATUS_UNKNOWN_ERROR; } } else { //CRC check failed - MITK_ERROR << "CRC Check failed"; + MITK_WARN << "CRC Check failed"; return IGTL_STATUS_CHECKSUM_ERROR; } } else { //Message size information and actual data size don't match. //this state is not suppossed to be reached, return unknown error - MITK_ERROR << "IGTL status unknown"; + MITK_WARN << "IGTL status unknown"; return IGTL_STATUS_UNKNOWN_ERROR; } } void mitk::IGTLDevice::SendMessage(mitk::IGTLMessage::Pointer msg) { m_MessageQueue->PushSendMessage(msg); } unsigned int mitk::IGTLDevice::SendMessagePrivate(mitk::IGTLMessage::Pointer msg, igtl::Socket::Pointer socket) { //check the input message if (msg.IsNull()) { MITK_ERROR("IGTLDevice") << "Could not send message because message is not " "valid. Please check."; return false; } igtl::MessageBase* sendMessage = msg->GetMessage(); // Pack (serialize) and send sendMessage->Pack(); int sendSuccess = socket->Send(sendMessage->GetPackPointer(), sendMessage->GetPackSize()); if (sendSuccess) { - if(m_LogMessages) - MITK_INFO << "Send IGTL message: " << msg->ToString(); - + if (m_LogMessages) { MITK_INFO << "Send IGTL message: " << msg->ToString(); } this->InvokeEvent(MessageSentEvent()); return IGTL_STATUS_OK; } else { return IGTL_STATUS_UNKNOWN_ERROR; } } void mitk::IGTLDevice::RunCommunication(void (IGTLDevice::*ComFunction)(void), itk::FastMutexLock* mutex) { if (this->GetState() != Running) return; try { // keep lock until end of scope MutexLockHolder communicationFinishedLockHolder(*mutex); // 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)) { (this->*ComFunction)(); /* Update the local copy of m_StopCommunication */ this->m_StopCommunicationMutex->Lock(); localStopCommunication = m_StopCommunication; this->m_StopCommunicationMutex->Unlock(); // time to relax, this sets the maximum ever possible framerate to 1000 Hz itksys::SystemTools::Delay(1); } } catch (...) { mutex->Unlock(); this->StopCommunication(); MITK_ERROR("IGTLDevice::RunCommunication") << "Error while communicating. Thread stopped."; //mitkThrowException(mitk::IGTException) << "Error while communicating. Thread stopped."; } // StopCommunication was called, thus the mode should be changed back to Ready now // that the tracking loop has ended. //this->SetState(Ready); //this is done elsewhere 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; // go to mode Running this->SetState(Running); // set a timeout for the sending and receiving this->m_Socket->SetTimeout(SOCKET_SEND_RECEIVE_TIMEOUT_MSEC); // 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_SendingFinishedMutex->Unlock(); m_ReceivingFinishedMutex->Unlock(); m_ConnectingFinishedMutex->Unlock(); // start new threads that execute the communication m_SendThreadID = m_MultiThreader->SpawnThread(this->ThreadStartSending, this); m_ReceiveThreadID = m_MultiThreader->SpawnThread(this->ThreadStartReceiving, this); m_ConnectThreadID = m_MultiThreader->SpawnThread(this->ThreadStartConnecting, 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_SendingFinishedMutex->Lock(); m_ReceivingFinishedMutex->Lock(); m_ConnectingFinishedMutex->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(); /* return to setup mode */ this->SetState(Setup); // this->InvokeEvent(mitk::LostConnectionEvent()); return true; } bool mitk::IGTLDevice::SendRTSMessage(const char* type) { //construct the device type for the return message, it starts with RTS_ and //continues with the requested type std::string returnType("RTS_"); returnType.append(type); //create a return message igtl::MessageBase::Pointer rtsMsg = this->m_MessageFactory->CreateInstance(returnType); //if retMsg is nullptr there is no return message defined and thus it is not //necessary to send one back if (rtsMsg.IsNotNull()) { this->SendMessage(mitk::IGTLMessage::New(rtsMsg)); return true; } else { return false; } } void mitk::IGTLDevice::Connect() { MITK_DEBUG << "mitk::IGTLDevice::Connect();"; } igtl::ImageMessage::Pointer mitk::IGTLDevice::GetNextImage2dMessage() { return this->m_MessageQueue->PullImage2dMessage(); } igtl::ImageMessage::Pointer mitk::IGTLDevice::GetNextImage3dMessage() { return this->m_MessageQueue->PullImage3dMessage(); } igtl::TransformMessage::Pointer mitk::IGTLDevice::GetNextTransformMessage() { return this->m_MessageQueue->PullTransformMessage(); } igtl::TrackingDataMessage::Pointer mitk::IGTLDevice::GetNextTrackingDataMessage() { igtl::TrackingDataMessage::Pointer msg = this->m_MessageQueue->PullTrackingMessage(); return msg; } igtl::StringMessage::Pointer mitk::IGTLDevice::GetNextStringMessage() { return this->m_MessageQueue->PullStringMessage(); } igtl::MessageBase::Pointer mitk::IGTLDevice::GetNextMiscMessage() { return this->m_MessageQueue->PullMiscMessage(); } igtl::MessageBase::Pointer mitk::IGTLDevice::GetNextCommand() { return m_MessageQueue->PullCommandMessage(); } void mitk::IGTLDevice::EnableNoBufferingMode(bool enable) { m_MessageQueue->EnableNoBufferingMode(enable); } void mitk::IGTLDevice::EnableNoBufferingMode( mitk::IGTLMessageQueue::Pointer queue, bool enable) { queue->EnableNoBufferingMode(enable); } ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartSending(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData; if (igtlDevice != nullptr) { igtlDevice->RunCommunication(&mitk::IGTLDevice::Send, igtlDevice->m_SendingFinishedMutex); } igtlDevice->m_SendThreadID = 0; // erase thread id because thread will end. return ITK_THREAD_RETURN_VALUE; } ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartReceiving(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData; if (igtlDevice != nullptr) { igtlDevice->RunCommunication(&mitk::IGTLDevice::Receive, igtlDevice->m_ReceivingFinishedMutex); } igtlDevice->m_ReceiveThreadID = 0; // erase thread id because thread will end. return ITK_THREAD_RETURN_VALUE; } ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartConnecting(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData; if (igtlDevice != nullptr) { igtlDevice->RunCommunication(&mitk::IGTLDevice::Connect, igtlDevice->m_ConnectingFinishedMutex); } igtlDevice->m_ConnectThreadID = 0; // erase thread id because thread will end. return ITK_THREAD_RETURN_VALUE; } diff --git a/Modules/OpenIGTLink/mitkIGTLMessage.cpp b/Modules/OpenIGTLink/mitkIGTLMessage.cpp index 5dd4b976c0..6ee7d0abd6 100644 --- a/Modules/OpenIGTLink/mitkIGTLMessage.cpp +++ b/Modules/OpenIGTLink/mitkIGTLMessage.cpp @@ -1,181 +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 "mitkIGTLMessage.h" #include "mitkException.h" #include "mitkIGTLMessageCommon.h" mitk::IGTLMessage::IGTLMessage() : itk::DataObject(), m_DataValid(false), m_IGTTimeStamp(0), m_Name() { m_Message = igtl::MessageBase::New(); } mitk::IGTLMessage::IGTLMessage(const mitk::IGTLMessage& toCopy) : itk::DataObject() { // TODO SW: Graft does the same, remove code duplications, set Graft to // deprecated, remove duplication in tescode this->Graft(&toCopy); } mitk::IGTLMessage::~IGTLMessage() { } mitk::IGTLMessage::IGTLMessage(igtl::MessageBase::Pointer message) { this->SetMessage(message); this->SetName(message->GetDeviceName()); } void mitk::IGTLMessage::Graft( const DataObject *data ) { // Attempt to cast data to an IGTLMessage const Self* msg; try { msg = dynamic_cast(data); } catch( ... ) { itkExceptionMacro( << "mitk::IGTLMessage::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name() ); return; } if (!msg) { // pointer could not be cast back down itkExceptionMacro( << "mitk::IGTLMessage::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name() ); return; } // Now copy anything that is needed this->SetMessage(msg->GetMessage()); this->SetDataValid(msg->IsDataValid()); this->SetIGTTimeStamp(msg->GetIGTTimeStamp()); this->SetName(msg->GetName()); } void mitk::IGTLMessage::SetMessage(igtl::MessageBase::Pointer msg) { m_Message = msg; unsigned int ts = 0; unsigned int frac = 0; m_Message->GetTimeStamp(&ts, &frac); //ts = seconds / frac = nanoseconds this->SetName(m_Message->GetDeviceName()); double timestamp = ts * 1000.0 + frac; this->SetIGTTimeStamp(timestamp); this->SetDataValid(true); } bool mitk::IGTLMessage::IsDataValid() const { return m_DataValid; } void mitk::IGTLMessage::PrintSelf(std::ostream& os, itk::Indent indent) const { - this->Superclass::PrintSelf(os, indent); os << indent << "name: " << this->GetName() << std::endl; - os << indent << "data valid: " << this->IsDataValid() << std::endl; - os << indent << "TimeStamp: " << this->GetIGTTimeStamp() << std::endl; + os << indent << "type: " << this->GetIGTLMessageType() << std::endl; + os << indent << "valid: " << this->IsDataValid() << std::endl; + os << indent << "timestamp: " << this->GetIGTTimeStamp() << std::endl; os << indent << "OpenIGTLinkMessage: " << std::endl; m_Message->Print(os); + this->Superclass::PrintSelf(os, indent); } std::string mitk::IGTLMessage::ToString() const { std::stringstream output; - output << "name: " << this->GetName() << std::endl << - "MessageType: " << this->GetIGTLMessageType() << std::endl << - "TimeStamp: " << this->GetIGTTimeStamp() << std::endl << - "OpenIGTLinkMessage: " << std::endl; + this->Print(output); return output.str(); } void mitk::IGTLMessage::CopyInformation( const DataObject* data ) { this->Superclass::CopyInformation( data ); const Self * nd = nullptr; try { nd = dynamic_cast(data); } catch( ... ) { // data could not be cast back down itkExceptionMacro(<< "mitk::IGTLMessage::CopyInformation() cannot cast " << typeid(data).name() << " to " << typeid(Self*).name() ); } if ( !nd ) { // pointer could not be cast back down itkExceptionMacro(<< "mitk::IGTLMessage::CopyInformation() cannot cast " << typeid(data).name() << " to " << typeid(Self*).name() ); } /* copy all meta data */ } bool mitk::Equal(const mitk::IGTLMessage& leftHandSide, const mitk::IGTLMessage& rightHandSide, ScalarType /*eps*/, bool verbose) { bool returnValue = true; if( std::string(rightHandSide.GetName()) != std::string(leftHandSide.GetName()) ) { if(verbose) { MITK_INFO << "[( IGTLMessage )] Name differs."; MITK_INFO << "leftHandSide is " << leftHandSide.GetName() << "rightHandSide is " << rightHandSide.GetName(); } returnValue = false; } if( rightHandSide.GetIGTTimeStamp() != leftHandSide.GetIGTTimeStamp() ) { if(verbose) { MITK_INFO << "[( IGTLMessage )] IGTTimeStamp differs."; MITK_INFO << "leftHandSide is " << leftHandSide.GetIGTTimeStamp() << "rightHandSide is " << rightHandSide.GetIGTTimeStamp(); } returnValue = false; } return returnValue; } const char* mitk::IGTLMessage::GetIGTLMessageType() const { return this->m_Message->GetDeviceType(); } template < typename IGTLMessageType > IGTLMessageType* mitk::IGTLMessage::GetMessage() const { return dynamic_cast(this->m_Message); } diff --git a/Modules/US/USModel/mitkUSIGTLDevice.cpp b/Modules/US/USModel/mitkUSIGTLDevice.cpp index e56532f27a..17afb40fcd 100644 --- a/Modules/US/USModel/mitkUSIGTLDevice.cpp +++ b/Modules/US/USModel/mitkUSIGTLDevice.cpp @@ -1,72 +1,72 @@ /*=================================================================== 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 mitk::USIGTLDevice::USIGTLDevice(std::string manufacturer, std::string model, std::string host, int port, bool server) : mitk::USDevice(manufacturer, model), m_Host(host), m_Port(port) { if (server) { m_Device = mitk::IGTLServer::New(true); } else { m_Device = mitk::IGTLClient::New(true); } m_Device->SetPortNumber(m_Port); m_Device->SetHostname(m_Host); m_Device->SetName(manufacturer + " - " + model); - m_TransformDeviceSource = mitk::IGTLTransformDeviceSource::New(); + m_TransformDeviceSource = mitk::IGTLTrackingDataDeviceSource::New(); m_TransformDeviceSource->SetIGTLDevice(m_Device); m_TransformDeviceSource->RegisterAsMicroservice(); m_DeviceSource = mitk::IGTL2DImageDeviceSource::New(); m_DeviceSource->SetIGTLDevice(m_Device); m_DeviceSource->RegisterAsMicroservice(); m_Filter = mitk::IGTLMessageToUSImageFilter::New(); m_Filter->SetNumberOfExpectedOutputs(1); m_Filter->ConnectTo(m_DeviceSource); } std::string mitk::USIGTLDevice::GetDeviceClass() { return "IGTL Client"; } mitk::USImageSource::Pointer mitk::USIGTLDevice::GetUSImageSource() { return m_Filter.GetPointer(); } bool mitk::USIGTLDevice::OnInitialization() { return true; } bool mitk::USIGTLDevice::OnConnection() { return m_Device->OpenConnection(); } bool mitk::USIGTLDevice::OnDisconnection() { return m_Device->CloseConnection(); } bool mitk::USIGTLDevice::OnActivation() { return m_Device->StartCommunication(); } bool mitk::USIGTLDevice::OnDeactivation() { return m_Device->StopCommunication(); } diff --git a/Modules/US/USModel/mitkUSIGTLDevice.h b/Modules/US/USModel/mitkUSIGTLDevice.h index 288dc556fe..066a6a9994 100644 --- a/Modules/US/USModel/mitkUSIGTLDevice.h +++ b/Modules/US/USModel/mitkUSIGTLDevice.h @@ -1,69 +1,69 @@ /*=================================================================== 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_HEADER_INCLUDED_ #define MITKIGTLDevice_H_HEADER_INCLUDED_ #include #include #include #include #include #include -#include +#include #include namespace mitk { /** * \brief A mitk::USIGTLDevice is a USDevice to receive images over an OpenIGTLink * connection. It registers an OIGTL device as a Microservice to receive image messages * and transforms them to mitk::Images. It can act both as a server (listening for * incoming connections) and as a client (connecting to an existing OIGTL server). * * \ingroup US */ class MITKUS_EXPORT USIGTLDevice : public mitk::USDevice { public: mitkClassMacro(USIGTLDevice, mitk::USDevice); // To open a device (Manufacturer, Model, Hostname, Port, IsServer) mitkNewMacro5Param(Self, std::string, std::string, std::string, int, bool); virtual std::string GetDeviceClass(); virtual USImageSource::Pointer GetUSImageSource(); USIGTLDevice(std::string manufacturer, std::string model, std::string host, int port, bool server); protected: virtual bool OnInitialization(); virtual bool OnConnection(); virtual bool OnDisconnection(); virtual bool OnActivation(); virtual bool OnDeactivation(); private: std::string m_Host; int m_Port; mitk::IGTLDevice::Pointer m_Device; mitk::IGTL2DImageDeviceSource::Pointer m_DeviceSource; - mitk::IGTLTransformDeviceSource::Pointer m_TransformDeviceSource; + mitk::IGTLTrackingDataDeviceSource::Pointer m_TransformDeviceSource; mitk::IGTLMessageToUSImageFilter::Pointer m_Filter; }; } // namespace mitk #endif // MITKIGTLDevice_H_HEADER_INCLUDED_ diff --git a/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.cpp b/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.cpp index 2446445572..4d4d5c753a 100644 --- a/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.cpp +++ b/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.cpp @@ -1,199 +1,199 @@ /*=================================================================== 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. ===================================================================*/ // Blueberry #include #include // Qmitk #include "OpenIGTLinkPlugin.h" // Qt #include //mitk image #include const std::string OpenIGTLinkPlugin::VIEW_ID = "org.mitk.views.openigtlinkplugin"; void OpenIGTLinkPlugin::SetFocus() { } void OpenIGTLinkPlugin::CreateQtPartControl(QWidget *parent) { // create GUI widgets from the Qt Designer's .ui file m_Controls.setupUi(parent); connect(m_Controls.buttonConnect, SIGNAL(clicked()), this, SLOT(ConnectButtonClicked())); connect(m_Controls.buttonReceive, SIGNAL(clicked()), this, SLOT(ReceivingButtonClicked())); connect(&m_Timer, SIGNAL(timeout()), this, SLOT(UpdatePipeline())); m_Image2dNode = mitk::DataNode::New(); m_State = IDLE; StateChanged(m_State); } void OpenIGTLinkPlugin::UpdatePipeline() { m_NavigationDataObjectVisualizationFilter->Update(); mitk::Image::Pointer image2d = m_ImageFilter2D->GetNextImage(); mitk::Image::Pointer image3d = m_ImageFilter3D->GetNextImage(); m_Image2dNode->SetName("US Image Stream"); m_Image2dNode->SetData(image2d); mitk::RenderingManager::GetInstance()->RequestUpdateAll(); } void OpenIGTLinkPlugin::OnSelectionChanged(berry::IWorkbenchPart::Pointer, const QList &) { // iterate all selected objects, adjust warning visibility } void OpenIGTLinkPlugin::ConnectButtonClicked() { bool success; switch (m_State) { case IDLE: m_IGTLClient = mitk::IGTLClient::New(true); m_IGTLClient->SetHostname(m_Controls.textEditHostname->text().toStdString()); m_IGTLClient->SetPortNumber(m_Controls.spinBoxPort->value()); success = m_IGTLClient->OpenConnection(); if (!success) { QMessageBox::warning(nullptr, QString("Connection failed"), QString("Client could not connect to given server."), QMessageBox::Ok, QMessageBox::Abort); } else { m_State = CONNECTED; StateChanged(m_State); } break; case CONNECTED: success = m_IGTLClient->CloseConnection(); m_State = IDLE; StateChanged(m_State); break; case RECEIVING: ReceivingButtonClicked(); success = m_IGTLClient->CloseConnection(); m_State = IDLE; StateChanged(m_State); break; } } void OpenIGTLinkPlugin::ReceivingButtonClicked() { switch (m_State) { case IDLE: QMessageBox::warning(nullptr, QString("Not ready.."), QString("The client must be connected to a server first."), QMessageBox::Ok, QMessageBox::Abort); break; case CONNECTED: m_IGTL2DImageDeviceSource = mitk::IGTL2DImageDeviceSource::New(); m_IGTL3DImageDeviceSource = mitk::IGTL3DImageDeviceSource::New(); - m_IGTLTransformDeviceSource = mitk::IGTLTransformDeviceSource::New(); + m_IGTLTransformDeviceSource = mitk::IGTLTrackingDataDeviceSource::New(); m_IGTL2DImageDeviceSource->SetIGTLDevice(m_IGTLClient); m_IGTL3DImageDeviceSource->SetIGTLDevice(m_IGTLClient); m_IGTLTransformDeviceSource->SetIGTLDevice(m_IGTLClient); this->GetDataStorage()->Add(m_Image2dNode); m_IGTLMessageToNavigationDataFilter = mitk::IGTLMessageToNavigationDataFilter::New(); m_NavigationDataObjectVisualizationFilter = mitk::NavigationDataObjectVisualizationFilter::New(); m_ImageFilter2D = mitk::IGTLMessageToUSImageFilter::New(); m_ImageFilter3D = mitk::IGTLMessageToUSImageFilter::New(); m_IGTLMessageToNavigationDataFilter->SetNumberOfExpectedOutputs(3); m_IGTLMessageToNavigationDataFilter->ConnectTo(m_IGTLTransformDeviceSource); m_NavigationDataObjectVisualizationFilter->ConnectTo(m_IGTLMessageToNavigationDataFilter); m_ImageFilter2D->ConnectTo(m_IGTL2DImageDeviceSource); m_ImageFilter3D->ConnectTo(m_IGTL3DImageDeviceSource); //create an object that will be moved respectively to the navigation data for (size_t i = 0; i < m_IGTLMessageToNavigationDataFilter->GetNumberOfIndexedOutputs(); i++) { mitk::DataNode::Pointer newNode = mitk::DataNode::New(); QString name("DemoNode T"); name.append(QString::number(i)); newNode->SetName(name.toStdString()); //create small sphere and use it as surface mitk::Surface::Pointer mySphere = mitk::Surface::New(); vtkSphereSource *vtkData = vtkSphereSource::New(); vtkData->SetRadius(2.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); mySphere->SetProperty("color", mitk::ColorProperty::New(1, 0, 0)); mySphere->SetVtkPolyData(vtkData->GetOutput()); vtkData->Delete(); newNode->SetData(mySphere); this->GetDataStorage()->Add(newNode); m_NavigationDataObjectVisualizationFilter->SetRepresentationObject(i, mySphere); } m_IGTLClient->StartCommunication(); m_Timer.setInterval(10); m_Timer.start(); m_State = RECEIVING; StateChanged(m_State); break; case RECEIVING: m_IGTLClient->StopCommunication(); this->GetDataStorage()->Remove(this->GetDataStorage()->GetAll()); m_Timer.stop(); m_State = CONNECTED; StateChanged(m_State); break; } } void OpenIGTLinkPlugin::StateChanged(OpenIGTLinkPlugin::State newState) { switch (newState) { case IDLE: m_Controls.buttonConnect->setText(QString("Connect To Server")); m_Controls.buttonReceive->setText(QString("Start Receiving")); m_Controls.buttonReceive->setDisabled(true); break; case CONNECTED: m_Controls.buttonConnect->setText(QString("Disconnect From Server")); m_Controls.buttonReceive->setText(QString("Start Receiving")); m_Controls.buttonReceive->setDisabled(false); break; case RECEIVING: m_Controls.buttonConnect->setText(QString("Disconnect From Server")); m_Controls.buttonReceive->setText(QString("Stop Receiving")); m_Controls.buttonReceive->setDisabled(false); break; } } diff --git a/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.h b/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.h index 2090aa3836..4b8bd73f1f 100644 --- a/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.h +++ b/Plugins/org.mitk.gui.qt.igtexamples/src/internal/OpenIGTLinkPlugin.h @@ -1,108 +1,108 @@ /*=================================================================== 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 OpenIGTLinkPlugin_h #define OpenIGTLinkPlugin_h #include #include #include "ui_OpenIGTLinkPluginControls.h" #include "qtimer.h" //OIGTL #include "mitkIGTLClient.h" #include "mitkIGTL2DImageDeviceSource.h" #include "mitkIGTL3DImageDeviceSource.h" -#include "mitkIGTLTransformDeviceSource.h" +#include "mitkIGTLTrackingDataDeviceSource.h" //mitk #include "mitkNavigationDataObjectVisualizationFilter.h" #include "mitkIGTLMessageToNavigationDataFilter.h" #include "mitkIGTLMessageToUSImageFilter.h" #include #include //vtk #include /** \brief OpenIGTLinkPlugin \warning This class is not yet documented. Use "git blame" and ask the author to provide basic documentation. \sa QmitkAbstractView \ingroup ${plugin_target}_internal */ class OpenIGTLinkPlugin : public QmitkAbstractView { // this is needed for all Qt objects that should have a Qt meta-object // (everything that derives from QObject and wants to have signal/slots) Q_OBJECT public: static const std::string VIEW_ID; protected slots: void ConnectButtonClicked(); void ReceivingButtonClicked(); void UpdatePipeline(); protected: enum State{ IDLE, CONNECTED, RECEIVING }; virtual void CreateQtPartControl(QWidget *parent) override; virtual void SetFocus() override; /// \brief called by QmitkFunctionality when DataManager's selection has changed virtual void OnSelectionChanged(berry::IWorkbenchPart::Pointer source, const QList& nodes) override; Ui::OpenIGTLinkPluginControls m_Controls; mitk::IGTL2DImageDeviceSource::Pointer m_IGTL2DImageDeviceSource; mitk::IGTL3DImageDeviceSource::Pointer m_IGTL3DImageDeviceSource; - mitk::IGTLTransformDeviceSource::Pointer m_IGTLTransformDeviceSource; + mitk::IGTLTrackingDataDeviceSource::Pointer m_IGTLTransformDeviceSource; mitk::IGTLClient::Pointer m_IGTLClient; QTimer m_Timer; mitk::IGTLMessageToNavigationDataFilter::Pointer m_IGTLMessageToNavigationDataFilter; mitk::NavigationDataObjectVisualizationFilter::Pointer m_NavigationDataObjectVisualizationFilter; mitk::IGTLMessageToUSImageFilter::Pointer m_ImageFilter3D; mitk::IGTLMessageToUSImageFilter::Pointer m_ImageFilter2D; mitk::DataNode::Pointer m_Image2dNode; private: void StateChanged(State newState); State m_State; }; #endif // OpenIGTLinkPlugin_h diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxView.cpp b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxView.cpp index 570df2261a..60e70eee44 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxView.cpp +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxView.cpp @@ -1,1460 +1,1462 @@ /*=================================================================== 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. ===================================================================*/ // Blueberry #include #include // Qmitk #include "QmitkMITKIGTTrackingToolboxView.h" #include "QmitkTrackingDeviceConfigurationWidget.h" // Qt #include #include #include // MITK #include #include #include #include #include #include #include //#include #include #include #include #include #include #include "mitkNDIAuroraTypeInformation.h" // vtk #include //for exceptions #include #include //for Microservice #include "mitkPluginActivator.h" #include #include #include "usServiceReference.h" const std::string QmitkMITKIGTTrackingToolboxView::VIEW_ID = "org.mitk.views.mitkigttrackingtoolbox"; QmitkMITKIGTTrackingToolboxView::QmitkMITKIGTTrackingToolboxView() : QmitkAbstractView() , m_Controls(nullptr) , m_DeviceTypeCollection(nullptr) { m_TrackingLoggingTimer = new QTimer(this); m_TrackingRenderTimer = new QTimer(this); m_TimeoutTimer = new QTimer(this); m_tracking = false; m_connected = false; m_logging = false; m_loggedFrames = 0; m_SimpleModeEnabled = false; //create filename for autosaving of tool storage QString loggingPathWithoutFilename = QString(mitk::LoggingBackend::GetLogFile().c_str()); if (!loggingPathWithoutFilename.isEmpty()) //if there already is a path for the MITK logging file use this one { //extract path from path+filename (if someone knows a better way to do this feel free to change it) int lengthOfFilename = QFileInfo(QString::fromStdString(mitk::LoggingBackend::GetLogFile())).fileName().size(); loggingPathWithoutFilename.resize(loggingPathWithoutFilename.size() - lengthOfFilename); m_AutoSaveFilename = loggingPathWithoutFilename + "TrackingToolboxAutoSave.IGTToolStorage"; } else //if not: use a temporary path from IOUtil { m_AutoSaveFilename = QString(mitk::IOUtil::GetTempPath().c_str()) + "TrackingToolboxAutoSave.IGTToolStorage"; } MITK_INFO("IGT Tracking Toolbox") << "Filename for auto saving of IGT ToolStorages: " << m_AutoSaveFilename.toStdString(); //! [Thread 1] //initialize worker thread m_WorkerThread = new QThread(); m_Worker = new QmitkMITKIGTTrackingToolboxViewWorker(); //! [Thread 1] ctkPluginContext* pluginContext = mitk::PluginActivator::GetContext(); if (pluginContext) { QString interfaceName = QString::fromStdString(us_service_interface_iid()); QList serviceReference = pluginContext->getServiceReferences(interfaceName); if (serviceReference.size() > 0) { m_DeviceTypeServiceReference = serviceReference.at(0); const ctkServiceReference& r = serviceReference.at(0); m_DeviceTypeCollection = pluginContext->getService(r); } else { MITK_INFO << "No Tracking Device Collection!"; } } } QmitkMITKIGTTrackingToolboxView::~QmitkMITKIGTTrackingToolboxView() { this->StoreUISettings(); m_TrackingLoggingTimer->stop(); m_TrackingRenderTimer->stop(); m_TimeoutTimer->stop(); delete m_TrackingLoggingTimer; delete m_TrackingRenderTimer; delete m_TimeoutTimer; try { //! [Thread 2] // wait for thread to finish m_WorkerThread->terminate(); m_WorkerThread->wait(); //clean up worker thread if (m_WorkerThread) { delete m_WorkerThread; } if (m_Worker) { delete m_Worker; } //! [Thread 2] //remove the tracking volume this->GetDataStorage()->Remove(m_TrackingVolumeNode); //unregister microservices if (m_toolStorage) { m_toolStorage->UnRegisterMicroservice(); } if (m_TrackingDeviceSource) { m_TrackingDeviceSource->UnRegisterMicroservice(); } if (m_IGTLMessageProvider.IsNotNull()){ m_IGTLMessageProvider->UnRegisterMicroservice(); } } catch (std::exception& e) { MITK_WARN << "Unexpected exception during clean up of tracking toolbox view: " << e.what(); } catch (...) { MITK_WARN << "Unexpected unknown error during clean up of tracking toolbox view!"; } //store tool storage and UI settings for persistence this->AutoSaveToolStorage(); this->StoreUISettings(); m_DeviceTypeCollection = nullptr; mitk::PluginActivator::GetContext()->ungetService(m_DeviceTypeServiceReference); } void QmitkMITKIGTTrackingToolboxView::CreateQtPartControl(QWidget *parent) { // build up qt view, unless already done if (!m_Controls) { // create GUI widgets from the Qt Designer's .ui file m_Controls = new Ui::QmitkMITKIGTTrackingToolboxViewControls; m_Controls->setupUi(parent); //create connections connect(m_Controls->m_LoadTools, SIGNAL(clicked()), this, SLOT(OnLoadTools())); connect(m_Controls->m_ConnectDisconnectButton, SIGNAL(clicked()), this, SLOT(OnConnectDisconnect())); connect(m_Controls->m_StartStopTrackingButton, SIGNAL(clicked()), this, SLOT(OnStartStopTracking())); connect(m_Controls->m_ConnectSimpleMode, SIGNAL(clicked()), this, SLOT(OnConnectDisconnect())); connect(m_Controls->m_StartTrackingSimpleMode, SIGNAL(clicked()), this, SLOT(OnStartStopTracking())); connect(m_Controls->m_FreezeUnfreezeTrackingButton, SIGNAL(clicked()), this, SLOT(OnFreezeUnfreezeTracking())); connect(m_TrackingLoggingTimer, SIGNAL(timeout()), this, SLOT(UpdateLoggingTrackingTimer())); connect(m_TrackingRenderTimer, SIGNAL(timeout()), this, SLOT(UpdateRenderTrackingTimer())); connect(m_TimeoutTimer, SIGNAL(timeout()), this, SLOT(OnTimeOut())); connect(m_Controls->m_ChooseFile, SIGNAL(clicked()), this, SLOT(OnChooseFileClicked())); connect(m_Controls->m_StartLogging, SIGNAL(clicked()), this, SLOT(StartLogging())); connect(m_Controls->m_StopLogging, SIGNAL(clicked()), this, SLOT(StopLogging())); connect(m_Controls->m_VolumeSelectionBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(OnTrackingVolumeChanged(QString))); connect(m_Controls->m_ShowTrackingVolume, SIGNAL(clicked()), this, SLOT(OnShowTrackingVolumeChanged())); connect(m_Controls->m_AutoDetectTools, SIGNAL(clicked()), this, SLOT(OnAutoDetectTools())); connect(m_Controls->m_ResetTools, SIGNAL(clicked()), this, SLOT(OnResetTools())); connect(m_Controls->m_AddSingleTool, SIGNAL(clicked()), this, SLOT(OnAddSingleTool())); connect(m_Controls->m_NavigationToolCreationWidget, SIGNAL(NavigationToolFinished()), this, SLOT(OnAddSingleToolFinished())); connect(m_Controls->m_NavigationToolCreationWidget, SIGNAL(Canceled()), this, SLOT(OnAddSingleToolCanceled())); connect(m_Controls->m_csvFormat, SIGNAL(clicked()), this, SLOT(OnToggleFileExtension())); connect(m_Controls->m_xmlFormat, SIGNAL(clicked()), this, SLOT(OnToggleFileExtension())); connect(m_Controls->m_UseDifferentUpdateRates, SIGNAL(clicked()), this, SLOT(OnToggleDifferentUpdateRates())); connect(m_Controls->m_RenderUpdateRate, SIGNAL(valueChanged(int)), this, SLOT(OnChangeRenderUpdateRate())); connect(m_Controls->m_DisableAllTimers, SIGNAL(stateChanged(int)), this, SLOT(EnableDisableTimerButtons(int))); connect(m_Controls->m_advancedUI, SIGNAL(clicked()), this, SLOT(OnToggleAdvancedSimpleMode())); connect(m_Controls->m_simpleUI, SIGNAL(clicked()), this, SLOT(OnToggleAdvancedSimpleMode())); //connections for the tracking device configuration widget connect(m_Controls->m_configurationWidget, SIGNAL(TrackingDeviceSelectionChanged()), this, SLOT(OnTrackingDeviceChanged())); //! [Thread 3] //connect worker thread connect(m_Worker, SIGNAL(AutoDetectToolsFinished(bool, QString)), this, SLOT(OnAutoDetectToolsFinished(bool, QString))); connect(m_Worker, SIGNAL(ConnectDeviceFinished(bool, QString)), this, SLOT(OnConnectFinished(bool, QString))); connect(m_Worker, SIGNAL(StartTrackingFinished(bool, QString)), this, SLOT(OnStartTrackingFinished(bool, QString))); connect(m_Worker, SIGNAL(StopTrackingFinished(bool, QString)), this, SLOT(OnStopTrackingFinished(bool, QString))); connect(m_Worker, SIGNAL(DisconnectDeviceFinished(bool, QString)), this, SLOT(OnDisconnectFinished(bool, QString))); connect(m_WorkerThread, SIGNAL(started()), m_Worker, SLOT(ThreadFunc())); //move the worker to the thread m_Worker->moveToThread(m_WorkerThread); //! [Thread 3] //initialize widgets m_Controls->m_TrackingToolsStatusWidget->SetShowPositions(true); m_Controls->m_TrackingToolsStatusWidget->SetTextAlignment(Qt::AlignLeft); m_Controls->m_simpleWidget->setVisible(false); //initialize tracking volume node m_TrackingVolumeNode = mitk::DataNode::New(); m_TrackingVolumeNode->SetName("TrackingVolume"); m_TrackingVolumeNode->SetBoolProperty("Backface Culling", true); mitk::Color red; red.SetRed(1); m_TrackingVolumeNode->SetColor(red); //initialize buttons m_Controls->m_AutoDetectTools->setVisible(false); //only visible if supported by tracking device m_Controls->m_StartStopTrackingButton->setEnabled(false); m_Controls->m_StartTrackingSimpleMode->setEnabled(false); m_Controls->m_FreezeUnfreezeTrackingButton->setEnabled(false); //initialize warning labels m_Controls->m_renderWarningLabel->setVisible(false); m_Controls->m_TrackingFrozenLabel->setVisible(false); //Update List of available models for selected tool. std::vector Compatibles; if ((m_Controls == nullptr) || //check all these stuff for nullptr, latterly this causes crashes from time to time (m_Controls->m_configurationWidget == nullptr) || (m_Controls->m_configurationWidget->GetTrackingDevice().IsNull())) { MITK_ERROR << "Couldn't get current tracking device or an object is nullptr, something went wrong!"; return; } else { Compatibles = m_DeviceTypeCollection->GetDeviceDataForLine(m_Controls->m_configurationWidget->GetTrackingDevice()->GetType()); } m_Controls->m_VolumeSelectionBox->clear(); for (std::size_t i = 0; i < Compatibles.size(); i++) { m_Controls->m_VolumeSelectionBox->addItem(Compatibles[i].Model.c_str()); } //initialize tool storage m_toolStorage = mitk::NavigationToolStorage::New(GetDataStorage()); m_toolStorage->SetName("TrackingToolbox Default Storage"); m_toolStorage->RegisterAsMicroservice("no tracking device"); //set home directory as default path for logfile m_Controls->m_LoggingFileName->setText(QDir::toNativeSeparators(QDir::homePath()) + QDir::separator() + "logfile.csv"); //tracking device may be changed already by the persistence of the //QmitkTrackingDeciveConfigurationWidget this->OnTrackingDeviceChanged(); this->LoadUISettings(); //add tracking volume node only to data storage this->GetDataStorage()->Add(m_TrackingVolumeNode); if (!m_Controls->m_ShowTrackingVolume->isChecked()) m_TrackingVolumeNode->SetOpacity(0.0); else m_TrackingVolumeNode->SetOpacity(0.25); //Update List of available models for selected tool. m_Controls->m_VolumeSelectionBox->clear(); for (std::size_t i = 0; i < Compatibles.size(); i++) { m_Controls->m_VolumeSelectionBox->addItem(Compatibles[i].Model.c_str()); } } } void QmitkMITKIGTTrackingToolboxView::SetFocus() { m_Controls->m_configurationWidget->setFocus(); } void QmitkMITKIGTTrackingToolboxView::OnLoadTools() { //read in filename QString filename = QFileDialog::getOpenFileName(nullptr, tr("Open Tool Storage"), "/", tr("Tool Storage Files (*.IGTToolStorage)")); if (filename.isNull()) return; //read tool storage from disk std::string errorMessage = ""; mitk::NavigationToolStorageDeserializer::Pointer myDeserializer = mitk::NavigationToolStorageDeserializer::New(GetDataStorage()); // try-catch block for exceptions try { this->ReplaceCurrentToolStorage(myDeserializer->Deserialize(filename.toStdString()), filename.toStdString()); } catch (mitk::IGTException) { std::string errormessage = "Error during loading the tool storage file. Please only load tool storage files created with the NavigationToolManager view."; QMessageBox::warning(nullptr, "Tool Storage Loading Error", errormessage.c_str()); return; } if (m_toolStorage->isEmpty()) { errorMessage = myDeserializer->GetErrorMessage(); MessageBox(errorMessage); return; } //update label UpdateToolStorageLabel(filename); //update tool preview m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); //save filename for persistent storage m_ToolStorageFilename = filename; } void QmitkMITKIGTTrackingToolboxView::OnResetTools() { this->ReplaceCurrentToolStorage(mitk::NavigationToolStorage::New(GetDataStorage()), "TrackingToolbox Default Storage"); m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); QString toolLabel = QString(""); m_Controls->m_toolLabel->setText(toolLabel); m_ToolStorageFilename = ""; } void QmitkMITKIGTTrackingToolboxView::OnStartStopTracking() { if (!m_connected) { MITK_WARN << "Can't start tracking if no device is connected. Aborting"; return; } if (m_tracking) { OnStopTracking(); } else { OnStartTracking(); } } void QmitkMITKIGTTrackingToolboxView::OnFreezeUnfreezeTracking() { if (m_Controls->m_FreezeUnfreezeTrackingButton->text() == "Freeze Tracking") { m_TrackingDeviceSource->Freeze(); m_Controls->m_FreezeUnfreezeTrackingButton->setText("Unfreeze Tracking"); m_Controls->m_TrackingFrozenLabel->setVisible(true); } else if (m_Controls->m_FreezeUnfreezeTrackingButton->text() == "Unfreeze Tracking") { m_TrackingDeviceSource->UnFreeze(); m_Controls->m_FreezeUnfreezeTrackingButton->setText("Freeze Tracking"); m_Controls->m_TrackingFrozenLabel->setVisible(false); } } void QmitkMITKIGTTrackingToolboxView::OnConnectDisconnect() { if (m_connected) { OnDisconnect(); } else { OnConnect(); } } void QmitkMITKIGTTrackingToolboxView::OnConnect() { MITK_INFO << "Connect Clicked"; //check if everything is ready to start tracking if (this->m_toolStorage.IsNull()) { MessageBox("Error: No Tools Loaded Yet!"); return; } else if (this->m_toolStorage->GetToolCount() == 0) { MessageBox("Error: No Way To Track Without Tools!"); return; } //parse tracking device data mitk::TrackingDeviceData data = mitk::UnspecifiedTrackingTypeInformation::GetDeviceDataUnspecified(); QString qstr = m_Controls->m_VolumeSelectionBox->currentText(); if ((!qstr.isNull()) || (!qstr.isEmpty())) { std::string str = qstr.toStdString(); data = m_DeviceTypeCollection->GetDeviceDataByName(str); //Data will be set later, after device generation } //! [Thread 4] //initialize worker thread m_Worker->SetWorkerMethod(QmitkMITKIGTTrackingToolboxViewWorker::eConnectDevice); m_Worker->SetTrackingDevice(this->m_Controls->m_configurationWidget->GetTrackingDevice()); m_Worker->SetInverseMode(m_Controls->m_InverseMode->isChecked()); m_Worker->SetNavigationToolStorage(this->m_toolStorage); m_Worker->SetTrackingDeviceData(data); //start worker thread m_WorkerThread->start(); //! [Thread 4] //disable buttons this->m_Controls->m_MainWidget->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::EnableDisableTimerButtons(int enable) { bool enableBool = enable; m_Controls->m_UpdateRateOptionsGroupBox->setEnabled(!enableBool); m_Controls->m_renderWarningLabel->setVisible(enableBool); } void QmitkMITKIGTTrackingToolboxView::OnConnectFinished(bool success, QString errorMessage) { m_WorkerThread->quit(); m_WorkerThread->wait(); //enable buttons this->m_Controls->m_MainWidget->setEnabled(true); if (!success) { MITK_WARN << errorMessage.toStdString(); MessageBox(errorMessage.toStdString()); return; } //! [Thread 6] //get data from worker thread m_TrackingDeviceSource = m_Worker->GetTrackingDeviceSource(); m_TrackingDeviceData = m_Worker->GetTrackingDeviceData(); m_ToolVisualizationFilter = m_Worker->GetToolVisualizationFilter(); //! [Thread 6] //enable/disable Buttons DisableOptionsButtons(); DisableTrackingConfigurationButtons(); m_Controls->m_TrackingControlLabel->setText("Status: connected"); m_Controls->m_ConnectDisconnectButton->setText("Disconnect"); m_Controls->m_ConnectSimpleMode->setText("Disconnect"); m_Controls->m_StartStopTrackingButton->setEnabled(true); m_Controls->m_StartTrackingSimpleMode->setEnabled(true); m_connected = true; } void QmitkMITKIGTTrackingToolboxView::OnDisconnect() { m_Worker->SetWorkerMethod(QmitkMITKIGTTrackingToolboxViewWorker::eDisconnectDevice); m_WorkerThread->start(); m_Controls->m_MainWidget->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::OnDisconnectFinished(bool success, QString errorMessage) { m_WorkerThread->quit(); m_WorkerThread->wait(); m_Controls->m_MainWidget->setEnabled(true); if (!success) { MITK_WARN << errorMessage.toStdString(); MessageBox(errorMessage.toStdString()); return; } //enable/disable Buttons m_Controls->m_StartStopTrackingButton->setEnabled(false); m_Controls->m_StartTrackingSimpleMode->setEnabled(false); EnableOptionsButtons(); EnableTrackingConfigurationButtons(); m_Controls->m_TrackingControlLabel->setText("Status: disconnected"); m_Controls->m_ConnectDisconnectButton->setText("Connect"); m_Controls->m_ConnectSimpleMode->setText("Connect"); m_Controls->m_FreezeUnfreezeTrackingButton->setText("Freeze Tracking"); m_Controls->m_TrackingFrozenLabel->setVisible(false); m_connected = false; } void QmitkMITKIGTTrackingToolboxView::OnStartTracking() { //show tracking volume this->OnTrackingVolumeChanged(m_Controls->m_VolumeSelectionBox->currentText()); //Reset the view to a defined start. Do it here and not in OnStartTrackingFinished, to give other tracking devices the chance to reset the view to a different direction. this->GlobalReinit(); m_Worker->SetWorkerMethod(QmitkMITKIGTTrackingToolboxViewWorker::eStartTracking); m_WorkerThread->start(); this->m_Controls->m_MainWidget->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::OnStartTrackingFinished(bool success, QString errorMessage) { //! [Thread 5] m_WorkerThread->quit(); m_WorkerThread->wait(); //! [Thread 5] this->m_Controls->m_MainWidget->setEnabled(true); if (!success) { MessageBox(errorMessage.toStdString()); MITK_WARN << errorMessage.toStdString(); return; } if (!(m_Controls->m_DisableAllTimers->isChecked())) { if (m_Controls->m_UseDifferentUpdateRates->isChecked()) { if (m_Controls->m_RenderUpdateRate->value() != 0) m_TrackingRenderTimer->start(1000 / (m_Controls->m_RenderUpdateRate->value())); m_TrackingLoggingTimer->start(1000 / (m_Controls->m_LogUpdateRate->value())); } else { m_TrackingRenderTimer->start(1000 / (m_Controls->m_UpdateRate->value())); m_TrackingLoggingTimer->start(1000 / (m_Controls->m_UpdateRate->value())); } } m_Controls->m_TrackingControlLabel->setText("Status: tracking"); //connect the tool visualization widget for (std::size_t i = 0; i < m_TrackingDeviceSource->GetNumberOfOutputs(); i++) { m_Controls->m_TrackingToolsStatusWidget->AddNavigationData(m_TrackingDeviceSource->GetOutput(i)); } m_Controls->m_TrackingToolsStatusWidget->ShowStatusLabels(); if (m_Controls->m_ShowToolQuaternions->isChecked()) { m_Controls->m_TrackingToolsStatusWidget->SetShowQuaternions(true); } else { m_Controls->m_TrackingToolsStatusWidget->SetShowQuaternions(false); } //if activated enable open IGT link microservice if (m_Controls->m_EnableOpenIGTLinkMicroService->isChecked()) { //create convertion filter m_IGTLConversionFilter = mitk::NavigationDataToIGTLMessageFilter::New(); m_IGTLConversionFilter->SetName("IGT Tracking Toolbox"); QString dataModeSelection = this->m_Controls->m_OpenIGTLinkDataFormat->currentText(); if (dataModeSelection == "TDATA") {m_IGTLConversionFilter->SetOperationMode(mitk::NavigationDataToIGTLMessageFilter::ModeSendTDataMsg);} else if (dataModeSelection == "TRANSFORM") {m_IGTLConversionFilter->SetOperationMode(mitk::NavigationDataToIGTLMessageFilter::ModeSendTransMsg);} else if (dataModeSelection == "QTDATA") {m_IGTLConversionFilter->SetOperationMode(mitk::NavigationDataToIGTLMessageFilter::ModeSendQTDataMsg);} else if (dataModeSelection == "POSITION") {m_IGTLConversionFilter->SetOperationMode(mitk::NavigationDataToIGTLMessageFilter::ModeSendQTransMsg);} m_IGTLConversionFilter->ConnectTo(m_ToolVisualizationFilter); m_IGTLConversionFilter->RegisterAsMicroservice(); //create server and message provider m_IGTLServer = mitk::IGTLServer::New(false); m_IGTLServer->SetName("Tracking Toolbox IGTL Server"); m_IGTLMessageProvider = mitk::IGTLMessageProvider::New(); m_IGTLMessageProvider->SetIGTLDevice(m_IGTLServer); m_IGTLMessageProvider->RegisterAsMicroservice(); } m_tracking = true; m_Controls->m_ConnectDisconnectButton->setEnabled(false); m_Controls->m_StartStopTrackingButton->setText("Stop Tracking"); m_Controls->m_StartTrackingSimpleMode->setText("Stop\nTracking"); m_Controls->m_FreezeUnfreezeTrackingButton->setEnabled(true); } void QmitkMITKIGTTrackingToolboxView::OnStopTracking() { if (!m_tracking) return; m_TrackingRenderTimer->stop(); m_TrackingLoggingTimer->stop(); m_Worker->SetWorkerMethod(QmitkMITKIGTTrackingToolboxViewWorker::eStopTracking); m_WorkerThread->start(); m_Controls->m_MainWidget->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::OnStopTrackingFinished(bool success, QString errorMessage) { m_WorkerThread->quit(); m_WorkerThread->wait(); m_Controls->m_MainWidget->setEnabled(true); if (!success) { MessageBox(errorMessage.toStdString()); MITK_WARN << errorMessage.toStdString(); return; } m_Controls->m_TrackingControlLabel->setText("Status: connected"); if (m_logging) StopLogging(); m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); m_tracking = false; m_Controls->m_StartStopTrackingButton->setText("Start Tracking"); m_Controls->m_StartTrackingSimpleMode->setText("Start\nTracking"); m_Controls->m_ConnectDisconnectButton->setEnabled(true); m_Controls->m_FreezeUnfreezeTrackingButton->setEnabled(false); //unregister open IGT link micro service if (m_Controls->m_EnableOpenIGTLinkMicroService->isChecked()) { m_IGTLConversionFilter->UnRegisterMicroservice(); m_IGTLMessageProvider->UnRegisterMicroservice(); } } void QmitkMITKIGTTrackingToolboxView::OnTrackingDeviceChanged() { mitk::TrackingDeviceType Type; if (m_Controls->m_configurationWidget->GetTrackingDevice().IsNotNull()) { Type = m_Controls->m_configurationWidget->GetTrackingDevice()->GetType(); //enable controls because device is valid m_Controls->m_TrackingToolsGoupBox->setEnabled(true); m_Controls->m_TrackingControlsGroupBox->setEnabled(true); } else { Type = mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName(); MessageBox("Error: This tracking device is not included in this project. Please make sure that the device is installed and activated in your MITK build."); m_Controls->m_TrackingToolsGoupBox->setEnabled(false); m_Controls->m_TrackingControlsGroupBox->setEnabled(false); return; } // Code to enable/disable device specific buttons if (m_Controls->m_configurationWidget->GetTrackingDevice()->AutoDetectToolsAvailable()) { m_Controls->m_AutoDetectTools->setVisible(true); } else { m_Controls->m_AutoDetectTools->setVisible(false); } if (Type == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) //Aurora { m_Controls->m_AddSingleTool->setEnabled(false);} else //other trackers { m_Controls->m_AddSingleTool->setEnabled(true); } // Code to select appropriate tracking volume for current type std::vector Compatibles = m_DeviceTypeCollection->GetDeviceDataForLine(Type); m_Controls->m_VolumeSelectionBox->clear(); for (std::size_t i = 0; i < Compatibles.size(); i++) { m_Controls->m_VolumeSelectionBox->addItem(Compatibles[i].Model.c_str()); } } void QmitkMITKIGTTrackingToolboxView::OnTrackingVolumeChanged(QString qstr) { if (qstr.isNull()) return; if (qstr.isEmpty()) return; mitk::TrackingVolumeGenerator::Pointer volumeGenerator = mitk::TrackingVolumeGenerator::New(); std::string str = qstr.toStdString(); mitk::TrackingDeviceData data = m_DeviceTypeCollection->GetDeviceDataByName(str); m_TrackingDeviceData = data; volumeGenerator->SetTrackingDeviceData(data); volumeGenerator->Update(); mitk::Surface::Pointer volumeSurface = volumeGenerator->GetOutput(); m_TrackingVolumeNode->SetData(volumeSurface); if (!m_Controls->m_ShowTrackingVolume->isChecked()) m_TrackingVolumeNode->SetOpacity(0.0); else m_TrackingVolumeNode->SetOpacity(0.25); GlobalReinit(); } void QmitkMITKIGTTrackingToolboxView::OnShowTrackingVolumeChanged() { if (m_Controls->m_ShowTrackingVolume->isChecked()) { OnTrackingVolumeChanged(m_Controls->m_VolumeSelectionBox->currentText()); m_TrackingVolumeNode->SetOpacity(0.25); } else { m_TrackingVolumeNode->SetOpacity(0.0); } } void QmitkMITKIGTTrackingToolboxView::OnAutoDetectTools() { if (m_Controls->m_configurationWidget->GetTrackingDevice()->AutoDetectToolsAvailable()) { DisableTrackingConfigurationButtons(); m_Worker->SetWorkerMethod(QmitkMITKIGTTrackingToolboxViewWorker::eAutoDetectTools); m_Worker->SetTrackingDevice(m_Controls->m_configurationWidget->GetTrackingDevice().GetPointer()); m_Worker->SetDataStorage(this->GetDataStorage()); m_WorkerThread->start(); m_TimeoutTimer->start(5000); //disable controls until worker thread is finished this->m_Controls->m_MainWidget->setEnabled(false); } } void QmitkMITKIGTTrackingToolboxView::OnAutoDetectToolsFinished(bool success, QString errorMessage) { m_TimeoutTimer->stop(); m_WorkerThread->quit(); m_WorkerThread->wait(); //enable controls again this->m_Controls->m_MainWidget->setEnabled(true); EnableTrackingConfigurationButtons(); if (!success) { MITK_WARN << errorMessage.toStdString(); MessageBox(errorMessage.toStdString()); EnableTrackingConfigurationButtons(); return; } mitk::NavigationToolStorage::Pointer autoDetectedStorage = m_Worker->GetNavigationToolStorage(); //save detected tools this->ReplaceCurrentToolStorage(autoDetectedStorage, "Autodetected NDI Aurora Storage"); //auto save the new storage to hard disc (for persistence) AutoSaveToolStorage(); //update label QString toolLabel = QString("Loaded Tools: ") + QString::number(m_toolStorage->GetToolCount()) + " Tools (Auto Detected)"; m_Controls->m_toolLabel->setText(toolLabel); //update tool preview m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); EnableTrackingConfigurationButtons(); //print a logging message about the detected tools switch (m_toolStorage->GetToolCount()) { case 0: MITK_INFO("IGT Tracking Toolbox") << "Found no tools. Empty ToolStorage was autosaved to " << m_ToolStorageFilename.toStdString(); break; case 1: MITK_INFO("IGT Tracking Toolbox") << "Found one tool. ToolStorage was autosaved to " << m_ToolStorageFilename.toStdString(); break; default: MITK_INFO("IGT Tracking Toolbox") << "Found " << m_toolStorage->GetToolCount() << " tools. ToolStorage was autosaved to " << m_ToolStorageFilename.toStdString(); } } void QmitkMITKIGTTrackingToolboxView::MessageBox(std::string s) { QMessageBox msgBox; msgBox.setText(s.c_str()); msgBox.exec(); } void QmitkMITKIGTTrackingToolboxView::UpdateRenderTrackingTimer() { //update filter m_ToolVisualizationFilter->Update(); MITK_DEBUG << "Number of outputs ToolVisualizationFilter: " << m_ToolVisualizationFilter->GetNumberOfIndexedOutputs(); MITK_DEBUG << "Number of inputs ToolVisualizationFilter: " << m_ToolVisualizationFilter->GetNumberOfIndexedInputs(); //update tool colors to show tool status for (unsigned int i = 0; i < m_ToolVisualizationFilter->GetNumberOfIndexedOutputs(); i++) { mitk::NavigationData::Pointer currentTool = m_ToolVisualizationFilter->GetOutput(i); if (currentTool->IsDataValid()) { this->m_toolStorage->GetTool(i)->GetDataNode()->SetColor(mitk::IGTColor_VALID); } else { this->m_toolStorage->GetTool(i)->GetDataNode()->SetColor(mitk::IGTColor_WARNING); } } //refresh view and status widget mitk::RenderingManager::GetInstance()->RequestUpdateAll(); m_Controls->m_TrackingToolsStatusWidget->Refresh(); } void QmitkMITKIGTTrackingToolboxView::UpdateLoggingTrackingTimer() { //update logging if (m_logging) { this->m_loggingFilter->Update(); m_loggedFrames = this->m_loggingFilter->GetNumberOfRecordedSteps(); this->m_Controls->m_LoggedFramesLabel->setText("Logged Frames: " + QString::number(m_loggedFrames)); //check if logging stopped automatically if ((m_loggedFrames > 1) && (!m_loggingFilter->GetRecording())){ StopLogging(); } } //refresh status widget m_Controls->m_TrackingToolsStatusWidget->Refresh(); } void QmitkMITKIGTTrackingToolboxView::OnChooseFileClicked() { QDir currentPath = QFileInfo(m_Controls->m_LoggingFileName->text()).dir(); // if no path was selected (QDir would select current working dir then) or the // selected path does not exist -> use home directory if (currentPath == QDir() || !currentPath.exists()) { currentPath = QDir(QDir::homePath()); } QString filename = QFileDialog::getSaveFileName(nullptr, tr("Choose Logging File"), currentPath.absolutePath(), "*.*"); if (filename == "") return; this->m_Controls->m_LoggingFileName->setText(filename); this->OnToggleFileExtension(); } // bug-16470: toggle file extension after clicking on radio button void QmitkMITKIGTTrackingToolboxView::OnToggleFileExtension() { QString currentInputText = this->m_Controls->m_LoggingFileName->text(); QString currentFile = QFileInfo(currentInputText).baseName(); QDir currentPath = QFileInfo(currentInputText).dir(); if (currentFile.isEmpty()) { currentFile = "logfile"; } // Setting currentPath to default home path when currentPath is empty or it does not exist if (currentPath == QDir() || !currentPath.exists()) { currentPath = QDir::homePath(); } // check if csv radio button is clicked if (this->m_Controls->m_csvFormat->isChecked()) { // you needn't add a seperator to the input text when currentpath is the rootpath if (currentPath.isRoot()) { this->m_Controls->m_LoggingFileName->setText(QDir::toNativeSeparators(currentPath.absolutePath()) + currentFile + ".csv"); } else { this->m_Controls->m_LoggingFileName->setText(QDir::toNativeSeparators(currentPath.absolutePath()) + QDir::separator() + currentFile + ".csv"); } } // check if xml radio button is clicked else if (this->m_Controls->m_xmlFormat->isChecked()) { // you needn't add a seperator to the input text when currentpath is the rootpath if (currentPath.isRoot()) { this->m_Controls->m_LoggingFileName->setText(QDir::toNativeSeparators(currentPath.absolutePath()) + currentFile + ".xml"); } else { this->m_Controls->m_LoggingFileName->setText(QDir::toNativeSeparators(currentPath.absolutePath()) + QDir::separator() + currentFile + ".xml"); } } } void QmitkMITKIGTTrackingToolboxView::OnToggleAdvancedSimpleMode() { if (m_SimpleModeEnabled) { m_Controls->m_simpleWidget->setVisible(false); m_Controls->m_MainWidget->setVisible(true); m_Controls->m_simpleUI->setChecked(false); m_SimpleModeEnabled = false; } else { m_Controls->m_simpleWidget->setVisible(true); m_Controls->m_MainWidget->setVisible(false); m_SimpleModeEnabled = true; } } void QmitkMITKIGTTrackingToolboxView::OnToggleDifferentUpdateRates() { if (m_Controls->m_UseDifferentUpdateRates->isChecked()) { if (m_Controls->m_RenderUpdateRate->value() == 0) m_Controls->m_renderWarningLabel->setVisible(true); else m_Controls->m_renderWarningLabel->setVisible(false); m_Controls->m_UpdateRate->setEnabled(false); m_Controls->m_OptionsUpdateRateLabel->setEnabled(false); m_Controls->m_RenderUpdateRate->setEnabled(true); m_Controls->m_OptionsRenderUpdateRateLabel->setEnabled(true); m_Controls->m_LogUpdateRate->setEnabled(true); m_Controls->m_OptionsLogUpdateRateLabel->setEnabled(true); } else { m_Controls->m_renderWarningLabel->setVisible(false); m_Controls->m_UpdateRate->setEnabled(true); m_Controls->m_OptionsUpdateRateLabel->setEnabled(true); m_Controls->m_RenderUpdateRate->setEnabled(false); m_Controls->m_OptionsRenderUpdateRateLabel->setEnabled(false); m_Controls->m_LogUpdateRate->setEnabled(false); m_Controls->m_OptionsLogUpdateRateLabel->setEnabled(false); } } void QmitkMITKIGTTrackingToolboxView::OnChangeRenderUpdateRate() { if (m_Controls->m_RenderUpdateRate->value() == 0) m_Controls->m_renderWarningLabel->setVisible(true); else m_Controls->m_renderWarningLabel->setVisible(false); } void QmitkMITKIGTTrackingToolboxView::StartLogging() { if (m_ToolVisualizationFilter.IsNull()) { MessageBox("Cannot activate logging without a connected device. Configure and connect a tracking device first."); return; } if (!m_logging) { //initialize logging filter m_loggingFilter = mitk::NavigationDataRecorder::New(); m_loggingFilter->SetRecordOnlyValidData(m_Controls->m_SkipInvalidData->isChecked()); m_loggingFilter->ConnectTo(m_ToolVisualizationFilter); if (m_Controls->m_LoggingLimit->isChecked()){ m_loggingFilter->SetRecordCountLimit(m_Controls->m_LoggedFramesLimit->value()); } //start filter with try-catch block for exceptions try { m_loggingFilter->StartRecording(); } catch (mitk::IGTException) { std::string errormessage = "Error during start recording. Recorder already started recording?"; QMessageBox::warning(nullptr, "IGTPlayer: Error", errormessage.c_str()); m_loggingFilter->StopRecording(); return; } //update labels / logging variables this->m_Controls->m_LoggingLabel->setText("Logging ON"); this->m_Controls->m_LoggedFramesLabel->setText("Logged Frames: 0"); m_loggedFrames = 0; m_logging = true; DisableLoggingButtons(); } } void QmitkMITKIGTTrackingToolboxView::StopLogging() { if (m_logging) { //stop logging m_loggingFilter->StopRecording(); m_logging = false; //update GUI this->m_Controls->m_LoggingLabel->setText("Logging OFF"); EnableLoggingButtons(); //write the results to a file if (m_Controls->m_csvFormat->isChecked() || m_Controls->m_xmlFormat->isChecked()) mitk::IOUtil::Save(m_loggingFilter->GetNavigationDataSet(), this->m_Controls->m_LoggingFileName->text().toStdString()); } } void QmitkMITKIGTTrackingToolboxView::OnAddSingleTool() { QString Identifier = "Tool#"; QString Name = "NewTool"; if (m_toolStorage.IsNotNull()) { Identifier += QString::number(m_toolStorage->GetToolCount()); Name += QString::number(m_toolStorage->GetToolCount()); } else { Identifier += "0"; Name += "0"; } m_Controls->m_NavigationToolCreationWidget->Initialize(GetDataStorage(), Identifier.toStdString(), Name.toStdString()); m_Controls->m_NavigationToolCreationWidget->SetTrackingDeviceType(m_Controls->m_configurationWidget->GetTrackingDevice()->GetType(), false); m_Controls->m_TrackingToolsWidget->setCurrentIndex(1); //disable tracking volume during tool editing lastTrackingVolumeState = m_Controls->m_ShowTrackingVolume->isChecked(); if (lastTrackingVolumeState) m_Controls->m_ShowTrackingVolume->click(); GlobalReinit(); } void QmitkMITKIGTTrackingToolboxView::OnAddSingleToolFinished() { m_Controls->m_TrackingToolsWidget->setCurrentIndex(0); if (this->m_toolStorage.IsNull()) { //this shouldn't happen! MITK_WARN << "No ToolStorage available, cannot add tool, aborting!"; return; } m_toolStorage->AddTool(m_Controls->m_NavigationToolCreationWidget->GetCreatedTool()); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); m_Controls->m_toolLabel->setText(""); //auto save current storage for persistence MITK_INFO << "Auto saving manually added tools for persistence."; AutoSaveToolStorage(); //enable tracking volume again if (lastTrackingVolumeState) m_Controls->m_ShowTrackingVolume->click(); GlobalReinit(); } void QmitkMITKIGTTrackingToolboxView::OnAddSingleToolCanceled() { m_Controls->m_TrackingToolsWidget->setCurrentIndex(0); //enable tracking volume again if (lastTrackingVolumeState) m_Controls->m_ShowTrackingVolume->click(); GlobalReinit(); } void QmitkMITKIGTTrackingToolboxView::GlobalReinit() { // get all nodes that have not set "includeInBoundingBox" to false mitk::NodePredicateNot::Pointer pred = mitk::NodePredicateNot::New(mitk::NodePredicateProperty::New("includeInBoundingBox", mitk::BoolProperty::New(false))); mitk::DataStorage::SetOfObjects::ConstPointer rs = this->GetDataStorage()->GetSubset(pred); // calculate bounding geometry of these nodes mitk::TimeGeometry::Pointer bounds = this->GetDataStorage()->ComputeBoundingGeometry3D(rs, "visible"); // initialize the views to the bounding geometry mitk::RenderingManager::GetInstance()->InitializeViews(bounds); } void QmitkMITKIGTTrackingToolboxView::DisableLoggingButtons() { m_Controls->m_StartLogging->setEnabled(false); m_Controls->m_LoggingFileName->setEnabled(false); m_Controls->m_ChooseFile->setEnabled(false); m_Controls->m_LoggingLimit->setEnabled(false); m_Controls->m_LoggedFramesLimit->setEnabled(false); m_Controls->m_csvFormat->setEnabled(false); m_Controls->m_xmlFormat->setEnabled(false); m_Controls->m_SkipInvalidData->setEnabled(false); m_Controls->m_StopLogging->setEnabled(true); } void QmitkMITKIGTTrackingToolboxView::EnableLoggingButtons() { m_Controls->m_StartLogging->setEnabled(true); m_Controls->m_LoggingFileName->setEnabled(true); m_Controls->m_ChooseFile->setEnabled(true); m_Controls->m_LoggingLimit->setEnabled(true); m_Controls->m_LoggedFramesLimit->setEnabled(true); m_Controls->m_csvFormat->setEnabled(true); m_Controls->m_xmlFormat->setEnabled(true); m_Controls->m_SkipInvalidData->setEnabled(true); m_Controls->m_StopLogging->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::DisableOptionsButtons() { m_Controls->m_ShowTrackingVolume->setEnabled(false); m_Controls->m_UseDifferentUpdateRates->setEnabled(false); m_Controls->m_UpdateRate->setEnabled(false); m_Controls->m_OptionsUpdateRateLabel->setEnabled(false); m_Controls->m_RenderUpdateRate->setEnabled(false); m_Controls->m_OptionsRenderUpdateRateLabel->setEnabled(false); m_Controls->m_LogUpdateRate->setEnabled(false); m_Controls->m_OptionsLogUpdateRateLabel->setEnabled(false); m_Controls->m_DisableAllTimers->setEnabled(false); m_Controls->m_OtherOptionsGroupBox->setEnabled(false); m_Controls->m_EnableOpenIGTLinkMicroService->setEnabled(false); m_Controls->m_OpenIGTLinkDataFormat->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::EnableOptionsButtons() { m_Controls->m_ShowTrackingVolume->setEnabled(true); m_Controls->m_UseDifferentUpdateRates->setEnabled(true); m_Controls->m_DisableAllTimers->setEnabled(true); m_Controls->m_OtherOptionsGroupBox->setEnabled(true); m_Controls->m_EnableOpenIGTLinkMicroService->setEnabled(true); m_Controls->m_OpenIGTLinkDataFormat->setEnabled(true); OnToggleDifferentUpdateRates(); } void QmitkMITKIGTTrackingToolboxView::EnableTrackingControls() { m_Controls->m_TrackingControlsGroupBox->setEnabled(true); } void QmitkMITKIGTTrackingToolboxView::DisableTrackingControls() { m_Controls->m_TrackingControlsGroupBox->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::EnableTrackingConfigurationButtons() { m_Controls->m_AutoDetectTools->setEnabled(true); if (m_Controls->m_configurationWidget->GetTrackingDevice()->GetType() != mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) m_Controls->m_AddSingleTool->setEnabled(true); m_Controls->m_LoadTools->setEnabled(true); m_Controls->m_ResetTools->setEnabled(true); } void QmitkMITKIGTTrackingToolboxView::DisableTrackingConfigurationButtons() { m_Controls->m_AutoDetectTools->setEnabled(false); if (m_Controls->m_configurationWidget->GetTrackingDevice()->GetType() != mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) m_Controls->m_AddSingleTool->setEnabled(false); m_Controls->m_LoadTools->setEnabled(false); m_Controls->m_ResetTools->setEnabled(false); } void QmitkMITKIGTTrackingToolboxView::ReplaceCurrentToolStorage(mitk::NavigationToolStorage::Pointer newStorage, std::string newStorageName) { //first: get rid of the old one //don't reset if there is no tool storage. BugFix #17793 if (m_toolStorage.IsNotNull()){ m_toolStorage->UnLockStorage(); //only to be sure... m_toolStorage->UnRegisterMicroservice(); m_toolStorage = nullptr; } //now: replace by the new one m_toolStorage = newStorage; m_toolStorage->SetName(newStorageName); m_toolStorage->RegisterAsMicroservice("no tracking device"); } void QmitkMITKIGTTrackingToolboxView::OnTimeOut() { m_WorkerThread->terminate(); m_WorkerThread->wait(); m_TimeoutTimer->stop(); } //! [StoreUISettings] void QmitkMITKIGTTrackingToolboxView::StoreUISettings() { // persistence service does not directly work in plugins for now // -> using QSettings QSettings settings; settings.beginGroup(QString::fromStdString(VIEW_ID)); - MITK_INFO << "Store UI settings"; + MITK_DEBUG << "Store UI settings"; // set the values of some widgets and attrbutes to the QSettings settings.setValue("ShowTrackingVolume", QVariant(m_Controls->m_ShowTrackingVolume->isChecked())); settings.setValue("toolStorageFilename", QVariant(m_ToolStorageFilename)); settings.setValue("VolumeSelectionBox", QVariant(m_Controls->m_VolumeSelectionBox->currentIndex())); settings.setValue("SimpleModeEnabled", QVariant(m_SimpleModeEnabled)); - + settings.setValue("OpenIGTLinkDataFormat", QVariant(m_Controls->m_OpenIGTLinkDataFormat->currentIndex())); + settings.setValue("EnableOpenIGTLinkMicroService", QVariant(m_Controls->m_EnableOpenIGTLinkMicroService->isChecked())); settings.endGroup(); } //! [StoreUISettings] //! [LoadUISettings] void QmitkMITKIGTTrackingToolboxView::LoadUISettings() { - // persistence service does not directly work in plugins for now - // -> using QSettings + // persistence service does not directly work in plugins for now -> using QSettings QSettings settings; settings.beginGroup(QString::fromStdString(VIEW_ID)); // set some widgets and attributes by the values from the QSettings m_Controls->m_ShowTrackingVolume->setChecked(settings.value("ShowTrackingVolume", true).toBool()); + m_Controls->m_EnableOpenIGTLinkMicroService->setChecked(settings.value("EnableOpenIGTLinkMicroService", true).toBool()); m_Controls->m_VolumeSelectionBox->setCurrentIndex(settings.value("VolumeSelectionBox", 0).toInt()); + m_Controls->m_OpenIGTLinkDataFormat->setCurrentIndex(settings.value("OpenIGTLinkDataFormat", 0).toInt()); m_ToolStorageFilename = settings.value("toolStorageFilename", QVariant("")).toString(); if (settings.value("SimpleModeEnabled", false).toBool()) { this->OnToggleAdvancedSimpleMode(); } settings.endGroup(); //! [LoadUISettings] //! [LoadToolStorage] // try to deserialize the tool storage from the given tool storage file name if (!m_ToolStorageFilename.isEmpty()) { // try-catch block for exceptions try { mitk::NavigationToolStorageDeserializer::Pointer myDeserializer = mitk::NavigationToolStorageDeserializer::New(GetDataStorage()); m_toolStorage->UnRegisterMicroservice(); m_toolStorage = myDeserializer->Deserialize(m_ToolStorageFilename.toStdString()); m_toolStorage->RegisterAsMicroservice("no tracking device"); //update label UpdateToolStorageLabel(m_ToolStorageFilename); //update tool preview m_Controls->m_TrackingToolsStatusWidget->RemoveStatusLabels(); m_Controls->m_TrackingToolsStatusWidget->PreShowTools(m_toolStorage); } catch (mitk::IGTException) { MITK_WARN("QmitkMITKIGTTrackingToolBoxView") << "Error during restoring tools. Problems with file (" << m_ToolStorageFilename.toStdString() << "), please check the file?"; this->OnResetTools(); //if there where errors reset the tool storage to avoid problems later on } } //! [LoadToolStorage] } void QmitkMITKIGTTrackingToolboxView::UpdateToolStorageLabel(QString pathOfLoadedStorage) { QFileInfo myPath(pathOfLoadedStorage); //use this to seperate filename from path QString toolLabel = myPath.fileName(); if (toolLabel.size() > 45) //if the tool storage name is to long trimm the string { toolLabel.resize(40); toolLabel += "[...]"; } m_Controls->m_toolLabel->setText(toolLabel); } void QmitkMITKIGTTrackingToolboxView::AutoSaveToolStorage() { m_ToolStorageFilename = m_AutoSaveFilename; mitk::NavigationToolStorageSerializer::Pointer mySerializer = mitk::NavigationToolStorageSerializer::New(); mySerializer->Serialize(m_ToolStorageFilename.toStdString(), m_toolStorage); } void QmitkMITKIGTTrackingToolboxViewWorker::SetWorkerMethod(WorkerMethod w) { m_WorkerMethod = w; } void QmitkMITKIGTTrackingToolboxViewWorker::SetTrackingDevice(mitk::TrackingDevice::Pointer t) { m_TrackingDevice = t; } void QmitkMITKIGTTrackingToolboxViewWorker::SetDataStorage(mitk::DataStorage::Pointer d) { m_DataStorage = d; } void QmitkMITKIGTTrackingToolboxViewWorker::SetInverseMode(bool mode) { m_InverseMode = mode; } void QmitkMITKIGTTrackingToolboxViewWorker::SetTrackingDeviceData(mitk::TrackingDeviceData d) { m_TrackingDeviceData = d; } void QmitkMITKIGTTrackingToolboxViewWorker::SetNavigationToolStorage(mitk::NavigationToolStorage::Pointer n) { m_NavigationToolStorage = n; } //! [Thread 7] void QmitkMITKIGTTrackingToolboxViewWorker::ThreadFunc() { switch (m_WorkerMethod) { case eAutoDetectTools: this->AutoDetectTools(); break; case eConnectDevice: this->ConnectDevice(); break; case eStartTracking: this->StartTracking(); break; case eStopTracking: this->StopTracking(); break; case eDisconnectDevice: this->DisconnectDevice(); break; default: MITK_WARN << "Undefined worker method was set ... something went wrong!"; break; } } //! [Thread 7] void QmitkMITKIGTTrackingToolboxViewWorker::AutoDetectTools() { //mitk::ProgressBar::GetInstance()->AddStepsToDo(2); mitk::NavigationToolStorage::Pointer autoDetectedStorage = mitk::NavigationToolStorage::New(m_DataStorage); try { mitk::NavigationToolStorage::Pointer tempStorage = m_TrackingDevice->AutoDetectTools(); //mitk::ProgressBar::GetInstance()->Progress(); for (int i = 0; i < tempStorage->GetToolCount(); i++) { autoDetectedStorage->AddTool(tempStorage->GetTool(i)); } } catch (mitk::Exception& e) { MITK_WARN << e.GetDescription(); //mitk::ProgressBar::GetInstance()->Reset(); emit AutoDetectToolsFinished(false, e.GetDescription()); return; } m_NavigationToolStorage = autoDetectedStorage; //::ProgressBar::GetInstance()->Progress(); emit AutoDetectToolsFinished(true, ""); } void QmitkMITKIGTTrackingToolboxViewWorker::ConnectDevice() { std::string message = ""; //mitk::ProgressBar::GetInstance()->AddStepsToDo(4); //build the IGT pipeline mitk::TrackingDevice::Pointer trackingDevice = m_TrackingDevice; trackingDevice->SetData(m_TrackingDeviceData); //set device to rotation mode transposed becaus we are working with VNL style quaternions if (m_InverseMode) { trackingDevice->SetRotationMode(mitk::TrackingDevice::RotationTransposed); } //Get Tracking Volume Data mitk::TrackingDeviceData data = m_TrackingDeviceData; //mitk::ProgressBar::GetInstance()->Progress(); //Create Navigation Data Source with the factory class mitk::TrackingDeviceSourceConfigurator::Pointer myTrackingDeviceSourceFactory = mitk::TrackingDeviceSourceConfigurator::New(m_NavigationToolStorage, trackingDevice); m_TrackingDeviceSource = myTrackingDeviceSourceFactory->CreateTrackingDeviceSource(m_ToolVisualizationFilter); //mitk::ProgressBar::GetInstance()->Progress(); if (m_TrackingDeviceSource.IsNull()) { message = std::string("Cannot connect to device: ") + myTrackingDeviceSourceFactory->GetErrorMessage(); emit ConnectDeviceFinished(false, QString(message.c_str())); return; } //set filter to rotation mode transposed becaus we are working with VNL style quaternions if (m_InverseMode) m_ToolVisualizationFilter->SetRotationMode(mitk::NavigationDataObjectVisualizationFilter::RotationTransposed); //First check if the created object is valid if (m_TrackingDeviceSource.IsNull()) { message = myTrackingDeviceSourceFactory->GetErrorMessage(); emit ConnectDeviceFinished(false, QString(message.c_str())); return; } MITK_INFO << "Number of tools: " << m_TrackingDeviceSource->GetNumberOfOutputs(); //mitk::ProgressBar::GetInstance()->Progress(); //The tools are maybe reordered after initialization, e.g. in case of auto-detected tools of NDI Aurora mitk::NavigationToolStorage::Pointer toolsInNewOrder = myTrackingDeviceSourceFactory->GetUpdatedNavigationToolStorage(); if ((toolsInNewOrder.IsNotNull()) && (toolsInNewOrder->GetToolCount() > 0)) { //so delete the old tools in wrong order and add them in the right order //we cannot simply replace the tool storage because the new storage is //not correctly initialized with the right data storage /* m_NavigationToolStorage->DeleteAllTools(); for (int i=0; i < toolsInNewOrder->GetToolCount(); i++) {m_NavigationToolStorage->AddTool(toolsInNewOrder->GetTool(i));} This was replaced and thereby fixed Bug 18318 DeleteAllTools() is not Threadsafe! */ for (int i = 0; i < toolsInNewOrder->GetToolCount(); i++) { m_NavigationToolStorage->AssignToolNumber(toolsInNewOrder->GetTool(i)->GetIdentifier(), i); } } //mitk::ProgressBar::GetInstance()->Progress(); //connect to device try { m_TrackingDeviceSource->Connect(); //mitk::ProgressBar::GetInstance()->Reset(); //Microservice registration: m_TrackingDeviceSource->RegisterAsMicroservice(); m_NavigationToolStorage->UnRegisterMicroservice(); m_NavigationToolStorage->RegisterAsMicroservice(m_TrackingDeviceSource->GetMicroserviceID()); m_NavigationToolStorage->LockStorage(); } catch (...) //todo: change to mitk::IGTException { message = "Error on connecting the tracking device."; emit ConnectDeviceFinished(false, QString(message.c_str())); return; } emit ConnectDeviceFinished(true, QString(message.c_str())); //mitk::ProgressBar::GetInstance()->Reset(); } void QmitkMITKIGTTrackingToolboxViewWorker::StartTracking() { QString errorMessage = ""; try { m_TrackingDeviceSource->StartTracking(); } catch (...) //todo: change to mitk::IGTException { errorMessage += "Error while starting the tracking device!"; emit StartTrackingFinished(false, errorMessage); return; } //remember the original colors of the tools m_OriginalColors = std::map(); for (int i = 0; i < this->m_NavigationToolStorage->GetToolCount(); i++) { mitk::DataNode::Pointer currentToolNode = m_NavigationToolStorage->GetTool(i)->GetDataNode(); float c[3]; currentToolNode->GetColor(c); mitk::Color color; color.SetRed(c[0]); color.SetGreen(c[1]); color.SetBlue(c[2]); m_OriginalColors[currentToolNode] = color; } emit StartTrackingFinished(true, errorMessage); } void QmitkMITKIGTTrackingToolboxViewWorker::StopTracking() { //stop tracking try { m_TrackingDeviceSource->StopTracking(); } catch (mitk::Exception& e) { emit StopTrackingFinished(false, e.GetDescription()); } //restore the original colors of the tools for (int i = 0; i < this->m_NavigationToolStorage->GetToolCount(); i++) { mitk::DataNode::Pointer currentToolNode = m_NavigationToolStorage->GetTool(i)->GetDataNode(); if (m_OriginalColors.find(currentToolNode) == m_OriginalColors.end()) { MITK_WARN << "Cannot restore original color of tool " << m_NavigationToolStorage->GetTool(i)->GetToolName(); } else { currentToolNode->SetColor(m_OriginalColors[currentToolNode]); } } //emit signal emit StopTrackingFinished(true, ""); } void QmitkMITKIGTTrackingToolboxViewWorker::DisconnectDevice() { try { if (m_TrackingDeviceSource->IsTracking()) { m_TrackingDeviceSource->StopTracking(); } m_TrackingDeviceSource->Disconnect(); m_TrackingDeviceSource->UnRegisterMicroservice(); m_NavigationToolStorage->UnLockStorage(); } catch (mitk::Exception& e) { emit DisconnectDeviceFinished(false, e.GetDescription()); } emit DisconnectDeviceFinished(true, ""); }