diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp index 26f9146a74..8dee9f55b0 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp @@ -1,446 +1,446 @@ /*=================================================================== 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 //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_IGTLDeviceSource = mitk::IGTLTransformDeviceSource::New(); m_IGTLDeviceSource->SetIGTLDevice(m_OpenIGTLinkClient); } mitk::OpenIGTLinkTrackingDevice::~OpenIGTLinkTrackingDevice() { } int mitk::OpenIGTLinkTrackingDevice::GetPortNumber() { return m_OpenIGTLinkClient->GetPortNumber(); } std::string mitk::OpenIGTLinkTrackingDevice::GetHostname() { return m_OpenIGTLinkClient->GetHostname(); } void mitk::OpenIGTLinkTrackingDevice::SetPortNumber(int portNumber) { m_OpenIGTLinkClient->SetPortNumber(portNumber); } void mitk::OpenIGTLinkTrackingDevice::SetHostname(std::string hostname) { m_OpenIGTLinkClient->SetHostname(hostname); } 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; } //send a message to the server: start tracking stream mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory(); std::string message = "STT_TDATA"; igtl::MessageBase::Pointer sttMsg = msgFactory->CreateInstance(message); //TODO: Fix this to dynamically get this from GUI ((igtl::StartTrackingDataMessage*)sttMsg.GetPointer())->SetResolution(m_UpdateRate); m_OpenIGTLinkClient->SendMessage(sttMsg); mitk::IGTLMessage::Pointer receivedMessage; 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)); } //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); switch (type) { case TDATA: return DiscoverToolsFromTData(dynamic_cast(receivedMessage->GetMessage().GetPointer())); case QTDATA: return DiscoverToolsFromQTData(dynamic_cast(receivedMessage->GetMessage().GetPointer())); case TRANSFORM: return DiscoverToolsFromTransform(); default: MITK_INFO << "Server does not send tracking data. Received data is not of a compatible type. Received type: " << msgType; return false; } } bool mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTData(igtl::TrackingDataMessage::Pointer tdMsg) { MITK_INFO << "Start discovering tools by TDATA messages"; if (tdMsg == nullptr) { MITK_WARN << "Message was not a TrackingDataMessage, aborting!"; return false; } 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(); AddNewToolForName(name, i); } m_IGTLDeviceSource->StopCommunication(); SetState(Ready); return true; } bool mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg) { MITK_INFO << "Start discovering tools by QTDATA messages"; if (msg == nullptr) { MITK_WARN << "Message was not a QuaternionTrackingDataMessage, aborting!"; return false; } 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(); AddNewToolForName(name, i); } m_IGTLDeviceSource->StopCommunication(); SetState(Ready); return true; } 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); } bool mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTransform() { MITK_INFO << "Start discovering tools by TRANSFORM messages"; std::map toolNameMap; bool condition = false; while (!condition) { //TODO: Fix this.. :/ std::this_thread::sleep_for(std::chrono::milliseconds(20)); m_IGTLDeviceSource->Update(); 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; } condition = true; 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()]; } for (std::map::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it) { if (it->second < 5) { condition = false; break; } } } int i = 0; for (std::map::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it) { AddNewToolForName(it->first, i++); } //TODO InternalAddTool for all tools return true; } 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; + //MITK_WARN << name; for (int i = 0; i < m_AllTools.size(); i++) { 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); 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; } 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; } } \ No newline at end of file diff --git a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.cpp b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.cpp index eba5359c04..25e4699a6d 100644 --- a/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.cpp +++ b/Modules/OpenIGTLink/DeviceSources/mitkIGTLTransformDeviceSource.cpp @@ -1,58 +1,58 @@ /*=================================================================== 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); - MITK_INFO << "Received valid image"; + //MITK_INFO << "Received valid image"; msgOut->SetMessage(msgIn); msgOut->SetName(msgIn->GetDeviceName()); } } diff --git a/Modules/OpenIGTLink/mitkIGTLClient.cpp b/Modules/OpenIGTLink/mitkIGTLClient.cpp index 53df927ec3..953c8a4ad2 100644 --- a/Modules/OpenIGTLink/mitkIGTLClient.cpp +++ b/Modules/OpenIGTLink/mitkIGTLClient.cpp @@ -1,124 +1,124 @@ /*=================================================================== 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 "mitkIGTLClient.h" //#include "mitkIGTTimeStamp.h" //#include "mitkIGTHardwareException.h" #include "igtlTrackingDataMessage.h" #include #include #include #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::IGTLClient::IGTLClient(bool ReadFully) : IGTLDevice(ReadFully) { } mitk::IGTLClient::~IGTLClient() { } bool mitk::IGTLClient::OpenConnection() { if (this->GetState() != Setup) { mitkThrowException(mitk::Exception) << "Can only try to open the connection if in setup mode. State was " << this->GetState(); return false; } std::string hostname = this->GetHostname(); int portNumber = this->GetPortNumber(); if (portNumber == -1 || hostname.size() <= 0) { //port number or hostname was not correct MITK_WARN << "Port number or hostname was not correct"; return false; } //create a new client socket m_Socket = igtl::ClientSocket::New(); //try to connect to the igtl server int response = dynamic_cast(m_Socket.GetPointer())-> ConnectToServer(hostname.c_str(), portNumber); //check the response if (response != 0) { MITK_ERROR << "The client could not connect to " << hostname << " port: " << portNumber; return false; } // everything is initialized and connected so the communication can be started this->SetState(Ready); //inform observers about this new client this->InvokeEvent(NewClientConnectionEvent()); return true; } void mitk::IGTLClient::Receive() { - MITK_INFO << "Trying to receive message"; + //MITK_INFO << "Trying to receive message"; //try to receive a message, if the socket is not present anymore stop the //communication unsigned int status = this->ReceivePrivate(this->m_Socket); if (status == IGTL_STATUS_NOT_PRESENT) { this->StopCommunicationWithSocket(this->m_Socket); //inform observers about loosing the connection to this socket this->InvokeEvent(LostConnectionEvent()); MITK_WARN("IGTLClient") << "Lost connection to server socket."; } } void mitk::IGTLClient::Send() { igtl::MessageBase::Pointer curMessage; //get the latest message from the queue curMessage = this->m_MessageQueue->PullSendMessage(); // there is no message => return if (curMessage.IsNull()) return; if (!this->SendMessagePrivate(curMessage.GetPointer(), this->m_Socket)) { MITK_WARN("IGTLDevice") << "Could not send the message."; } } void mitk::IGTLClient::StopCommunicationWithSocket(igtl::Socket* /*socket*/) { m_StopCommunicationMutex->Lock(); m_StopCommunication = true; m_StopCommunicationMutex->Unlock(); } unsigned int mitk::IGTLClient::GetNumberOfConnections() { return this->m_Socket->GetConnected(); } diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.cpp b/Modules/OpenIGTLink/mitkIGTLDevice.cpp index 94da6b323b..54d8e2b87e 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; + //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 { this->m_MessageQueue->PushMessage(curMessage); this->InvokeEvent(MessageReceivedEvent()); } return IGTL_STATUS_OK; } else { MITK_ERROR("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"; 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"; return IGTL_STATUS_UNKNOWN_ERROR; } } void mitk::IGTLDevice::SendMessage(const mitk::IGTLMessage* msg) { this->SendMessage(msg->GetMessage()); } void mitk::IGTLDevice::SendMessage(igtl::MessageBase::Pointer msg) { //add the message to the queue m_MessageQueue->PushSendMessage(msg); } unsigned int mitk::IGTLDevice::SendMessagePrivate(igtl::MessageBase::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; } // add the name of this device to the message msg->SetDeviceName(this->GetName().c_str()); // Pack (serialize) and send msg->Pack(); int sendSuccess = socket->Send(msg->GetPackPointer(), msg->GetPackSize()); if (sendSuccess) { 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(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::EnableInfiniteBufferingMode( mitk::IGTLMessageQueue::Pointer queue, bool enable) { queue->EnableInfiniteBuffering(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/mitkIGTLMessageQueue.cpp b/Modules/OpenIGTLink/mitkIGTLMessageQueue.cpp index 86aebe3253..4d2244067b 100644 --- a/Modules/OpenIGTLink/mitkIGTLMessageQueue.cpp +++ b/Modules/OpenIGTLink/mitkIGTLMessageQueue.cpp @@ -1,312 +1,312 @@ /*=================================================================== 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 "mitkIGTLMessageQueue.h" #include #include "igtlMessageBase.h" void mitk::IGTLMessageQueue::PushSendMessage(igtl::MessageBase::Pointer message) { this->m_Mutex->Lock(); if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_SendQueue.clear(); m_SendQueue.push_back(message); this->m_Mutex->Unlock(); } void mitk::IGTLMessageQueue::PushCommandMessage(igtl::MessageBase::Pointer message) { this->m_Mutex->Lock(); if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_CommandQueue.clear(); m_CommandQueue.push_back(message); this->m_Mutex->Unlock(); } void mitk::IGTLMessageQueue::PushMessage(igtl::MessageBase::Pointer msg) { this->m_Mutex->Lock(); std::stringstream infolog; infolog << "Received message of type "; if (dynamic_cast(msg.GetPointer()) != nullptr) { if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_TrackingDataQueue.clear(); this->m_TrackingDataQueue.push_back(dynamic_cast(msg.GetPointer())); infolog << "TDATA"; } else if (dynamic_cast(msg.GetPointer()) != nullptr) { if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_TransformQueue.clear(); this->m_TransformQueue.push_back(dynamic_cast(msg.GetPointer())); infolog << "TRANSFORM"; } else if (dynamic_cast(msg.GetPointer()) != nullptr) { if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_StringQueue.clear(); this->m_StringQueue.push_back(dynamic_cast(msg.GetPointer())); infolog << "STRING"; } else if (dynamic_cast(msg.GetPointer()) != nullptr) { igtl::ImageMessage::Pointer imageMsg = dynamic_cast(msg.GetPointer()); int* dim = new int[3]; imageMsg->GetDimensions(dim); if (dim[2] > 1) { if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_Image3dQueue.clear(); this->m_Image3dQueue.push_back(dynamic_cast(msg.GetPointer())); infolog << "IMAGE3D"; } else { if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_Image2dQueue.clear(); this->m_Image2dQueue.push_back(dynamic_cast(msg.GetPointer())); infolog << "IMAGE2D"; } } else { if (this->m_BufferingType == IGTLMessageQueue::Infinit) m_MiscQueue.clear(); this->m_MiscQueue.push_back(msg); infolog << "OTHER"; } m_Latest_Message = msg; - MITK_INFO << infolog.str(); + //MITK_INFO << infolog.str(); this->m_Mutex->Unlock(); } igtl::MessageBase::Pointer mitk::IGTLMessageQueue::PullSendMessage() { igtl::MessageBase::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_SendQueue.size() > 0) { ret = this->m_SendQueue.front(); this->m_SendQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } igtl::MessageBase::Pointer mitk::IGTLMessageQueue::PullMiscMessage() { igtl::MessageBase::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_MiscQueue.size() > 0) { ret = this->m_MiscQueue.front(); this->m_MiscQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } igtl::ImageMessage::Pointer mitk::IGTLMessageQueue::PullImage2dMessage() { igtl::ImageMessage::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_Image2dQueue.size() > 0) { ret = this->m_Image2dQueue.front(); this->m_Image2dQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } igtl::ImageMessage::Pointer mitk::IGTLMessageQueue::PullImage3dMessage() { igtl::ImageMessage::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_Image3dQueue.size() > 0) { ret = this->m_Image3dQueue.front(); this->m_Image3dQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } igtl::TrackingDataMessage::Pointer mitk::IGTLMessageQueue::PullTrackingMessage() { igtl::TrackingDataMessage::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_TrackingDataQueue.size() > 0) { ret = this->m_TrackingDataQueue.front(); this->m_TrackingDataQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } igtl::MessageBase::Pointer mitk::IGTLMessageQueue::PullCommandMessage() { igtl::MessageBase::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_CommandQueue.size() > 0) { ret = this->m_CommandQueue.front(); this->m_CommandQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } igtl::StringMessage::Pointer mitk::IGTLMessageQueue::PullStringMessage() { igtl::StringMessage::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_StringQueue.size() > 0) { ret = this->m_StringQueue.front(); this->m_StringQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } igtl::TransformMessage::Pointer mitk::IGTLMessageQueue::PullTransformMessage() { igtl::TransformMessage::Pointer ret = nullptr; this->m_Mutex->Lock(); if (this->m_TransformQueue.size() > 0) { ret = this->m_TransformQueue.front(); this->m_TransformQueue.pop_front(); } this->m_Mutex->Unlock(); return ret; } std::string mitk::IGTLMessageQueue::GetNextMsgInformationString() { this->m_Mutex->Lock(); std::stringstream s; if (this->m_Latest_Message != nullptr) { s << "Device Type: " << this->m_Latest_Message->GetDeviceType() << std::endl; s << "Device Name: " << this->m_Latest_Message->GetDeviceName() << std::endl; } else { s << "No Msg"; } this->m_Mutex->Unlock(); return s.str(); } std::string mitk::IGTLMessageQueue::GetNextMsgDeviceType() { this->m_Mutex->Lock(); std::stringstream s; if (m_Latest_Message != nullptr) { s << this->m_Latest_Message->GetDeviceType(); } else { s << ""; } this->m_Mutex->Unlock(); return s.str(); } std::string mitk::IGTLMessageQueue::GetLatestMsgInformationString() { this->m_Mutex->Lock(); std::stringstream s; if (m_Latest_Message != nullptr) { s << "Device Type: " << this->m_Latest_Message->GetDeviceType() << std::endl; s << "Device Name: " << this->m_Latest_Message->GetDeviceName() << std::endl; } else { s << "No Msg"; } this->m_Mutex->Unlock(); return s.str(); } std::string mitk::IGTLMessageQueue::GetLatestMsgDeviceType() { this->m_Mutex->Lock(); std::stringstream s; if (m_Latest_Message != nullptr) { s << this->m_Latest_Message->GetDeviceType(); } else { s << ""; } this->m_Mutex->Unlock(); return s.str(); } int mitk::IGTLMessageQueue::GetSize() { return (this->m_CommandQueue.size() + this->m_Image2dQueue.size() + this->m_Image3dQueue.size() + this->m_MiscQueue.size() + this->m_StringQueue.size() + this->m_TrackingDataQueue.size() + this->m_TransformQueue.size()); } void mitk::IGTLMessageQueue::EnableInfiniteBuffering(bool enable) { this->m_Mutex->Lock(); if (enable) this->m_BufferingType = IGTLMessageQueue::BufferingType::Infinit; else this->m_BufferingType = IGTLMessageQueue::BufferingType::NoBuffering; this->m_Mutex->Unlock(); } mitk::IGTLMessageQueue::IGTLMessageQueue() { this->m_Mutex = itk::FastMutexLock::New(); this->m_BufferingType = IGTLMessageQueue::Infinit; } mitk::IGTLMessageQueue::~IGTLMessageQueue() { this->m_Mutex->Unlock(); } \ No newline at end of file