diff --git a/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp b/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp index 7d83c6345e..743edb1cb7 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 == NULL) // if an input is set to NULL, remove it { this->RemoveInput(idx); } else { // ProcessObject is not const-correct so a const_cast is required here this->ProcessObject::SetNthInput(idx, const_cast(msg)); } this->CreateOutputsForAllInputs(); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(void) const { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(this->ProcessObject::GetInput(0)); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(unsigned int idx) const { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(this->ProcessObject::GetInput(idx)); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(std::string messageName) const { const DataObjectPointerArray& inputs = const_cast(this)->GetInputs(); for (DataObjectPointerArray::const_iterator it = inputs.begin(); it != inputs.end(); ++it) { if (std::string(messageName) == (static_cast(it->GetPointer()))->GetName()) { return static_cast(it->GetPointer()); } } return NULL; } itk::ProcessObject::DataObjectPointerArraySizeType mitk::IGTLMessageToNavigationDataFilter::GetInputIndex(std::string messageName) { DataObjectPointerArray outputs = this->GetInputs(); for (DataObjectPointerArray::size_type i = 0; i < outputs.size(); ++i) { if (messageName == (static_cast(outputs.at(i).GetPointer()))->GetName()) { return i; } } throw std::invalid_argument("output name does not exist"); } void mitk::IGTLMessageToNavigationDataFilter::ConnectTo( mitk::IGTLMessageSource* UpstreamFilter) { for (DataObjectPointerArraySizeType i = 0; i < UpstreamFilter->GetNumberOfOutputs(); i++) { this->SetInput(i, UpstreamFilter->GetOutput(i)); } } void mitk::IGTLMessageToNavigationDataFilter::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) == NULL) { 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/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp index 79d17f912c..3a208eceb9 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp @@ -1,523 +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::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); switch (type) { case UNKNOWN: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::UNKNOWN); break; case TDATA: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::TDATA); break; case QTDATA: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::QTDATA); break; case TRANSFORM: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::TRANSFORM); break; } 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(); switch (type) { 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 NULL; return t.GetPointer(); } bool mitk::OpenIGTLinkTrackingDevice::InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::OpenIGTLinkTrackingDevice::DiscoverTools(int waitingTime) { if (m_OpenIGTLinkClient->GetPortNumber() == -1) { MITK_WARN << "Connection not initialized, aborting (invalid port number)."; return false; } try { m_IGTLDeviceSource->Connect(); m_IGTLDeviceSource->StartCommunication(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } 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 = 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; } 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::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 NULL; else return this->m_AllTools[toolNumber]; } bool mitk::OpenIGTLinkTrackingDevice::OpenConnection() { //check tracking state if (this->GetState() != Setup) { MITK_WARN << "Cannot open connection, device is already connected!"; return false; } try { m_IGTLDeviceSource->Connect(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } this->SetState(Ready); return true; } bool mitk::OpenIGTLinkTrackingDevice::CloseConnection() { //check tracking state if (this->GetState() != Ready) { MITK_WARN << "Cannot close connection, device is in the wrong state!"; return false; } try { m_IGTLDeviceSource->Disconnect(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to disconnect: " << e.what(); return false; } this->SetState(Setup); return true; } std::vector mitk::OpenIGTLinkTrackingDevice::GetAllTools() { return this->m_AllTools; } 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/OpenIGTLink/mitkIGTLDevice.cpp b/Modules/OpenIGTLink/mitkIGTLDevice.cpp index adbeba1bf6..ff807c59f1 100644 --- a/Modules/OpenIGTLink/mitkIGTLDevice.cpp +++ b/Modules/OpenIGTLink/mitkIGTLDevice.cpp @@ -1,562 +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); //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(); } 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 NULL 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; }