diff --git a/Modules/IGT/Common/mitkTrackingTypes.h b/Modules/IGT/Common/mitkTrackingTypes.h index 0e4791d3eb..3cd18b3bad 100644 --- a/Modules/IGT/Common/mitkTrackingTypes.h +++ b/Modules/IGT/Common/mitkTrackingTypes.h @@ -1,253 +1,257 @@ /*=================================================================== 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 MITKTRACKINGTYPES_H_HEADER_INCLUDED_ #define MITKTRACKINGTYPES_H_HEADER_INCLUDED_ #include #include //#include namespace mitk { /**Documentation * \brief Error codes of NDI tracking devices */ enum NDIErrorCode { NDIOKAY = 0, NDIERROR = 1, SERIALINTERFACENOTSET, SERIALSENDERROR, SERIALRECEIVEERROR, SROMFILETOOLARGE, SROMFILETOOSMALL, NDICRCERROR, // reply has crc error, local computer detected the error NDIINVALIDCOMMAND, NDICOMMANDTOOLONG, NDICOMMANDTOOSHORT, NDICRCDOESNOTMATCH, // command had crc error, tracking device detected the error NDITIMEOUT, NDIUNABLETOSETNEWCOMMPARAMETERS, NDIINCORRECTNUMBEROFPARAMETERS, NDIINVALIDPORTHANDLE, NDIINVALIDTRACKINGPRIORITY, NDIINVALIDLED, NDIINVALIDLEDSTATE, NDICOMMANDINVALIDINCURRENTMODE, NDINOTOOLFORPORT, NDIPORTNOTINITIALIZED, NDISYSTEMNOTINITIALIZED, NDIUNABLETOSTOPTRACKING, NDIUNABLETOSTARTTRACKING, NDIINITIALIZATIONFAILED, NDIINVALIDVOLUMEPARAMETERS, NDICANTSTARTDIAGNOSTICMODE, NDICANTINITIRDIAGNOSTICS, NDIFAILURETOWRITESROM, NDIENABLEDTOOLSNOTSUPPORTED, NDICOMMANDPARAMETEROUTOFRANGE, NDINOMEMORYAVAILABLE, NDIPORTHANDLENOTALLOCATED, NDIPORTHASBECOMEUNOCCUPIED, NDIOUTOFHANDLES, NDIINCOMPATIBLEFIRMWAREVERSIONS, NDIINVALIDPORTDESCRIPTION, NDIINVALIDOPERATIONFORDEVICE, NDIWARNING, NDIUNKNOWNERROR, NDIUNEXPECTEDREPLY, UNKNOWNHANDLERETURNED, TRACKINGDEVICERESET, TRACKINGDEVICENOTSET }; /**Documentation * \brief identifier for tracking device. The way it is currently used * represents a product line rather than an actal type. Refactoring is a future option. */ enum TrackingDeviceType { NDIPolaris, ///< Polaris: optical Tracker from NDI NDIAurora, ///< Aurora: electromagnetic Tracker from NDI ClaronMicron, ///< Micron Tracker: optical Tracker from Claron IntuitiveDaVinci, ///< Intuitive Surgical: DaVinci Telemanipulator API Interface AscensionMicroBird, ///< Ascension microBird / PCIBird family VirtualTracker, ///< Virtual Tracking device class that produces random tracking coordinates TrackingSystemNotSpecified,///< entry for not specified or initialized tracking system TrackingSystemInvalid ///< entry for invalid state (mainly for testing) }; /**Documentation * \brief Error codes of NDI tracking devices */ enum OperationMode { ToolTracking6D, ToolTracking5D, MarkerTracking3D, HybridTracking }; /** * \brief This enum is deprecated. In future, please use the new TrackingDeviceData to model Specific tracking Volumes * Represents the setting of the tracking volume of a NDI tracking device. The tracking volume of * a tracking device itself (as 3d-Object) is represented by an instance of the class mitk::TrackingVolume * as defined by NDI API SFLIST (Aurora and Polaris API guide) * */ + /* enum NDITrackingVolume { Standard, Pyramid, SpectraPyramid, VicraVolume, Cube, - Dome + Dome, + SpectraExtendedPyramid }; - +*/ /** * /brief This structure defines key variables of a device model and type. * It is specifically used to find out which models belong to which vendor, and what volume * to use for a specific Model. Leaving VolumeModelLocation set to null will instruct the Generator * to generate a field to the best of his ability */ struct TrackingDeviceData { TrackingDeviceType Line; std::string Model; std::string VolumeModelLocation; }; /** * Here all supported devices are defined. Dont forget to introduce new Devices into the TrackingDeviceList Array at the bottom! * If a model does not have a corresponding tracking volume yet, pass an empty string to denote "No Model". pass "cube" to render * a default cube of 400x400 px. You can define additional magic strings in the TrackingVolumeGenerator. */ static TrackingDeviceData DeviceDataAuroraCompact = {NDIAurora, "Aurora Compact", "NDIAuroraCompactFG_Dome.stl"}; static TrackingDeviceData DeviceDataAuroraPlanarCube = {NDIAurora, "Aurora Planar (Cube)", "NDIAurora.stl"}; static TrackingDeviceData DeviceDataAuroraPlanarDome = {NDIAurora, "Aurora Planar (Dome)","NDIAuroraPlanarFG_Dome.stl"}; static TrackingDeviceData DeviceDataAuroraTabletop = {NDIAurora, "Aurora Tabletop", "NDIAuroraTabletopFG_Dome.stl"}; // The following entry is for the tabletop prototype, which had an lower barrier of 8cm. The new version has a lower barrier of 12cm. //static TrackingDeviceData DeviceDataAuroraTabletopPrototype = {NDIAurora, "Aurora Tabletop Prototype", "NDIAuroraTabletopFG_Prototype_Dome.stl"}; static TrackingDeviceData DeviceDataMicronTrackerH40 = {ClaronMicron, "Micron Tracker H40", "ClaronMicron.stl"}; static TrackingDeviceData DeviceDataPolarisSpectra = {NDIPolaris, "Polaris Spectra", "NDIPolaris.stl"}; static TrackingDeviceData DeviceDataPolarisVicra = {NDIPolaris, "Polaris Vicra", "NDIPolarisVicra.stl"}; static TrackingDeviceData DeviceDataDaVinci = {IntuitiveDaVinci, "Intuitive DaVinci", "IntuitiveDaVinci.stl"}; static TrackingDeviceData DeviceDataMicroBird = {AscensionMicroBird, "Ascension MicroBird", ""}; static TrackingDeviceData DeviceDataVirtualTracker = {VirtualTracker, "Virtual Tracker", "cube"}; static TrackingDeviceData DeviceDataUnspecified = {TrackingSystemNotSpecified, "Unspecified System", "cube"}; + static TrackingDeviceData DeviceDataSpectraExtendedPyramid = {NDIPolaris, "Polaris Spectra Extended Pyramid", "ExtendedPyramid.stl"}; // Careful when changing the "invalid" device: The mitkTrackingTypeTest is using it's data. static TrackingDeviceData DeviceDataInvalid = {TrackingSystemInvalid, "Invalid Tracking System", ""}; + static TrackingDeviceData TrackingDeviceList[] = {DeviceDataAuroraPlanarCube, DeviceDataAuroraPlanarDome, DeviceDataAuroraCompact, DeviceDataAuroraTabletop, DeviceDataMicronTrackerH40, DeviceDataPolarisSpectra, DeviceDataPolarisVicra, - DeviceDataDaVinci, DeviceDataMicroBird, DeviceDataVirtualTracker, DeviceDataUnspecified, DeviceDataInvalid}; + DeviceDataDaVinci, DeviceDataMicroBird, DeviceDataVirtualTracker, DeviceDataUnspecified, DeviceDataSpectraExtendedPyramid, DeviceDataInvalid}; /** * /brief Returns all devices compatibel to the given Line of Devices */ static std::vector GetDeviceDataForLine(TrackingDeviceType Type){ std::vector Result; int size = (sizeof (TrackingDeviceList) / sizeof*(TrackingDeviceList)); for(int i=0; i < size; i++) { if(TrackingDeviceList[i].Line == Type ) Result.push_back(TrackingDeviceList[i]); } return Result; } /** * /brief Returns the first TracingDeviceData mathing a given line. Useful for backward compatibility * with the old way to manage Devices */ static TrackingDeviceData GetFirstCompatibleDeviceDataForLine(TrackingDeviceType Type){ return GetDeviceDataForLine(Type).front(); } /** * /brief Returns the device Data set matching the model name or the invalid device, if none was found */ static TrackingDeviceData GetDeviceDataByName(std::string modelName){ int size = (sizeof (TrackingDeviceList) / sizeof*(TrackingDeviceList)); for(int i=0; i < size; i++) { if(TrackingDeviceList[i].Model == modelName) return TrackingDeviceList[i]; } return DeviceDataInvalid; } /**Documentation * \brief activation rate of IR illuminator for NDI Polaris tracking device */ enum IlluminationActivationRate { Hz20 = 20, Hz30 = 30, Hz60 = 60 }; /**Documentation * \brief Data transfer mode for NDI tracking devices */ enum DataTransferMode { TX = 0, BX = 1 }; /**Documentation * \brief Query mode for NDI tracking devices */ enum PHSRQueryType { ALL = 0x00, FREED = 0x01, OCCUPIED = 0x02, INITIALIZED = 0x03, ENABLED = 0x04 }; typedef itk::Point MarkerPointType; typedef std::vector MarkerPointContainerType; /** * \brief Defines the tools (arms) of the daVinci system: * PSM1 - Patient side manipulator 1 * PSM2 - Patient side manipulator 2 * ECM - Endoscopic camera manipulator * MTML - Left master target manipulator * MTMR - Right master target manipulator * PSM - Patient side manipulator 3 (if not existent, its data will always be zero) **/ enum DaVinciToolType { PSM1 = 0, PSM2 = 1, ECM = 2, MTML = 3, MTMR = 4, PSM = 5, //UIEvents = 6, }; } // namespace mitk #endif /* MITKTRACKINGTYPES_H_HEADER_INCLUDED_ */ diff --git a/Modules/IGT/Resources/ExtendedPyramid.stl b/Modules/IGT/Resources/ExtendedPyramid.stl new file mode 100644 index 0000000000..5d732e7181 Binary files /dev/null and b/Modules/IGT/Resources/ExtendedPyramid.stl differ diff --git a/Modules/IGT/Resources/NDIPolaris.stl b/Modules/IGT/Resources/NDIPolaris.stl index a5a9b7dee6..ab62f25351 100644 Binary files a/Modules/IGT/Resources/NDIPolaris.stl and b/Modules/IGT/Resources/NDIPolaris.stl differ diff --git a/Modules/IGT/TrackingDevices/mitkNDIProtocol.cpp b/Modules/IGT/TrackingDevices/mitkNDIProtocol.cpp index d8e893e926..0101782fb3 100644 --- a/Modules/IGT/TrackingDevices/mitkNDIProtocol.cpp +++ b/Modules/IGT/TrackingDevices/mitkNDIProtocol.cpp @@ -1,1889 +1,1891 @@ /*=================================================================== 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 "mitkNDIProtocol.h" #include "mitkNDITrackingDevice.h" #include #include #include #include #include mitk::NDIProtocol::NDIProtocol() : itk::Object(), m_TrackingDevice(NULL), m_UseCRC(true) { } mitk::NDIProtocol::~NDIProtocol() { } mitk::NDIErrorCode mitk::NDIProtocol::COMM(mitk::SerialCommunication::BaudRate baudRate , mitk::SerialCommunication::DataBits dataBits, mitk::SerialCommunication::Parity parity, mitk::SerialCommunication::StopBits stopBits, mitk::SerialCommunication::HardwareHandshake hardwareHandshake) { /* Build parameter string */ std::string param; switch (baudRate) { case mitk::SerialCommunication::BaudRate14400: param += "1"; break; case mitk::SerialCommunication::BaudRate19200: param += "2"; break; case mitk::SerialCommunication::BaudRate38400: param += "3"; break; case mitk::SerialCommunication::BaudRate57600: param += "4"; break; case mitk::SerialCommunication::BaudRate115200: param += "5"; break; case mitk::SerialCommunication::BaudRate9600: default: // assume 9600 Baud as default param += "0"; break; } switch (dataBits) { case mitk::SerialCommunication::DataBits7: param += "1"; break; case mitk::SerialCommunication::DataBits8: default: // set 8 data bits as default param += "0"; break; } switch (parity) { case mitk::SerialCommunication::Odd: param += "1"; break; case mitk::SerialCommunication::Even: param += "2"; break; case mitk::SerialCommunication::None: default: // set no parity as default param += "0"; break; } switch (stopBits) { case mitk::SerialCommunication::StopBits2: param += "1"; break; case mitk::SerialCommunication::StopBits1: default: // set 1 stop bit as default param += "0"; break; } switch (hardwareHandshake) { case mitk::SerialCommunication::HardwareHandshakeOn: param += "1"; break; case mitk::SerialCommunication::HardwareHandshakeOff: default: // set no hardware handshake as default param += "0"; break; } return GenericCommand("COMM", ¶m); } mitk::NDIErrorCode mitk::NDIProtocol::INIT() { return GenericCommand("INIT"); } mitk::NDIErrorCode mitk::NDIProtocol::DSTART() { return GenericCommand("DSTART"); } mitk::NDIErrorCode mitk::NDIProtocol::DSTOP() { return GenericCommand("DSTOP"); } mitk::NDIErrorCode mitk::NDIProtocol::IRINIT() { return GenericCommand("IRINIT"); } mitk::NDIErrorCode mitk::NDIProtocol::IRCHK(bool* IRdetected) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from trackingsystem if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; /* send command */ std::string fullcommand; if (m_UseCRC == true) fullcommand = "IRCHK:0001"; // command string format 1: with crc else fullcommand = "IRCHK 0001"; // command string format 2: without crc returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); if (returnValue != NDIOKAY) // check for send error { m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove any reply return returnValue; } /* wait for the trackingsystem to process the command */ itksys::SystemTools::Delay(100); /* read and parse the reply from tracking device */ // the reply for IRCHK can be either Infrared Source Information or ERROR## // because we use the simple reply format, the answer will be only one char: // "0" - no IR detected // "1" - IR detected std::string reply; char b; m_TrackingDevice->ReceiveByte(&b);// read the first byte reply = b; if ((b == '0') || (b == '1')) // normal answer { /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code { if ( b == '0') *IRdetected = false; else *IRdetected = true; returnValue = NDIOKAY; } else // return error in CRC { returnValue = NDICRCERROR; *IRdetected = false; // IRdetected is only valid if return code of this function is NDIOKAY } } else if (b =='E') // expect ERROR## { std::string errorstring; m_TrackingDevice->Receive(&errorstring, 4); // read the remaining 4 characters of ERROR reply += errorstring; static const std::string error("ERROR"); if (error.compare(0, 5, reply) == 0) // check for "ERROR" { std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code /* perform CRC checking */ reply += errorcode; // build complete reply string std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else returnValue = NDICRCERROR; // return error in CRC } } else // something else, that we do not expect returnValue = NDIUNEXPECTEDREPLY; /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::PHSR(PHSRQueryType queryType, std::string* portHandles) { NDIErrorCode returnValue = NDIUNKNOWNERROR; if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; /* send command */ std::string command; char stringQueryType[3]; sprintf(stringQueryType, "%02X", queryType);// convert numerical queryType to string in hexadecimal format if (m_UseCRC == true) command = std::string("PHSR:") + std::string(stringQueryType); else //if (m_UseCRC != true) command = std::string("PHSR ") + std::string(stringQueryType); // command string format 2: without crc returnValue = m_TrackingDevice->Send(&command, m_UseCRC); if (returnValue != NDIOKAY) // check for send error { m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove any reply return returnValue; } itksys::SystemTools::Delay(100); std::string reply; m_TrackingDevice->Receive(&reply, 2); // read first 2 characters of reply ("Number of Handles" as a 2 digit hexadecimal number) static const std::string error("ERROR"); if (error.compare(0, 2, reply) == 0) // check for "ERROR" (compare for "ER" because we can not be sure that the reply is more than 2 characters (in case of 0 port handles returned) { std::string ror; m_TrackingDevice->Receive(&ror, 3); // read rest of ERROR (=> "ROR") reply += ror; // build complete reply string std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else // No error, expect number of handles as a 2 character hexadecimal value { unsigned int numberOfHandles = 0; // convert the hexadecimal string representation to a numerical using a stringstream std::stringstream s; s << reply; s >> numberOfHandles; if (numberOfHandles > 16) // there can not be more than 16 handles ToDo: exact maximum number depend on tracking device and firmware revision. these data could be read with VER and used here { returnValue = NDIUNKNOWNERROR; } else { std::string handleInformation; portHandles->clear(); for (unsigned int i = 0; i < numberOfHandles; i++) // read 5 characters for each handle and extract port handle { m_TrackingDevice->Receive(&handleInformation, 5); *portHandles += handleInformation.substr(0, 2); // Append the port handle to the portHandles string reply += handleInformation; // build complete reply string for crc checking } /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return okay returnValue = NDIOKAY; else // return error in CRC returnValue = NDICRCERROR; } } /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::PHRQ(std::string* portHandle) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from tracking device if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; /* send command */ std::string command; if (m_UseCRC == true) command = "PHRQ:*********1****"; // command string format 1: with crc else command = "PHRQ *********1****"; // command string format 2: without crc returnValue = m_TrackingDevice->Send(&command, m_UseCRC); if (returnValue != NDIOKAY) // check for send error { m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove any reply return returnValue; } itksys::SystemTools::Delay(100); // give the tracking device some time to process the command std::string reply; m_TrackingDevice->Receive(&reply, 2); // read first 2 characters of reply ("Number of Handles" as a 2 digit hexadecimal number) static const std::string error("ERROR"); if (error.compare(0, 2, reply) == 0) // check for "ERROR" (compare for "ER" because we can not be sure that the reply is more than 2 characters (in case of 0 port handles returned) { std::string ror; m_TrackingDevice->Receive(&ror, 3); // read rest of ERROR (=> "ROR") reply += ror; // build complete reply string std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else // No error, expect port handle as a 2 character hexadecimal value { *portHandle = reply; // assign the port handle to the return string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return okay returnValue = NDIOKAY; else // else return error in CRC returnValue = NDICRCERROR; } /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::PVWR(std::string* portHandle, const unsigned char* sromData, unsigned int sromDataLength) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from trackingsystem if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; if (sromDataLength > 1024) return SROMFILETOOLARGE; if (sromDataLength == 0) return SROMFILETOOSMALL; /* build send commands */ std::string basecommand; if (m_UseCRC == true) basecommand = "PVWR:"; // command string format 1: with crc else basecommand = "PVWR "; // command string format 2: without crc std::string hexSROMData; hexSROMData.reserve(2 * sromDataLength); char hexcharacter[20]; // 7 bytes should be enough (in send loop) for (unsigned int i = 0; i < sromDataLength; i++) { sprintf(hexcharacter, "%02X", sromData[i]); // convert srom byte to string in hexadecimal format //hexSROMData += "12"; hexSROMData += hexcharacter; // append hex string to srom data in hex format } /* data must be written in chunks of 64 byte (128 hex characters). To ensure 64 byte chunks the last chunk must be padded with 00 */ unsigned int zerosToPad = 128 - (hexSROMData.size() % 128); // hexSROMData must be a multiple of 128 if (zerosToPad > 0) hexSROMData.append(zerosToPad, '0'); /* now we have all the data, send it in 128 character chunks */ std::string fullcommand; for (unsigned int j = 0; j < hexSROMData.size(); j += 128) { sprintf(hexcharacter, "%s%04X", portHandle->c_str(), j/2); // build the first two parameters: PortHandle and SROM device adress (not in hex characters, but in bytes) fullcommand = basecommand + hexcharacter + hexSROMData.substr(j, 128); // build complete command string returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); // send command itksys::SystemTools::Delay(50); // Wait for trackingsystem to process the data if (returnValue != NDIOKAY) // check for send error break; returnValue = this->ParseOkayError(); // parse answer if (returnValue != NDIOKAY) // check for error returned from tracking device break; } /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::PINIT(std::string* portHandle) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from trackingsystem if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; /* send command */ std::string fullcommand; if (m_UseCRC == true) fullcommand = std::string("PINIT:") + *portHandle; // command string format 1: with crc else fullcommand = std::string("PINIT ") + *portHandle; // command string format 2: without crc returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); if (returnValue != NDIOKAY) // check for send error { m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove any reply return returnValue; } /* wait for the trackingsystem to process the command */ itksys::SystemTools::Delay(100); std::string reply; m_TrackingDevice->Receive(&reply, 4); // read first 4 characters of reply /* Parse reply from tracking device */ static const std::string okay("OKAYA896"); // OKAY is static, so we can perform a static crc check static const std::string error("ERROR"); static const std::string warning("WARNING7423"); // WARNING has a static crc too if (okay.compare(0, 4, reply) == 0) // check for "OKAY": compare first 4 characters from okay with reply { // OKAY was found, now check the CRC16 too m_TrackingDevice->Receive(&reply, 4); // read 4 hexadecimal characters for CRC16 if (okay.compare(4, 4, reply, 0, 4) == 0) // first 4 from new reply should match last 4 from okay returnValue = NDIOKAY; else returnValue = NDICRCERROR; } else if (warning.compare(0, 4, reply) == 0) // check for "WARNING" { // WARN was found, now check remaining characters and CRC16 m_TrackingDevice->Receive(&reply, 4); // read 4 hexadecimal characters for CRC16 if (warning.compare(4, 7, reply, 0, 7) == 0) // first 7 from new reply should match last 7 from okay returnValue = NDIWARNING; else returnValue = NDICRCERROR; } else if (error.compare(0, 4, reply) == 0) // check for "ERRO" { char b; // The ERROR reply is not static, so we can not use a static crc check. m_TrackingDevice->ReceiveByte(&b); // read next character ("R" from ERROR) reply += b; // to build complete reply string std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else // else it is something else, that we do not expect returnValue = NDIUNEXPECTEDREPLY; //read cr carriage return char b; m_TrackingDevice->ReceiveByte(&b); /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::PENA(std::string* portHandle, TrackingPriority prio) { std::string param; if (portHandle != NULL) param = *portHandle + (char) prio; else param = ""; return this->GenericCommand("PENA", ¶m); } mitk::NDIErrorCode mitk::NDIProtocol::PHINF(std::string portHandle, std::string* portInfo) { std::string command; if (m_UseCRC) command = "PHINF:" + portHandle; else command = "PHINF " + portHandle; mitk::NDIErrorCode returnValue = m_TrackingDevice->Send(&command, m_UseCRC); if (returnValue==NDIOKAY) { m_TrackingDevice->ClearReceiveBuffer(); m_TrackingDevice->Receive(portInfo, 33); m_TrackingDevice->ClearReceiveBuffer(); } else m_TrackingDevice->ClearReceiveBuffer(); return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::PDIS(std::string* portHandle) { return this->GenericCommand("PDIS", portHandle); } mitk::NDIErrorCode mitk::NDIProtocol::PHF(std::string* portHandle) { return this->GenericCommand("PHF", portHandle); } mitk::NDIErrorCode mitk::NDIProtocol::IRATE(IlluminationActivationRate rate) { std::string param; switch (rate) { case Hz60: param = "2"; break; case Hz30: param = "1"; break; case Hz20: default: param = "0"; break; } return this->GenericCommand("IRATE", ¶m); } mitk::NDIErrorCode mitk::NDIProtocol::BEEP(unsigned char count) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from trackingsystem if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; std::string p; if ((count >= 1) && (count <= 9)) p = (count + '0'); // convert the number count to a character representation else return NDICOMMANDPARAMETEROUTOFRANGE; /* send command */ std::string fullcommand; if (m_UseCRC == true) fullcommand = "BEEP:" + p; // command string format 1: with crc else fullcommand = "BEEP " + p; // command string format 2: without crc returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); if (returnValue != NDIOKAY) // check for send error { m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove any reply return returnValue; } /* wait for the trackingsystem to process the command */ itksys::SystemTools::Delay(100); std::string reply; char b; m_TrackingDevice->ReceiveByte(&b); // read the first byte reply = b; if ((b == '0') || (b == '1')) // normal answer { /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = NDIOKAY; else // return error in CRC returnValue = NDICRCERROR; } else if (b =='E') // expect ERROR## { std::string errorstring; m_TrackingDevice->Receive(&errorstring, 4); // read the remaining 4 characters of ERROR reply += errorstring; static const std::string error("ERROR"); if (error.compare(0, 5, reply) == 0) // check for "ERROR" { std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code /* perform CRC checking */ reply += errorcode; // build complete reply string std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else returnValue = NDICRCERROR; // return error in CRC } } else // something else, that we do not expect returnValue = NDIUNEXPECTEDREPLY; /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::TSTART(bool resetFrameCounter) { std::string param = "80"; if (resetFrameCounter == true) return this->GenericCommand("TSTART", ¶m); else return this->GenericCommand("TSTART"); } mitk::NDIErrorCode mitk::NDIProtocol::TSTOP() { return this->GenericCommand("TSTOP"); } mitk::NDIErrorCode mitk::NDIProtocol::PSOUT(std::string portHandle, std::string state) { std::string param = portHandle + state; return this->GenericCommand("PSOUT", ¶m); } mitk::NDIErrorCode mitk::NDIProtocol::TX(bool trackIndividualMarkers, MarkerPointContainerType* markerPositions) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from tracking device if(trackIndividualMarkers) markerPositions->clear(); if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; /* send command */ std::string fullcommand; if (trackIndividualMarkers) { if (m_UseCRC == true) fullcommand = "TX:1001"; // command string format 1: with crc else fullcommand = "TX 1001"; // command string format 2: without crc } else { if (m_UseCRC == true) fullcommand = "TX:"; // command string format 1: with crc else fullcommand = "TX "; // command string format 2: without crc } returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); if (returnValue != NDIOKAY) { /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } /* read number of handles returned */ std::string reply; std::string s; m_TrackingDevice->Receive(&reply, 2); // read first 2 characters of reply (error or number of handles returned) //printf("%d",reply); static const std::string error("ERROR"); if (error.compare(0, 2, reply) == 0) { m_TrackingDevice->Receive(&s, 3); // read next characters ("ROR" from ERROR) reply += s; // to build complete reply string std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code { returnValue = this->GetErrorCode(&errorcode); } else // return error in CRC { returnValue = NDICRCERROR; } } else // transformation data is returned { /* parse number of handles from first 2 characters */ std::stringstream converter; unsigned int numberOfHandles = 0; converter << std::hex << reply; // insert reply into stringstream converter >> numberOfHandles; // extract number of handles as unsigned byte converter.clear(); // converter must be cleared to be reused converter.str(""); /* read and parse transformation data for each handle */ for (unsigned int i = 0; i < numberOfHandles; i++) // for each handle { /* Read port handle */ m_TrackingDevice->Receive(&s, 2); // read port handle reply += s; // build complete command string NDIPassiveTool::Pointer tool = m_TrackingDevice->GetInternalTool(s); // get tool object for that handle if (tool.IsNull()) { returnValue = UNKNOWNHANDLERETURNED; break; // if we do not know the handle, we can not assume anything about the remaining data, so we better abort (we could read up to the next LF) } /* Parse reply from tracking device */ static const std::string missing("MISSING"); static const std::string disabled("DISABLED"); static const std::string unoccupied("UNOCCUPIED"); m_TrackingDevice->Receive(&s, 6); // read next 6 characters: either an error message or part of the transformation data reply += s; // build complete command string if (missing.compare(0, 6, s) == 0) { tool->SetErrorMessage("Tool is reported as 'missing'."); tool->SetDataValid(false); m_TrackingDevice->Receive(&s, 18); // after 'missin', 1 character for 'g', 8 characters for port status, 8 characters for frame number and one for line feed are send reply += s; // build complete command string } else if (disabled.compare(0, 6, s) == 0) { tool->SetErrorMessage("Tool is reported as 'disabled'."); tool->SetDataValid(false); m_TrackingDevice->Receive(&s, 3); // read last characters of disabled plus 8 characters for port status, 8 characters for frame number and one for line feed reply += s; // build complete command string } else if (unoccupied.compare(0, 6, s) == 0) { tool->SetErrorMessage("Tool is reported as 'unoccupied'."); tool->SetDataValid(false); m_TrackingDevice->Receive(&s, 21); // read remaining characters of UNOCCUPIED reply += s; // build complete command string } else // transformation data { /* define local copies */ signed int number = 0; float localPos[3] = {0.0, 0.0, 0.0}; float localQuat[4] = {0.0, 0.0, 0.0, 0.0}; float localError = 0.0; unsigned long localPortStatus = 0; unsigned int localFrameNumber = 0; /* read and parse the four 6 character quaternion values */ //std::cout << "s = " << s << std::endl; converter << std::dec << s; // insert string with first number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); //std::cout << "number = " << number << std::endl; localQuat[0] = number / 10000.0; // the value is send with an implied decimal point with 4 digits to the right for (unsigned int i = 1; i < 4; i++)// read the next 3 numbers { m_TrackingDevice->Receive(&s, 6); // read the next number reply += s; // build complete command string converter << std::dec << s; // insert string with first number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); localQuat[i] = number / 10000.0; // the value is send with an implied decimal point with 4 digits to the right } /* read and parse the three 7 character translation values */ for (unsigned int i = 0; i < 3; i++) { m_TrackingDevice->Receive(&s, 7); // read the next position vector number reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); localPos[i] = number / 100.0; // the value is send with an implied decimal point with 2 digits to the right } /* read and parse 6 character error value */ m_TrackingDevice->Receive(&s, 6); // read the error value reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract the number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); localError = number / 10000.0; // the error value is send with an implied decimal point with 4 digits to the right /* read and parse 8 character port status */ m_TrackingDevice->Receive(&s, 8); // read the port status value reply += s; // build complete command string converter << std::hex << s; // insert string into stringstream converter >> localPortStatus; // extract the number as unsigned long converter.clear(); // converter must be cleared to be reused converter.str(""); /* read and parse 8 character frame number as hexadecimal */ m_TrackingDevice->Receive(&s, 8); // read the frame number value reply += s; // build complete command string converter << std::hex << s; // insert string with hex value encoded number into stringstream converter >> localFrameNumber; // extract the number as unsigned long converter.clear(); // converter must be cleared to be reused converter.str(""); /* copy local values to the tool */ mitk::Quaternion orientation(localQuat[1], localQuat[2], localQuat[3], localQuat[0]); //If the rotation mode is vnlTransposed we have to transpose the quaternion if (m_TrackingDevice->GetRotationMode() == mitk::NDITrackingDevice::RotationTransposed) { orientation[0] *= -1; //qx orientation[1] *= -1; //qy orientation[2] *= -1; //qz //qr is not inverted } tool->SetOrientation(orientation); mitk::Point3D position; position[0] = localPos[0]; position[1] = localPos[1]; position[2] = localPos[2]; tool->SetPosition(position); tool->SetTrackingError(localError); tool->SetErrorMessage(""); tool->SetDataValid(true); m_TrackingDevice->Receive(&s, 1); // read the line feed character, that terminates each handle data reply += s; // build complete command string } } // for //Read Reply Option 1000 data if(trackIndividualMarkers) { /* parse number of markers from first 2 characters */ m_TrackingDevice->Receive(&s, 2); reply += s; unsigned int numberOfMarkers = 0; converter << std::hex << s; // insert reply into stringstream converter >> numberOfMarkers; // extract number of markers as unsigned byte converter.clear(); // converter must be cleared to be reused converter.str(""); unsigned int oovReplySize = (unsigned int)ceil((double)numberOfMarkers/4.0); unsigned int nbMarkersInVolume = 0; char c; // parse oov data to find out how many marker positions were recorded for (unsigned int i = 0; i < oovReplySize; i++) { m_TrackingDevice->ReceiveByte(&c); reply += c; nbMarkersInVolume += ByteToNbBitsOn(c); } nbMarkersInVolume = numberOfMarkers-nbMarkersInVolume; /* read and parse position data for each marker */ for (unsigned int i = 0; i < nbMarkersInVolume; i++) { /* define local copies */ signed int number = 0; MarkerPointType markerPosition; /* read and parse the three 7 character translation values */ for (unsigned int i = 0; i < 3; i++) { m_TrackingDevice->Receive(&s, 7); // read the next position vector number reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); markerPosition[i] = number / 100.0; // the value is send with an implied decimal point with 2 digits to the right } markerPositions->push_back(markerPosition); } // end for all markers } //END read Reply Option 1000 data /* Read System Status */ m_TrackingDevice->Receive(&s, 4); // read system status reply += s; // build complete command string /* now the reply string is complete, perform crc checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return okay { returnValue = NDIOKAY; } else // return error in CRC { returnValue = NDICRCERROR; /* Invalidate all tools because the received data contained an error */ m_TrackingDevice->InvalidateAll(); if(trackIndividualMarkers) markerPositions->clear(); } } // else /* cleanup and return */ m_TrackingDevice->Receive(&s, 1); // read the last linde feed (because the tracking system device is sometimes to slow to send it before we clear the buffer. In this case, the LF would remain in the receive buffer and be read as the first character of the next command m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::TX1000(MarkerPointContainerType* markerPositions) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from trackingsystem markerPositions->clear(); if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; /* send command */ std::string fullcommand; if (m_UseCRC == true) fullcommand = "TX:1001"; // command string format 1: with crc else fullcommand = "TX 1001"; // command string format 2: without crc returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); /* read number of handles returned */ std::string reply; std::string s; m_TrackingDevice->Receive(&reply, 2); // read first 2 characters of reply (error or number of handles returned) static const std::string error("ERROR"); if (error.compare(0, 2, reply) == 0) { m_TrackingDevice->Receive(&s, 3); // read next characters ("ROR" from ERROR) reply += s; // to build complete reply string std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code { returnValue = this->GetErrorCode(&errorcode); } else // return error in CRC { returnValue = NDICRCERROR; } } else // transformation data is returned { /* parse number of handles from first 2 characters */ std::stringstream converter; unsigned int numberOfHandles = 0; converter << std::hex << reply; // insert reply into stringstream converter >> numberOfHandles; // extract number of handles as unsigned byte converter.clear(); // converter must be cleared to be reused converter.str(""); /* read and parse transformation data for each handle */ for (unsigned int i = 0; i < numberOfHandles; i++) // for each handle { /* Read port handle */ m_TrackingDevice->Receive(&s, 2); // read port handle reply += s; // build complete command string NDIPassiveTool::Pointer tool = m_TrackingDevice->GetInternalTool(s); // get tool object for that handle if (tool.IsNull()) { returnValue = UNKNOWNHANDLERETURNED; break; // if we do not know the handle, we can not assume anything about the remaining data, so we better abort (we could read up to the next LF) } /* Parse reply from tracking device */ static const std::string missing("MISSING"); static const std::string disabled("DISABLED"); static const std::string unoccupied("UNOCCUPIED"); m_TrackingDevice->Receive(&s, 6); // read next 6 characters: either an error message or part of the transformation data reply += s; // build complete command string if (missing.compare(0, 6, s) == 0) { tool->SetErrorMessage("Tool is reported as 'missing'."); tool->SetDataValid(false); m_TrackingDevice->Receive(&s, 18); // after 'missin', 1 character for 'g', 8 characters for port status, 8 characters for frame number and one for line feed are send reply += s; // build complete command string } else if (disabled.compare(0, 6, s) == 0) { tool->SetErrorMessage("Tool is reported as 'disabled'."); tool->SetDataValid(false); m_TrackingDevice->Receive(&s, 19); // read last characters of disabled plus 8 characters for port status, 8 characters for frame number and one for line feed reply += s; // build complete command string } else if (unoccupied.compare(0, 6, s) == 0) { tool->SetErrorMessage("Tool is reported as 'unoccupied'."); tool->SetDataValid(false); m_TrackingDevice->Receive(&s, 21); // read remaining characters of UNOCCUPIED reply += s; // build complete command string } else // transformation data { /* define local copies */ signed int number = 0; float localPos[3] = {0.0, 0.0, 0.0}; float localQuat[4] = {0.0, 0.0, 0.0, 0.0}; float localError = 0.0; unsigned long localPortStatus = 0; unsigned int localFrameNumber = 0; /* read and parse the four 6 character quaternion values */ //std::cout << "s = " << s << std::endl; converter << std::dec << s; // insert string with first number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); //std::cout << "number = " << number << std::endl; localQuat[0] = number / 10000.0; // the value is send with an implied decimal point with 4 digits to the right for (unsigned int i = 1; i < 4; i++)// read the next 3 numbers { m_TrackingDevice->Receive(&s, 6); // read the next number reply += s; // build complete command string converter << std::dec << s; // insert string with first number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); localQuat[i] = number / 10000.0; // the value is send with an implied decimal point with 4 digits to the right } /* read and parse the three 7 character translation values */ for (unsigned int i = 0; i < 3; i++) { m_TrackingDevice->Receive(&s, 7); // read the next position vector number reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); localPos[i] = number / 100.0; // the value is send with an implied decimal point with 2 digits to the right } /* read and parse 6 character error value */ m_TrackingDevice->Receive(&s, 6); // read the error value reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract the number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); localError = number / 10000.0; // the error value is send with an implied decimal point with 4 digits to the right /* read and parse 8 character port status */ m_TrackingDevice->Receive(&s, 8); // read the port status value reply += s; // build complete command string converter << std::hex << s; // insert string into stringstream converter >> localPortStatus; // extract the number as unsigned long converter.clear(); // converter must be cleared to be reused converter.str(""); /* read and parse 8 character frame number as hexadecimal */ m_TrackingDevice->Receive(&s, 8); // read the frame number value reply += s; // build complete command string converter << std::hex << s; // insert string with hex value encoded number into stringstream converter >> localFrameNumber; // extract the number as unsigned long converter.clear(); // converter must be cleared to be reused converter.str(""); /* copy local values to the tool */ mitk::Quaternion orientation(localQuat[1], localQuat[2], localQuat[3], localQuat[0]); tool->SetOrientation(orientation); mitk::Point3D position; position[0] = localPos[0]; position[1] = localPos[1]; position[2] = localPos[2]; tool->SetPosition(position); tool->SetTrackingError(localError); tool->SetErrorMessage(""); tool->SetDataValid(true); m_TrackingDevice->Receive(&s, 1); // read the line feed character, that terminates each handle data reply += s; // build complete command string } } //Read Reply Option 1000 data /* parse number of markers from first 2 characters */ m_TrackingDevice->Receive(&s, 2); reply += s; unsigned int numberOfMarkers = 0; converter << std::hex << s; // insert reply into stringstream converter >> numberOfMarkers; // extract number of markers as unsigned byte converter.clear(); // converter must be cleared to be reused converter.str(""); unsigned int oovReplySize = (unsigned int)ceil((double)numberOfMarkers/4.0); unsigned int nbMarkersInVolume = 0; char c; // parse oov data to find out how many marker positions were recorded for (unsigned int i = 0; i < oovReplySize; i++) { m_TrackingDevice->ReceiveByte(&c); reply += c; nbMarkersInVolume += ByteToNbBitsOn(c); } nbMarkersInVolume = numberOfMarkers-nbMarkersInVolume; /* read and parse position data for each marker */ for (unsigned int i = 0; i < nbMarkersInVolume; i++) { /* define local copies */ signed int number = 0; MarkerPointType markerPosition; /* read and parse the three 7 character translation values */ for (unsigned int i = 0; i < 3; i++) { m_TrackingDevice->Receive(&s, 7); // read the next position vector number reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract first number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); markerPosition[i] = number / 100.0; // the value is send with an implied decimal point with 2 digits to the right } markerPositions->push_back(markerPosition); } // end for all markers //m_TrackingDevice->Receive(&s, 1); // read the line feed character, that terminates each handle data //reply += s; // build complete command string // //END read Reply Option 1000 data m_TrackingDevice->Receive(&s, 4); // read system status reply += s; // build complete command string /* now the reply string is complete, perform crc checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return okay { returnValue = NDIOKAY; } else // return error in CRC { returnValue = NDICRCERROR; /* Invalidate all tools because the received data contained an error */ markerPositions->clear(); m_TrackingDevice->InvalidateAll(); } } // else /* cleanup and return */ m_TrackingDevice->Receive(&s, 1); // read the last linde feed (because the tracking system device is sometimes to slow to send it before we clear the buffer. In this case, the LF would remain in the receive buffer and be read as the first character of the next command m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::BX() { std::cout << "BX() not implemented yet, using TX() instead." << std::endl; return this->TX(); } mitk::NDIErrorCode mitk::NDIProtocol::VER(mitk::TrackingDeviceType& t) { if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from tracking system /* send command */ std::string fullcommand; if (m_UseCRC == true) fullcommand = "VER:4"; // command string format 1: with crc else fullcommand = "VER 4"; // command string format 2: without crc returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); if (returnValue != NDIOKAY) { /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } /* read number of handles returned */ std::string reply; m_TrackingDevice->Receive(&reply, 5); // read first 5 characters of reply (error beginning of version information) static const std::string error("ERROR"); if (error.compare(0, 6, reply) == 0) // ERROR case { std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else // no error, valid reply { std::string s; m_TrackingDevice->ReceiveLine(&s); // read until first LF character reply += s; std::string upperCaseReply; upperCaseReply.resize(reply.size()); std::transform (reply.begin(), reply.end(), upperCaseReply.begin(), toupper); // convert reply to uppercase to ease finding if (upperCaseReply.find("POLARIS") != std::string::npos) t = mitk::NDIPolaris; else if (upperCaseReply.find("AURORA") != std::string::npos) t = mitk::NDIAurora; else t = mitk::TrackingSystemNotSpecified; // check for "VICRA", "SPECTRA", "ACCEDO" /* do not check for remaining reply, do not check for CRC, just delete remaining reply */ itksys::SystemTools::Delay(500); // wait until reply should be finished m_TrackingDevice->ClearReceiveBuffer(); returnValue = NDIOKAY; } return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::POS3D(MarkerPointContainerType* markerPositions) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from trackingsystem if (m_TrackingDevice == NULL) { return TRACKINGDEVICENOTSET; } if (markerPositions == NULL) { std::cout << "ERROR: markerPositions==NULL" << std::endl; return NDIUNKNOWNERROR; } markerPositions->clear(); // empty point container /* try to obtain a porthandle */ if (m_TrackingDevice->GetToolCount() == 0) { std::cout << "ERROR: no tools present" << std::endl; return NDIUNKNOWNERROR; } const TrackingTool* t = m_TrackingDevice->GetTool(static_cast(0)); const NDIPassiveTool* t2 = dynamic_cast(t); if (t2 == NULL) { std::cout << "ERROR: no tool present" << std::endl; return NDIUNKNOWNERROR; } std::string portHandle = t2->GetPortHandle(); if (portHandle.size() == 0) { std::cout << "ERROR: no port handle" << std::endl; return NDIUNKNOWNERROR; } /* send command */ std::string fullcommand; if (m_UseCRC == true) fullcommand = "3D:" + portHandle + "5"; // command string format 1: with crc else fullcommand = "3D " + portHandle + "5"; // command string format 2: without crc m_TrackingDevice->ClearReceiveBuffer(); returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); /* read number of markers returned */ std::string reply; std::string s; mitk::NDIErrorCode receivevalue = m_TrackingDevice->Receive(&reply, 3); // read first 3 characters of reply (error or number of markers returned) if(receivevalue != NDIOKAY) { std::cout << "ERROR: receive_value != NDIOKAY" << std::endl; return receivevalue; } static const std::string error("ERROR"); if (error.compare(0, 3, reply) == 0) { m_TrackingDevice->Receive(&s, 2); // read next characters ("OR" from ERROR) reply += s; // to build complete reply string std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code { returnValue = this->GetErrorCode(&errorcode); } else // return error in CRC { returnValue = NDICRCERROR; } } else // transformation data is returned { signed int number = 0; //float localPos[3] = {0.0, 0.0, 0.0}; MarkerPointType p; float lineSeparation = 0.0; /* parse number of markers from first 3 characters */ std::stringstream converter; unsigned int numberOfMarkers = 0; converter << std::dec << reply; // insert reply into stringstream converter >> numberOfMarkers; // extract number of handles as unsigned byte converter.clear(); // converter must be cleared to be reused converter.str(""); /* read and parse 3D data for each marker */ for (unsigned int markerID = 0; markerID < numberOfMarkers; markerID++) // for each marker { m_TrackingDevice->Receive(&s, 1); // read line feed reply += s; // build complete command string /* read and parse the three 9 character translation values */ for (unsigned int i = 0; i < 3; i++) { receivevalue = m_TrackingDevice->Receive(&s, 9); // read the next position vector number if(receivevalue != NDIOKAY) { markerPositions->clear(); std::cout << "ERROR: receive_value != NDIOKAY" << std::endl; return receivevalue; } reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract the number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); p[i] = number / 10000.0; // the value is send with an implied decimal point with 4 digits to the right } /* read and parse 4 character line separation value */ receivevalue = m_TrackingDevice->Receive(&s, 4); // read the line separation value if(receivevalue != NDIOKAY) { markerPositions->clear(); std::cout << "ERROR: receive_value != NDIOKAY" << std::endl; return receivevalue; } reply += s; // build complete command string converter << std::dec << s; // insert string with number into stringstream converter >> number; // extract the number as integer converter.clear(); // converter must be cleared to be reused converter.str(""); lineSeparation = number / 100.0; // the line separation value is send with an implied decimal point with 2 digits to the right /* read and parse 1 character out of volume value */ receivevalue = m_TrackingDevice->Receive(&s, 1); // read the port status value if(receivevalue != NDIOKAY) { markerPositions->clear(); std::cout << std::endl << std::endl << std::endl << "ERROR: POS3D != NDIOKAY" << std::endl; return receivevalue; } reply += s; // build complete command string /* store the marker positions in the point container */ markerPositions->push_back(p); } //std::cout << "INFO: Found " << markerPositions->size() << " markers." << std::endl; /* now the reply string is complete, perform crc checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return okay { returnValue = NDIOKAY; } else // return error in CRC { returnValue = NDICRCERROR; std::cout << "ERROR: receive_value != NDIOKAY" << std::endl; /* delete all marker positions because the received data contained an error */ markerPositions->clear(); } } // else /* cleanup and return */ m_TrackingDevice->Receive(&s, 1); // read the last linde feed (because the tracking system device is sometimes to slow to send it before we clear the buffer. In this case, the LF would remain in the receive buffer and be read as the first character of the next command m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::GenericCommand(const std::string command, const std::string* parameter) { NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from trackingsystem if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; std::string p; if (parameter != NULL) p = *parameter; else p = ""; /* send command */ std::string fullcommand; if (m_UseCRC == true) fullcommand = command + ":" + p; // command string format 1: with crc else fullcommand = command + " " + p; // command string format 2: without crc m_TrackingDevice->ClearReceiveBuffer(); // This is a workaround for a linux specific issue: // after sending the TSTART command and expecting an "okay" there are some unexpected bytes left in the buffer. // this issue is explained in bug 11825 returnValue = m_TrackingDevice->Send(&fullcommand, m_UseCRC); if (returnValue != NDIOKAY) // check for send error { m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove any reply return returnValue; } /* wait for the trackingsystem to process the command */ itksys::SystemTools::Delay(100); /* read and parse the reply from tracking device */ // the reply for a generic command can be OKAY or ERROR## // so we can use the generic parse method for these replies this->ParseOkayError(); return returnValue; } unsigned int mitk::NDIProtocol::ByteToNbBitsOn(char& c) const { if(c == '0') return 0; else if (c == '1' || c == '2' || c == '4' || c == '8') return 1; else if (c == '3' || c == '5' || c == '9' || c == '6' || c == 'A' || c == 'C') return 2; else if (c == '7' || c == 'B' || c == 'D' || c == 'E') return 3; else if (c == 'F') return 4; else return 0; } mitk::NDIErrorCode mitk::NDIProtocol::ParseOkayError() { NDIErrorCode returnValue = NDIUNKNOWNERROR; /* read reply from tracking device */ // the reply is expected to be OKAY or ERROR## // define reply strings std::string reply; m_TrackingDevice->Receive(&reply, 4); // read first 4 characters of reply /* Parse reply from tracking device */ static const std::string okay("OKAYA896"); // OKAY is static, so we can perform a static crc check static const std::string error("ERROR"); if (okay.compare(0, 4, reply) == 0) // check for "OKAY": compare first 4 characters from okay with reply { // OKAY was found, now check the CRC16 too m_TrackingDevice->Receive(&reply, 4); // read 4 hexadecimal characters for CRC16 if (okay.compare(4, 4, reply, 0, 4) == 0) // first 4 from new reply should match last 4 from okay returnValue = NDIOKAY; else returnValue = NDICRCERROR; } else if (error.compare(0, 4, reply) == 0) // check for "ERRO" { char b; // The ERROR reply is not static, so we can not use a static crc check. m_TrackingDevice->ReceiveByte(&b); // read next character ("R" from ERROR) reply += b; // to build complete reply string std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else // something else, that we do not expect returnValue = NDIUNEXPECTEDREPLY; /* cleanup and return */ char b; m_TrackingDevice->ReceiveByte(&b); // read CR character m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::GetErrorCode(const std::string* input) { if (input->compare("01") == 0) return NDIINVALIDCOMMAND; else if (input->compare("02") == 0) return NDICOMMANDTOOLONG; else if (input->compare("03") == 0) return NDICOMMANDTOOSHORT; else if (input->compare("04") == 0) return NDICRCDOESNOTMATCH; else if (input->compare("05") == 0) return NDITIMEOUT; else if (input->compare("06") == 0) return NDIUNABLETOSETNEWCOMMPARAMETERS; else if (input->compare("07") == 0) return NDIINCORRECTNUMBEROFPARAMETERS; else if (input->compare("08") == 0) return NDIINVALIDPORTHANDLE; else if (input->compare("09") == 0) return NDIINVALIDTRACKINGPRIORITY; else if (input->compare("0A") == 0) return NDIINVALIDLED; else if (input->compare("0B") == 0) return NDIINVALIDLEDSTATE; else if (input->compare("0C") == 0) return NDICOMMANDINVALIDINCURRENTMODE; else if (input->compare("0D") == 0) return NDINOTOOLFORPORT; else if (input->compare("0E") == 0) return NDIPORTNOTINITIALIZED; // ... else if (input->compare("10") == 0) return NDISYSTEMNOTINITIALIZED; else if (input->compare("11") == 0) return NDIUNABLETOSTOPTRACKING; else if (input->compare("12") == 0) return NDIUNABLETOSTARTTRACKING; else if (input->compare("13") == 0) return NDIINITIALIZATIONFAILED; else if (input->compare("14") == 0) return NDIINVALIDVOLUMEPARAMETERS; else if (input->compare("16") == 0) return NDICANTSTARTDIAGNOSTICMODE; else if (input->compare("1B") == 0) return NDICANTINITIRDIAGNOSTICS; else if (input->compare("1F") == 0) return NDIFAILURETOWRITESROM; else if (input->compare("22") == 0) return NDIENABLEDTOOLSNOTSUPPORTED; else if (input->compare("23") == 0) return NDICOMMANDPARAMETEROUTOFRANGE; else if (input->compare("2A") == 0) return NDINOMEMORYAVAILABLE; else if (input->compare("2B") == 0) return NDIPORTHANDLENOTALLOCATED; else if (input->compare("2C") == 0) return NDIPORTHASBECOMEUNOCCUPIED; else if (input->compare("2D") == 0) return NDIOUTOFHANDLES; else if (input->compare("2E") == 0) return NDIINCOMPATIBLEFIRMWAREVERSIONS; else if (input->compare("2F") == 0) return NDIINVALIDPORTDESCRIPTION; else if (input->compare("32") == 0) return NDIINVALIDOPERATIONFORDEVICE; // ... else return NDIUNKNOWNERROR; } mitk::NDIErrorCode mitk::NDIProtocol::APIREV(std::string* revision) { if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from tracking system /* send command */ std::string command; if (m_UseCRC) command = "APIREV:"; else command = "APIREV "; returnValue = m_TrackingDevice->Send(&command, m_UseCRC); if (returnValue != NDIOKAY) { /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } //wait for tracking system to compute the output //itksys::SystemTools::Delay(100); /* read number of handles returned */ std::string reply; m_TrackingDevice->Receive(&reply, 5); //look for ERROR // read first 5 characters of reply (error beginning of version information) static const std::string error("ERROR"); if (error.compare(0, 6, reply) == 0) // ERROR case { std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else // no error, valid reply: expect something like: D.001.00450D4 (.. { std::string s; m_TrackingDevice->Receive(&s, 4); // read further reply += s; /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = NDIOKAY; else // return error in CRC returnValue = NDICRCERROR; } *revision = reply; m_TrackingDevice->ClearReceiveBuffer(); return returnValue; } mitk::NDIErrorCode mitk::NDIProtocol::SFLIST(std::string* info) { if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from tracking system /* send command */ std::string command; if (m_UseCRC) command = "SFLIST:03"; else command = "SFLIST 03"; returnValue = m_TrackingDevice->Send(&command, m_UseCRC); if (returnValue != NDIOKAY) { /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } /* read number of handles returned */ std::string reply; m_TrackingDevice->Receive(&reply, 5); //look for "ERROR" static const std::string error("ERROR"); if (error.compare(0,6,reply) == 0) // ERROR case { std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else // no error, valid reply: expect numbers of devices in hex, and then info of each featured tracking volume { /* parse number of volumes from first character in hex */ std::stringstream converter; unsigned int numberOfVolumes = 0; converter << std::hex << reply[0]; // insert reply into stringstream converter >> numberOfVolumes; // extract number of handles as unsigned byte converter.clear(); // converter must be cleared to be reused converter.str(""); //reply currently contains the first 5 elements if (numberOfVolumes>0) { //for each featured volume for (unsigned int i = 0; iReceive(&s, 69);// 69 characters to get all dimensions plus two characters at the end: one reserved and one for metal resistance. reply += s; currentVolume += s; } else { //read to the end of the line from the last volume //(needed here, because if only one volume is supported, //then there is no lineending before CRC checksum std::string l; m_TrackingDevice->ReceiveLine(&l); reply += l; std::string s; m_TrackingDevice->Receive(&s, 73); //need total of 73 bytes for a volume reply += s; currentVolume += s; } - MITK_INFO << "currentVolume " << i <<" : " <CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits MITK_INFO << "expectedCRC: " << expectedCRC; MITK_INFO << "readCRC: " << readCRC; //if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = NDIOKAY; // else // return error in CRC // returnValue = NDICRCERROR; // } *info = reply; - MITK_INFO << "info: " << *info; + //MITK_INFO << "info: " << *info; m_TrackingDevice->ClearReceiveBuffer(); return returnValue; } -mitk::NDIErrorCode mitk::NDIProtocol::VSEL(mitk::NDITrackingVolume volume) +mitk::NDIErrorCode mitk::NDIProtocol::VSEL(mitk::TrackingDeviceData volume) { if (m_TrackingDevice == NULL) return TRACKINGDEVICENOTSET; NDIErrorCode returnValue = NDIUNKNOWNERROR; // return code for this function. Will be set according to reply from tracking system //get information about the order of volumes from tracking device. Then choose the number needed for VSEL by the output and parameter "device" unsigned int numberOfVolumes; mitk::NDITrackingDevice::NDITrackingVolumeContainerType volumes; mitk::NDITrackingDevice::TrackingVolumeDimensionType volumesDimensions; if (!m_TrackingDevice->GetSupportedVolumes(&numberOfVolumes, &volumes, &volumesDimensions)) return returnValue; //interested in volumes(!) if (volumes.empty()) return returnValue; //with the order within volumes we can define our needed parameter for VSEL //find the index where volumes[n] == device unsigned int index = 1; //the index for VSEL starts at 1 + MITK_INFO << "volume: " << volume.Model; mitk::NDITrackingDevice::NDITrackingVolumeContainerType::iterator it = volumes.begin(); while (it != volumes.end()) { - if ((*it) == volume) + MITK_INFO << "volumes" << index << ": " << *it; + if ((*it) == volume.Model) break; it++, index++; } - //if (it == volumes.end() || index > numberOfVolumes) //not found / volume not supported - // return NDIINVALIDOPERATIONFORDEVICE; + if (it == volumes.end() || index > numberOfVolumes) //not found / volume not supported + return NDIINVALIDOPERATIONFORDEVICE; //index now contains the information on which position the desired volume is situated /* send command */ std::string command; if (m_UseCRC) command = "VSEL:"; else command = "VSEL "; //add index to command std::stringstream s; - s << index-1; + s << index; command += s.str(); returnValue = m_TrackingDevice->Send(&command, m_UseCRC); if (returnValue != NDIOKAY) { /* cleanup and return */ m_TrackingDevice->ClearReceiveBuffer(); // flush the buffer to remove the remaining carriage return or unknown/unexpected reply return returnValue; } /* read number of handles returned */ std::string reply; m_TrackingDevice->Receive(&reply, 4); //look for "ERROR" or "OKAY" static const std::string error("ERRO"); if (error.compare(reply) == 0) // ERROR case { std::string s; m_TrackingDevice->Receive(&s, 1); //get the last "R" in "ERROR" reply += s; std::string errorcode; m_TrackingDevice->Receive(&errorcode, 2); // now read 2 bytes error code reply += errorcode; // build complete reply string /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = this->GetErrorCode(&errorcode); else // return error in CRC returnValue = NDICRCERROR; } else { /* perform CRC checking */ std::string expectedCRC = m_TrackingDevice->CalcCRC(&reply); // calculate crc for received reply string std::string readCRC; // read attached crc value m_TrackingDevice->Receive(&readCRC, 4); // CRC16 is 2 bytes long, which is transmitted as 4 hexadecimal digits - // if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code + if (expectedCRC == readCRC) // if the read CRC is correct, return normal error code returnValue = NDIOKAY; - // else // return error in CRC - // returnValue = NDICRCERROR; + else // return error in CRC + returnValue = NDICRCERROR; } m_TrackingDevice->ClearReceiveBuffer(); return returnValue; } diff --git a/Modules/IGT/TrackingDevices/mitkNDIProtocol.h b/Modules/IGT/TrackingDevices/mitkNDIProtocol.h index b7bc5cf338..b388dc6001 100644 --- a/Modules/IGT/TrackingDevices/mitkNDIProtocol.h +++ b/Modules/IGT/TrackingDevices/mitkNDIProtocol.h @@ -1,114 +1,114 @@ /*=================================================================== 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 MITKNDIPROTOCOL_H_HEADER_INCLUDED_ #define MITKNDIPROTOCOL_H_HEADER_INCLUDED_ #include #include #include #include "mitkSerialCommunication.h" #include "mitkNDIPassiveTool.h" namespace mitk { class NDITrackingDevice; /**Documentation * \brief The NDI Protocol class provides building and parsing of * command strings and answers to and from a NDI tracking device. * * \ingroup IGT */ class MitkIGT_EXPORT NDIProtocol : public itk::Object { public: mitkClassMacro(NDIProtocol, itk::Object); itkNewMacro(Self); itkSetObjectMacro(TrackingDevice, NDITrackingDevice); typedef mitk::SerialCommunication::PortNumber PortNumber; ///< Port number of the serial connection typedef mitk::SerialCommunication::BaudRate BaudRate; ///< Baud rate of the serial connection typedef mitk::SerialCommunication::DataBits DataBits; ///< Number of data bits used in the serial connection typedef mitk::SerialCommunication::Parity Parity; ///< Parity mode used in the serial connection typedef mitk::SerialCommunication::StopBits StopBits; ///< Number of stop bits used in the serial connection typedef mitk::SerialCommunication::HardwareHandshake HardwareHandshake; ///< Hardware handshake mode of the serial connection typedef mitk::NDIPassiveTool::TrackingPriority TrackingPriority; ///< Tracking priority used for tracking a tool /** * \brief Gives information about the tool which is assosiated with the port handle. Writes portInfo to the string. */ NDIErrorCode APIREV(std::string* revision); NDIErrorCode PHINF(std::string portHandle, std::string* portInfo); NDIErrorCode PSOUT(std::string portHandle, std::string state); ///< Set GPIO Output (Aurora) NDIErrorCode COMM(mitk::SerialCommunication::BaudRate baudRate, mitk::SerialCommunication::DataBits dataBits, mitk::SerialCommunication::Parity parity, mitk::SerialCommunication::StopBits stopBits, mitk::SerialCommunication::HardwareHandshake hardwareHandshake); ///< Change Serial Communication Parameters NDIErrorCode INIT(); ///< Initialize the Measurement System NDIErrorCode DSTART(); ///< Start the Diagnostic Mode NDIErrorCode DSTOP(); ///< Stop the Diagnostic Mode NDIErrorCode IRINIT(); ///< Initialize the System to Check for Infrared NDIErrorCode IRCHK(bool* IRdetected); ///< This version of IRCHK uses only the simple "presence of infrared light" call, that returns a binary "IR detected/no IR detected" answer NDIErrorCode PHSR(PHSRQueryType queryType, std::string* portHandles); ///< Port Handle Search. Will write returned port handles to the string portHandles NDIErrorCode PHF(std::string* portHandle); ///< Port Handle Free. Frees the port handle. NDIErrorCode PHRQ(std::string* portHandle); ///< Port Handle Request. Will request a Port Handle for a wireless tool and return it in the string portHandle NDIErrorCode PVWR(std::string* portHandle, const unsigned char* sromData, unsigned int sromDataLength); ///< Port Virtual Write. Writes an SROM Image data to a tool NDIErrorCode PINIT(std::string* portHandle); ///< Port Initialize. Will initialize a Port that has been acquired with PHRQ and has been assigned a SROM File with PVWR NDIErrorCode PENA(std::string* portHandle, TrackingPriority prio); ///< Port Enable. Will enable a port that has been initialized with PINIT. NDIErrorCode PDIS(std::string* portHandle); ///< Port Disable. Will disable a port that has been enabled with PENA NDIErrorCode IRATE(IlluminationActivationRate rate); ///< Setting the illuminator rate. Will set the refresh rate for the illuminator for wireless tools NDIErrorCode BEEP(unsigned char count); ///< Sounding the measurement system beeper. The tracking system will beep one to nine times NDIErrorCode SFLIST(std::string* info); ///< Returns information about the supported feature of the tracking system NDIErrorCode TSTART(bool resetFrameCounter = false); ///< Start Tracking Mode. The tracking system must be in setup mode and must be initialized. NDIErrorCode TSTOP(); ///< Stop Tracking Mode. The tracking system must be in Tracking mode. NDIErrorCode TX(bool trackIndividualMarkers = false, MarkerPointContainerType* markerPositions = NULL); ///< Report transformations in text mode. Optionally, individual markers can be tracked NDIErrorCode BX(); ///< Report transformations in binary mode. NDIErrorCode POS3D(MarkerPointContainerType* markerPositions); ///< Report 3D Positions of single markers. can only be used in diagnostics mode NDIErrorCode VER(mitk::TrackingDeviceType& t); ///< returns if the tracking device is a Polaris or an Aurora system - NDIErrorCode VSEL(mitk::NDITrackingVolume volume); ///< Sets the tracking volume to the given type. Check available tracking volumes with SFLIST first + NDIErrorCode VSEL(mitk::TrackingDeviceData volume); ///< Sets the tracking volume to the given type. Check available tracking volumes with SFLIST first NDIErrorCode TX1000(MarkerPointContainerType* markerPositions); ///< Report transformations in text mode. unsigned int ByteToNbBitsOn(char& c) const; ///< itkGetConstMacro(UseCRC, bool); ///< Get whether to append a CRC16 checksum to each message itkSetMacro(UseCRC, bool); ///< Set whether to append a CRC16 checksum to each message itkBooleanMacro(UseCRC); ///< Set whether to append a CRC16 checksum to each message protected: NDIProtocol(); virtual ~NDIProtocol(); /**Documentation * Reads the reply from the tracking device and checks if it is either "OKAY" or "ERROR##". * if it reads an error, it returns the equivalent NDIErrorCode * Replies other than OKAY or ERROR result in an NDIUNEXPECTEDREPLY */ NDIErrorCode ParseOkayError(); /**Documentation * Sends the command command to the tracking system and checks for OKAY and ERROR as replies * This is used by commands like INIT, DSTART, DSTOP,... that do not need parameters * or special parsing of replies */ NDIErrorCode GenericCommand(const std::string command, const std::string* parameter = NULL); /**Documentation * \brief returns the error code for an Error String returned from the NDI tracking device */ NDIErrorCode GetErrorCode(const std::string* input); NDITrackingDevice* m_TrackingDevice; ///< tracking device to which the commands will be send bool m_UseCRC; ///< whether to append a CRC16 checksum to each message }; } // namespace mitk #endif /* MITKNDIPROTOCOL_H_HEADER_INCLUDED_ */ diff --git a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp index ebb0026655..b1568faec1 100644 --- a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp @@ -1,1280 +1,1281 @@ /*=================================================================== 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 "mitkNDITrackingDevice.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include #include typedef itk::MutexLockHolder MutexLockHolder; const unsigned char CR = 0xD; // == '\r' - carriage return const unsigned char LF = 0xA; // == '\n' - line feed mitk::NDITrackingDevice::NDITrackingDevice() : TrackingDevice(),m_DeviceName(""), m_PortNumber(mitk::SerialCommunication::COM5), m_BaudRate(mitk::SerialCommunication::BaudRate9600), m_DataBits(mitk::SerialCommunication::DataBits8), m_Parity(mitk::SerialCommunication::None), m_StopBits(mitk::SerialCommunication::StopBits1), -m_HardwareHandshake(mitk::SerialCommunication::HardwareHandshakeOff), m_NDITrackingVolume(Standard), +m_HardwareHandshake(mitk::SerialCommunication::HardwareHandshakeOff), m_IlluminationActivationRate(Hz20), m_DataTransferMode(TX), m_6DTools(), m_ToolsMutex(NULL), m_SerialCommunication(NULL), m_SerialCommunicationMutex(NULL), m_DeviceProtocol(NULL), m_MultiThreader(NULL), m_ThreadID(0), m_OperationMode(ToolTracking6D), m_MarkerPointsMutex(NULL), m_MarkerPoints() { m_Data = mitk::DeviceDataUnspecified; m_6DTools.clear(); m_SerialCommunicationMutex = itk::FastMutexLock::New(); m_DeviceProtocol = NDIProtocol::New(); m_DeviceProtocol->SetTrackingDevice(this); m_DeviceProtocol->UseCRCOn(); m_MultiThreader = itk::MultiThreader::New(); m_ToolsMutex = itk::FastMutexLock::New(); m_MarkerPointsMutex = itk::FastMutexLock::New(); m_MarkerPoints.reserve(50); // a maximum of 50 marker positions can be reported by the tracking device } bool mitk::NDITrackingDevice::UpdateTool(mitk::TrackingTool* tool) { if (this->GetState() != Setup) { mitk::NDIPassiveTool* ndiTool = dynamic_cast(tool); if (ndiTool == NULL) return false; std::string portHandle = ndiTool->GetPortHandle(); //return false if the SROM Data has not been set if (ndiTool->GetSROMData() == NULL) return false; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PVWR(&portHandle, ndiTool->GetSROMData(), ndiTool->GetSROMDataLength()); if (returnvalue != NDIOKAY) return false; returnvalue = m_DeviceProtocol->PINIT(&portHandle); if (returnvalue != NDIOKAY) return false; returnvalue = m_DeviceProtocol->PENA(&portHandle, ndiTool->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) return false; return true; } else { return false; } } void mitk::NDITrackingDevice::SetRotationMode(RotationMode r) { m_RotationMode = r; } mitk::NDITrackingDevice::~NDITrackingDevice() { /* stop tracking and disconnect from tracking device */ if (GetState() == Tracking) { this->StopTracking(); } if (GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if ((m_ThreadID != 0) && (m_MultiThreader.IsNotNull())) { m_MultiThreader->TerminateThread(m_ThreadID); } m_MultiThreader = NULL; /* free serial communication interface */ if (m_SerialCommunication.IsNotNull()) { m_SerialCommunication->ClearReceiveBuffer(); m_SerialCommunication->ClearSendBuffer(); m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; } } void mitk::NDITrackingDevice::SetPortNumber(const PortNumber _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting PortNumber to " << _arg); if (this->m_PortNumber != _arg) { this->m_PortNumber = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetDeviceName(std::string _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting eviceName to " << _arg); if (this->m_DeviceName != _arg) { this->m_DeviceName = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetBaudRate(const BaudRate _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting BaudRate to " << _arg); if (this->m_BaudRate != _arg) { this->m_BaudRate = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetDataBits(const DataBits _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting DataBits to " << _arg); if (this->m_DataBits != _arg) { this->m_DataBits = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetParity(const Parity _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting Parity to " << _arg); if (this->m_Parity != _arg) { this->m_Parity = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetStopBits(const StopBits _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting StopBits to " << _arg); if (this->m_StopBits != _arg) { this->m_StopBits = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetHardwareHandshake(const HardwareHandshake _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting HardwareHandshake to " << _arg); if (this->m_HardwareHandshake != _arg) { this->m_HardwareHandshake = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetIlluminationActivationRate(const IlluminationActivationRate _arg) { if (this->GetState() == Tracking) return; itkDebugMacro("setting IlluminationActivationRate to " << _arg); if (this->m_IlluminationActivationRate != _arg) { this->m_IlluminationActivationRate = _arg; this->Modified(); if (this->GetState() == Ready) // if the connection to the tracking system is established, send the new rate to the tracking device too m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate); } } void mitk::NDITrackingDevice::SetDataTransferMode(const DataTransferMode _arg) { itkDebugMacro("setting DataTransferMode to " << _arg); if (this->m_DataTransferMode != _arg) { this->m_DataTransferMode = _arg; this->Modified(); } } mitk::NDIErrorCode mitk::NDITrackingDevice::Send(const std::string* input, bool addCRC) { if (input == NULL) return SERIALSENDERROR; std::string message; if (addCRC == true) message = *input + CalcCRC(input) + std::string(1, CR); else message = *input + std::string(1, CR); //unsigned int messageLength = message.length() + 1; // +1 for CR // Clear send buffer this->ClearSendBuffer(); // Send the date to the device MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Send(message); if (returnvalue == 0) return SERIALSENDERROR; else return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::Receive(std::string* answer, unsigned int numberOfBytes) { if (answer == NULL) return SERIALRECEIVEERROR; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Receive(*answer, numberOfBytes); // never read more bytes than the device has send, the function will block until enough bytes are send... if (returnvalue == 0) return SERIALRECEIVEERROR; else return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveByte(char* answer) { if (answer == NULL) return SERIALRECEIVEERROR; std::string m; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Receive(m, 1); if ((returnvalue == 0) ||(m.size() != 1)) return SERIALRECEIVEERROR; *answer = m.at(0); return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveLine(std::string* answer) { if (answer == NULL) return SERIALRECEIVEERROR; std::string m; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex do { long returnvalue = m_SerialCommunication->Receive(m, 1); if ((returnvalue == 0) ||(m.size() != 1)) return SERIALRECEIVEERROR; *answer += m; } while (m.at(0) != LF); return NDIOKAY; } void mitk::NDITrackingDevice::ClearSendBuffer() { MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex m_SerialCommunication->ClearSendBuffer(); } void mitk::NDITrackingDevice::ClearReceiveBuffer() { MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex m_SerialCommunication->ClearReceiveBuffer(); } const std::string mitk::NDITrackingDevice::CalcCRC(const std::string* input) { if (input == NULL) return ""; /* the crc16 calculation code is taken from the NDI API guide example code section */ static int oddparity[16] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0}; unsigned int data; // copy of the input string's current character unsigned int crcValue = 0; // the crc value is stored here unsigned int* puCRC16 = &crcValue; // the algorithm uses a pointer to crcValue, so it's easier to provide that than to change the algorithm for (unsigned int i = 0; i < input->length(); i++) { data = (*input)[i]; data = (data ^ (*(puCRC16) & 0xff)) & 0xff; *puCRC16 >>= 8; if (oddparity[data & 0x0f] ^ oddparity[data >> 4]) { *(puCRC16) ^= 0xc001; } data <<= 6; *puCRC16 ^= data; data <<= 1; *puCRC16 ^= data; } // crcValue contains now the CRC16 value. Convert it to a string and return it char returnvalue[13]; sprintf(returnvalue,"%04X", crcValue); // 4 hexadecimal digit with uppercase format return std::string(returnvalue); } bool mitk::NDITrackingDevice::OpenConnection() { //this->m_ModeMutex->Lock(); if (this->GetState() != Setup) {mitkThrowException(mitk::IGTException) << "Can only try to open the connection if in setup mode";} m_SerialCommunication = mitk::SerialCommunication::New(); /* init local com port to standard com settings for a NDI tracking device: 9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake */ if (m_DeviceName.empty()) m_SerialCommunication->SetPortNumber(m_PortNumber); else m_SerialCommunication->SetDeviceName(m_DeviceName); m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600); m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8); m_SerialCommunication->SetParity(mitk::SerialCommunication::None); m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); if (m_SerialCommunication->OpenConnection() == 0) // 0 == ERROR_VALUE { m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; mitkThrowException(mitk::IGTHardwareException) << "Can not open serial port"; } /* Reset Tracking device by sending a serial break for 500ms */ m_SerialCommunication->SendBreak(400); /* Read answer from tracking device (RESETBE6F) */ static const std::string reset("RESETBE6F\r"); std::string answer = ""; this->Receive(&answer, reset.length()); // read answer (should be RESETBE6F) this->ClearReceiveBuffer(); // flush the receive buffer of all remaining data (carriage return, strings other than reset if (reset.compare(answer) != 0) // check for RESETBE6F { if (m_SerialCommunication.IsNotNull()) { m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; } mitkThrowException(mitk::IGTHardwareException) << "Hardware Reset of tracking device did not work"; } /* Now the tracking device is reset, start initialization */ NDIErrorCode returnvalue; /* set device com settings to new values and wait for the device to change them */ returnvalue = m_DeviceProtocol->COMM(m_BaudRate, m_DataBits, m_Parity, m_StopBits, m_HardwareHandshake); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "Could not set comm settings in trackingdevice";} //after changing COMM wait at least 100ms according to NDI Api documentation page 31 itksys::SystemTools::Delay(500); /* now change local com settings accordingly */ m_SerialCommunication->CloseConnection(); m_SerialCommunication->SetBaudRate(m_BaudRate); m_SerialCommunication->SetDataBits(m_DataBits); m_SerialCommunication->SetParity(m_Parity); m_SerialCommunication->SetStopBits(m_StopBits); m_SerialCommunication->SetHardwareHandshake(m_HardwareHandshake); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); m_SerialCommunication->OpenConnection(); /* initialize the tracking device */ returnvalue = m_DeviceProtocol->INIT(); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "Could not initialize the tracking device";} if (this->GetType() == mitk::TrackingSystemNotSpecified) // if the type of tracking device is not specified, try to query the connected device { mitk::TrackingDeviceType deviceType; returnvalue = m_DeviceProtocol->VER(deviceType); if ((returnvalue != NDIOKAY) || (deviceType == mitk::TrackingSystemNotSpecified)) {mitkThrowException(mitk::IGTHardwareException) << "Could not determine tracking device type. Please set manually and try again.";} this->SetType(deviceType); } /**** Optional Polaris specific code, Work in progress // start diagnostic mode returnvalue = m_DeviceProtocol->DSTART(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not start diagnostic mode"); return false; } else // we are in diagnostic mode { // initialize extensive IR checking returnvalue = m_DeviceProtocol->IRINIT(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not initialize intense infrared light checking"); return false; } bool intenseIR = false; returnvalue = m_DeviceProtocol->IRCHK(&intenseIR); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not execute intense infrared light checking"); return false; } if (intenseIR == true) // do something - warn the user, raise exception, write to protocol or similar std::cout << "Warning: Intense infrared light detected. Accurate tracking will probably not be possible.\n"; // stop diagnictic mode returnvalue = m_DeviceProtocol->DSTOP(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not stop diagnostic mode"); return false; } } *** end of optional polaris code ***/ /** * now add tools to the tracking system **/ /* First, check if the tracking device has port handles that need to be freed and free them */ returnvalue = FreePortHandles(); // non-critical, therefore no error handling /** * POLARIS: initialize the tools that were added manually **/ { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex std::string portHandle; Tool6DContainerType::iterator endIt = m_6DTools.end(); for(Tool6DContainerType::iterator it = m_6DTools.begin(); it != endIt; ++it) { /* get a port handle for the tool */ returnvalue = m_DeviceProtocol->PHRQ(&portHandle); if (returnvalue == NDIOKAY) { (*it)->SetPortHandle(portHandle.c_str()); /* now write the SROM file of the tool to the tracking system using PVWR */ if (this->m_Data.Line == NDIPolaris) { returnvalue = m_DeviceProtocol->PVWR(&portHandle, (*it)->GetSROMData(), (*it)->GetSROMDataLength()); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + (*it)->GetToolName() + std::string("' to tracking device")).c_str();} returnvalue = m_DeviceProtocol->PINIT(&portHandle); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize tool '") + (*it)->GetToolName()).c_str();} if ((*it)->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&portHandle, (*it)->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + portHandle + std::string("' for tool '")+ (*it)->GetToolName() + std::string("'")).c_str(); } } } } } } // end of toolsmutexlockholder scope /* check for wired tools and add them too */ if (this->DiscoverWiredTools() == false) // query the tracking device for wired tools and add them to our tool list return false; // \TODO: could we continue anyways? /*POLARIS: set the illuminator activation rate */ if (this->m_Data.Line == NDIPolaris) { returnvalue = m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "Could not set the illuminator activation rate";} } /* finish - now all tools should be added, initialized and enabled, so that tracking can be started */ this->SetState(Ready); - SetVolume(mitk::Standard); + MITK_INFO << "m_Data.Model: " <m_Data.Model; + SetVolume(this->m_Data); return true; } bool mitk::NDITrackingDevice::InitializeWiredTools() { NDIErrorCode returnvalue; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected";} /* if there are port handles that need to be initialized, initialize them. Furthermore instantiate tools for each handle that has no tool yet. */ std::string ph; for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); mitk::NDIPassiveTool* pt = this->GetInternalTool(ph); if ( pt == NULL) // if we don't have a tool, something is wrong. Tools should be discovered first by calling DiscoverWiredTools() continue; if (pt->GetSROMData() == NULL) continue; returnvalue = m_DeviceProtocol->PVWR(&ph, pt->GetSROMData(), pt->GetSROMDataLength()); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + pt->GetToolName() + std::string("' to tracking device")).c_str();} returnvalue = m_DeviceProtocol->PINIT(&ph); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize tool '") + pt->GetToolName()).c_str();} if (pt->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&ph, pt->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + portHandle + std::string("' for tool '")+ pt->GetToolName() + std::string("'")).c_str(); } } } return true; } mitk::TrackingDeviceType mitk::NDITrackingDevice::TestConnection() { if (this->GetState() != Setup) { return mitk::TrackingSystemNotSpecified; } m_SerialCommunication = mitk::SerialCommunication::New(); //m_DeviceProtocol = mitk::NDIProtocol::New(); //m_DeviceProtocol->SetTrackingDevice(this); //m_DeviceProtocol->UseCRCOn(); /* init local com port to standard com settings for a NDI tracking device: 9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake */ if (m_DeviceName.empty()) m_SerialCommunication->SetPortNumber(m_PortNumber); else m_SerialCommunication->SetDeviceName(m_DeviceName); m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600); m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8); m_SerialCommunication->SetParity(mitk::SerialCommunication::None); m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); if (m_SerialCommunication->OpenConnection() == 0) // error { m_SerialCommunication = NULL; return mitk::TrackingSystemNotSpecified; } /* Reset Tracking device by sending a serial break for 500ms */ m_SerialCommunication->SendBreak(400); /* Read answer from tracking device (RESETBE6F) */ static const std::string reset("RESETBE6F\r"); std::string answer = ""; this->Receive(&answer, reset.length()); // read answer (should be RESETBE6F) this->ClearReceiveBuffer(); // flush the receive buffer of all remaining data (carriage return, strings other than reset if (reset.compare(answer) != 0) // check for RESETBE6F { m_SerialCommunication->CloseConnection(); m_SerialCommunication = NULL; mitkThrowException(mitk::IGTHardwareException) << "Hardware Reset of tracking device did not work"; } /* Now the tracking device is reset, start initialization */ NDIErrorCode returnvalue; /* initialize the tracking device */ //returnvalue = m_DeviceProtocol->INIT(); //if (returnvalue != NDIOKAY) //{ // this->SetErrorMessage("Could not initialize the tracking device"); // return mitk::TrackingSystemNotSpecified; //} mitk::TrackingDeviceType deviceType; returnvalue = m_DeviceProtocol->VER(deviceType); if ((returnvalue != NDIOKAY) || (deviceType == mitk::TrackingSystemNotSpecified)) { m_SerialCommunication = NULL; return mitk::TrackingSystemNotSpecified; } m_SerialCommunication = NULL; return deviceType; } bool mitk::NDITrackingDevice::CloseConnection() { if (this->GetState() != Setup) { //init before closing to force the field generator from aurora to switch itself off m_DeviceProtocol->INIT(); /* close the serial connection */ m_SerialCommunication->CloseConnection(); /* invalidate all tools */ this->InvalidateAll(); /* return to setup mode */ this->SetState(Setup); m_SerialCommunication = NULL; } return true; } ITK_THREAD_RETURN_TYPE mitk::NDITrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == NULL) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == NULL) { return ITK_THREAD_RETURN_VALUE; } NDITrackingDevice *trackingDevice = (NDITrackingDevice*)pInfo->UserData; if (trackingDevice != NULL) { if (trackingDevice->GetOperationMode() == ToolTracking6D) trackingDevice->TrackTools(); // call TrackTools() from the original object else if (trackingDevice->GetOperationMode() == MarkerTracking3D) trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object else if (trackingDevice->GetOperationMode() == ToolTracking5D) trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object else if (trackingDevice->GetOperationMode() == HybridTracking) { trackingDevice->TrackToolsAndMarkers(); } } trackingDevice->m_ThreadID = 0; // erase thread id, now that this thread will end. return ITK_THREAD_RETURN_VALUE; } bool mitk::NDITrackingDevice::StartTracking() { if (this->GetState() != Ready) return false; this->SetState(Tracking); // go to mode Tracking this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method mitk::IGTTimeStamp::GetInstance()->Start(this); return true; } void mitk::NDITrackingDevice::TrackTools() { if (this->GetState() != Tracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->TSTART(); if (returnvalue != NDIOKAY) return; /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); while ((this->GetState() == Tracking) && (localStopTracking == false)) { if (this->m_DataTransferMode == TX) { returnvalue = this->m_DeviceProtocol->TX(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors break; } else { returnvalue = this->m_DeviceProtocol->BX(); if (returnvalue != NDIOKAY) break; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->TSTOP(); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "An error occured while tracking tools.";} return; // returning from this function (and ThreadStartTracking()) this will end the thread and transfer control back to main thread by releasing trackingFinishedLockHolder } void mitk::NDITrackingDevice::TrackMarkerPositions() { if (m_OperationMode == ToolTracking6D) return; if (this->GetState() != Tracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->DSTART(); // Start Diagnostic Mode if (returnvalue != NDIOKAY) return; MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); while ((this->GetState() == Tracking) && (localStopTracking == false)) { m_MarkerPointsMutex->Lock(); // lock points data structure returnvalue = this->m_DeviceProtocol->POS3D(&m_MarkerPoints); // update points data structure with new position data from tracking device m_MarkerPointsMutex->Unlock(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors { std::cout << "Error in POS3D: could not read data. Possibly no markers present." << std::endl; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); itksys::SystemTools::Delay(1); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->DSTOP(); if (returnvalue != NDIOKAY) return; // how can this thread tell the application, that an error has occured? this->SetState(Ready); return; // returning from this function (and ThreadStartTracking()) this will end the thread } void mitk::NDITrackingDevice::TrackToolsAndMarkers() { if (m_OperationMode != HybridTracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->TSTART(); // Start Diagnostic Mode if (returnvalue != NDIOKAY) return; MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); while ((this->GetState() == Tracking) && (localStopTracking == false)) { m_MarkerPointsMutex->Lock(); // lock points data structure returnvalue = this->m_DeviceProtocol->TX(true, &m_MarkerPoints); // update points data structure with new position data from tracking device m_MarkerPointsMutex->Unlock(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors { std::cout << "Error in TX: could not read data. Possibly no markers present." << std::endl; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->TSTOP(); if (returnvalue != NDIOKAY) return; // how can this thread tell the application, that an error has occurred? this->SetState(Ready); return; // returning from this function (and ThreadStartTracking()) this will end the thread } mitk::TrackingTool* mitk::NDITrackingDevice::GetTool(unsigned int toolNumber) const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex if (toolNumber < m_6DTools.size()) return m_6DTools.at(toolNumber); return NULL; } mitk::TrackingTool* mitk::NDITrackingDevice::GetToolByName(std::string name) const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::const_iterator end = m_6DTools.end(); for (Tool6DContainerType::const_iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) if (name.compare((*iterator)->GetToolName()) == 0) return *iterator; return NULL; } mitk::NDIPassiveTool* mitk::NDITrackingDevice::GetInternalTool(std::string portHandle) { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) if (portHandle.compare((*iterator)->GetPortHandle()) == 0) return *iterator; return NULL; } unsigned int mitk::NDITrackingDevice::GetToolCount() const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex return m_6DTools.size(); } bool mitk::NDITrackingDevice::Beep(unsigned char count) { if (this->GetState() != Setup) { return (m_DeviceProtocol->BEEP(count) == NDIOKAY); } else { return false; } } mitk::TrackingTool* mitk::NDITrackingDevice::AddTool( const char* toolName, const char* fileName, TrackingPriority p /*= NDIPassiveTool::Dynamic*/ ) { mitk::NDIPassiveTool::Pointer t = mitk::NDIPassiveTool::New(); if (t->LoadSROMFile(fileName) == false) return NULL; t->SetToolName(toolName); t->SetTrackingPriority(p); if (this->InternalAddTool(t) == false) return NULL; return t.GetPointer(); } bool mitk::NDITrackingDevice::InternalAddTool(mitk::NDIPassiveTool* tool) { if (tool == NULL) return false; NDIPassiveTool::Pointer p = tool; /* if the connection to the tracking device is already established, add the new tool to the device now */ if (this->GetState() == Ready) { /* get a port handle for the tool */ std::string newPortHandle; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PHRQ(&newPortHandle); if (returnvalue == NDIOKAY) { p->SetPortHandle(newPortHandle.c_str()); /* now write the SROM file of the tool to the tracking system using PVWR */ returnvalue = m_DeviceProtocol->PVWR(&newPortHandle, p->GetSROMData(), p->GetSROMDataLength()); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + p->GetToolName() + std::string("' to tracking device")).c_str();} /* initialize the port handle */ returnvalue = m_DeviceProtocol->PINIT(&newPortHandle); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize port '") + newPortHandle + std::string("' for tool '")+ p->GetToolName() + std::string("'")).c_str(); } /* enable the port handle */ if (p->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&newPortHandle, p->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + newPortHandle + std::string("' for tool '")+ p->GetToolName() + std::string("'")).c_str(); } } } /* now that the tool is added to the device, add it to list too */ m_ToolsMutex->Lock(); this->m_6DTools.push_back(p); m_ToolsMutex->Unlock(); this->Modified(); return true; } else if (this->GetState() == Setup) { /* In Setup mode, we only add it to the list, so that OpenConnection() can add it later */ m_ToolsMutex->Lock(); this->m_6DTools.push_back(p); m_ToolsMutex->Unlock(); this->Modified(); return true; } else // in Tracking mode, no tools can be added return false; } bool mitk::NDITrackingDevice::RemoveTool(mitk::TrackingTool* tool) { mitk::NDIPassiveTool* ndiTool = dynamic_cast(tool); if (ndiTool == NULL) return false; std::string portHandle = ndiTool->GetPortHandle(); /* a valid portHandle has length 2. If a valid handle exists, the tool is already added to the tracking device, so we have to remove it there if the connection to the tracking device has already been established. */ if ((portHandle.length() == 2) && (this->GetState() == Ready)) // do not remove a tool in tracking mode { NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PHF(&portHandle); if (returnvalue != NDIOKAY) return false; /* Now that the tool is removed from the tracking device, remove it from our tool list too */ MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex (scope is inside the if-block Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) { if (iterator->GetPointer() == ndiTool) { m_6DTools.erase(iterator); this->Modified(); return true; } } return false; } else if (this->GetState() == Setup) // in Setup Mode, we are not connected to the tracking device, so we can just remove the tool from the tool list { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) { if ((*iterator).GetPointer() == ndiTool) { m_6DTools.erase(iterator); this->Modified(); return true; } } return false; } return false; } void mitk::NDITrackingDevice::InvalidateAll() { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex Tool6DContainerType::iterator end = m_6DTools.end(); for (Tool6DContainerType::iterator iterator = m_6DTools.begin(); iterator != end; ++iterator) (*iterator)->SetDataValid(false); } bool mitk::NDITrackingDevice::SetOperationMode(OperationMode mode) { if (GetState() == Tracking) return false; m_OperationMode = mode; return true; } mitk::OperationMode mitk::NDITrackingDevice::GetOperationMode() { return m_OperationMode; } bool mitk::NDITrackingDevice::GetMarkerPositions(MarkerPointContainerType* markerpositions) { m_MarkerPointsMutex->Lock(); *markerpositions = m_MarkerPoints; // copy the internal vector to the one provided m_MarkerPointsMutex->Unlock(); return (markerpositions->size() != 0) ; } bool mitk::NDITrackingDevice::DiscoverWiredTools() { /* First, check for disconnected tools and remove them */ this->FreePortHandles(); /* check for new tools, add and initialize them */ NDIErrorCode returnvalue; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected";} /* if there are port handles that need to be initialized, initialize them. Furthermore instantiate tools for each handle that has no tool yet. */ std::string ph; /* we need to remember the ports which are occupied to be able to readout the serial numbers of the connected tools later */ std::vector occupiedPorts = std::vector(); int numberOfToolsAtStart = this->GetToolCount(); //also remember the number of tools at start to identify the automatically detected tools later for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); if (this->GetInternalTool(ph) != NULL) // if we already have a tool with this handle continue; // then skip the initialization //instantiate an object for each tool that is connected mitk::NDIPassiveTool::Pointer newTool = mitk::NDIPassiveTool::New(); newTool->SetPortHandle(ph.c_str()); newTool->SetTrackingPriority(mitk::NDIPassiveTool::Dynamic); //set a name for identification newTool->SetToolName((std::string("Port ") + ph).c_str()); returnvalue = m_DeviceProtocol->PINIT(&ph); if (returnvalue != NDIINITIALIZATIONFAILED) //if the initialization failed (AURORA) it can not be enabled. A srom file will have to be specified manually first. Still return true to be able to continue { if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize port '") + ph + std::string("' for tool '")+ newTool->GetToolName() + std::string("'")).c_str(); } /* enable the port handle */ returnvalue = m_DeviceProtocol->PENA(&ph, newTool->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + ph + std::string("' for tool '")+ newTool->GetToolName() + std::string("'")).c_str(); } } //we have to temporarily unlock m_ModeMutex here to avoid a deadlock with another lock inside InternalAddTool() if (this->InternalAddTool(newTool) == false) {mitkThrowException(mitk::IGTException) << "Error while adding new tool";} else occupiedPorts.push_back(i); } // after initialization readout serial numbers of automatically detected tools for (unsigned int i = 0; i < occupiedPorts.size(); i++) { ph = portHandle.substr(occupiedPorts.at(i), 2); std::string portInfo; NDIErrorCode returnvaluePort = m_DeviceProtocol->PHINF(ph, &portInfo); if ((returnvaluePort==NDIOKAY) && (portInfo.size()>31)) dynamic_cast(this->GetTool(i+numberOfToolsAtStart))->SetSerialNumber(portInfo.substr(23,8)); itksys::SystemTools::Delay(10); } return true; } mitk::NDIErrorCode mitk::NDITrackingDevice::FreePortHandles() { /* first search for port handles that need to be freed: e.g. because of a reset of the tracking system */ NDIErrorCode returnvalue = NDIOKAY; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(FREED, &portHandle); if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that need to be freed";} /* if there are port handles that need to be freed, free them */ if (portHandle.empty() == true) return returnvalue; std::string ph; for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); mitk::NDIPassiveTool* t = this->GetInternalTool(ph); if (t != NULL) // if we have a tool for the port handle that needs to be freed { if (this->RemoveTool(t) == false) // remove it (this will free the port too) returnvalue = NDIERROR; } else // we don't have a tool, the port handle exists only in the tracking device { returnvalue = m_DeviceProtocol->PHF(&ph); // free it there // What to do if port handle could not be freed? This seems to be a non critical error if (returnvalue != NDIOKAY) {mitkThrowException(mitk::IGTHardwareException) << "Could not free all port handles";} } } return returnvalue; } int mitk::NDITrackingDevice::GetMajorFirmwareRevisionNumber() { std::string revision; if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9) ) { MITK_ERROR << "Could not receive firmware revision number!"; return 0; } const std::string majrevno = revision.substr(2,3); //cut out "004" from "D.004.001" return std::atoi(majrevno.c_str()); } const char* mitk::NDITrackingDevice::GetFirmwareRevisionNumber() { static std::string revision; if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9) ) { MITK_ERROR << "Could not receive firmware revision number!"; revision = ""; return revision.c_str(); } return revision.c_str(); } bool mitk::NDITrackingDevice::GetSupportedVolumes(unsigned int* numberOfVolumes, mitk::NDITrackingDevice::NDITrackingVolumeContainerType* volumes, mitk::NDITrackingDevice::TrackingVolumeDimensionType* volumesDimensions) { if (numberOfVolumes == NULL || volumes == NULL || volumesDimensions == NULL) return false; static std::string info; if (m_DeviceProtocol->SFLIST(&info) != mitk::NDIOKAY || info.empty()) { MITK_ERROR << "Could not receive tracking volume information of tracking system!"; return false; } /*info contains the following: (+n times:) */ (*numberOfVolumes) = (unsigned int) std::atoi(info.substr(0,1).c_str()); for (unsigned int i=0; i<(*numberOfVolumes); i++) { //e.g. for cube: "9-025000+025000-025000+025000-055000-005000+000000+000000+000000+00000011" //for dome: "A+005000+048000+005000+066000+000000+000000+000000+000000+000000+00000011" std::string::size_type offset, end; offset = (i*73)+1; end = 73+(i*73); std::string currentVolume = info.substr(offset, end);//i=0: from 1 to 73 characters; i=1: from 75 to 148 char; // if i>0 then we have a return statement infront if (i>0) currentVolume = currentVolume.substr(1, currentVolume.size()); std::string standard = "0"; std::string pyramid = "4"; std::string spectraPyramid = "5-2"; std::string spectraExtendedPyramid = "5-3"; std::string vicraVolume = "7"; std::string cube = "9"; std::string dome = "A"; if (currentVolume.compare(0,1,standard)==0) - volumes->push_back(mitk::Standard); + volumes->push_back("Polaris Spectra"); if (currentVolume.compare(0,1,pyramid)==0) - volumes->push_back(mitk::Pyramid); + volumes->push_back("Polaris Spectra"); if (currentVolume.compare(0,3,spectraPyramid)==0) - volumes->push_back(mitk::SpectraPyramid); + volumes->push_back("Polaris Spectra"); if (currentVolume.compare(1,3,spectraExtendedPyramid)==0) { currentVolume = currentVolume.substr(1,currentVolume.size()); - volumes->push_back(mitk::SpectraPyramid); + volumes->push_back("Polaris Spectra Extended Pyramid"); } if (currentVolume.compare(0,1,vicraVolume)==0) - volumes->push_back(mitk::VicraVolume); + volumes->push_back("Polaris Vicra"); else if (currentVolume.compare(0,1,cube)==0) - volumes->push_back(mitk::Cube);//alias cube + volumes->push_back("Aurora Planar (Cube)");//alias cube else if (currentVolume.compare(0,1,dome)==0) - volumes->push_back(mitk::Dome); + volumes->push_back("Aurora Planar (Dome)"); //fill volumesDimensions for (unsigned int index = 0; index < 10; index++) { std::string::size_type offD, endD; offD = 1+(index*7); //7 digits per dimension and the first is the type of volume endD = offD+7; int dimension = std::atoi(currentVolume.substr(offD, endD).c_str()); dimension /= 100; //given in mm. 7 digits are xxxx.xx according to NDI //strange, the last two digits (11) also for the metal flag get read also... volumesDimensions->push_back(dimension); } } return true; } -bool mitk::NDITrackingDevice::SetVolume(NDITrackingVolume volume) +bool mitk::NDITrackingDevice::SetVolume(mitk::TrackingDeviceData volume) { if (m_DeviceProtocol->VSEL(volume) != mitk::NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not set volume!"; } return true; } diff --git a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.h b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.h index c97f6b0082..66009a2fc3 100644 --- a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.h @@ -1,320 +1,324 @@ /*=================================================================== 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 MITKNDITRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 #define MITKNDITRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 #include "mitkTrackingDevice.h" #include #include #include "itkFastMutexLock.h" #include #include "mitkTrackingTypes.h" #include "mitkNDIProtocol.h" #include "mitkNDIPassiveTool.h" #include "mitkSerialCommunication.h" namespace mitk { class NDIProtocol; /** Documentation * \brief superclass for specific NDI tracking Devices that use serial communication. * * implements the TrackingDevice interface for NDI tracking devices (POLARIS, AURORA) * * \ingroup IGT */ class MitkIGT_EXPORT NDITrackingDevice : public TrackingDevice { friend class NDIProtocol; public: typedef std::vector Tool6DContainerType; ///< List of 6D tools of the correct type for this tracking device - typedef std::vector TrackingVolumeDimensionType; ///< List of the supported tracking volume dimensions. + typedef mitk::TrackingDeviceType NDITrackingDeviceType; ///< This enumeration includes the two types of NDI tracking devices (Polaris, Aurora). - typedef std::vector NDITrackingVolumeContainerType; ///< vector of tracking volumes typedef mitk::SerialCommunication::PortNumber PortNumber; ///< Port number of the serial connection typedef mitk::SerialCommunication::BaudRate BaudRate; ///< Baud rate of the serial connection typedef mitk::SerialCommunication::DataBits DataBits; ///< Number of data bits used in the serial connection typedef mitk::SerialCommunication::Parity Parity; ///< Parity mode used in the serial connection typedef mitk::SerialCommunication::StopBits StopBits; ///< Number of stop bits used in the serial connection typedef mitk::SerialCommunication::HardwareHandshake HardwareHandshake; ///< Hardware handshake mode of the serial connection typedef mitk::NDIPassiveTool::TrackingPriority TrackingPriority; ///< Tracking priority used for tracking a tool - mitkClassMacro(NDITrackingDevice, TrackingDevice); itkNewMacro(Self); /** * \brief Set the type of the NDI Tracking Device because it can not jet handle this itself */ //itkSetMacro(Type, TrackingDeviceType); /** * \brief initialize the connection to the tracking device * * OpenConnection() establishes the connection to the tracking device by: * - initializing the serial port with the given parameters (port number, baud rate, ...) * - connection to the tracking device * - initializing the device * - initializing all manually added passive tools (user supplied srom file) * - initializing active tools that are connected to the tracking device * @throw mitk::IGTHardwareException Throws an exception if there are errors while connecting to the device. * @throw mitk::IGTException Throws a normal IGT exception if an error occures which is not related to the hardware. */ virtual bool OpenConnection(); /** * \brief Closes the connection * * CloseConnection() resets the tracking device, invalidates all tools and then closes the serial port. */ virtual bool CloseConnection(); /** @throw mitk::IGTHardwareException Throws an exception if there are errors while connecting to the device. */ bool InitializeWiredTools(); /** Sets the rotation mode of this class. See documentation of enum RotationMode for details * on the different modes. */ virtual void SetRotationMode(RotationMode r); /** * \brief TestConnection() tries to connect to a NDI tracking device on the current port/device and returns which device it has found * * TestConnection() tries to connect to a NDI tracking device on the current port/device. * \return It returns the type of the device that answers at the port/device. Throws an exception if no device is available on that port. * @throw mitk::IGTHardwareException Throws an exception if there are errors while connecting to the device. */ virtual mitk::TrackingDeviceType TestConnection(); /** * \brief retrieves all wired tools from the tracking device * * This method queries the tracking device for all wired tools, initializes them and creates TrackingTool representation objects * for them * \return True if the method was executed successful. * @throw mitk::IGTHardwareException Throws an exception if there are errors while connecting to the device. * @throw mitk::IGTException Throws a normal IGT exception if an error occures which is not related to the hardware. */ bool DiscoverWiredTools(); /** * \brief Start the tracking. * * A new thread is created, which continuously reads the position and orientation information of each tool and stores them inside the tools. * Depending on the current operation mode (see SetOperationMode()), either the 6D tools (ToolTracking6D), 5D tools (ToolTracking5D), * 3D marker positions (MarkerTracking3D) or both 6D tools and 3D markers (HybridTracking) are updated. * Call StopTracking() to stop the tracking thread. */ virtual bool StartTracking(); /** * \brief return the tool with index toolNumber */ virtual TrackingTool* GetTool(unsigned int toolNumber) const; virtual mitk::TrackingTool* GetToolByName(std::string name) const; /** * \brief return current number of tools */ virtual unsigned int GetToolCount() const; /** * \brief Create a passive 6D tool with toolName and fileName and add it to the list of tools * * This method will create a new NDIPassiveTool object, load the SROM file fileName, * set the tool name toolName and the tracking priority p and then add * it to the list of tools. It returns a pointer of type mitk::TrackingTool to the tool * that can be used to read tracking data from it. * This is the only way to add tools to NDITrackingDevice. * @throw mitk::IGTHardwareException Throws an exception if there are errors while adding the tool. * * \warning adding tools is not possible in tracking mode, only in setup and ready. */ mitk::TrackingTool* AddTool(const char* toolName, const char* fileName, TrackingPriority p = NDIPassiveTool::Dynamic); /** * \brief Remove a passive 6D tool from the list of tracked tools. * * \warning removing tools is not possible in tracking mode, only in setup and ready modes. */ virtual bool RemoveTool(TrackingTool* tool); /** * \brief reloads the srom file and reinitializes the tool */ virtual bool UpdateTool(mitk::TrackingTool* tool); virtual void SetPortNumber(const PortNumber _arg); ///< set port number for serial communication itkGetConstMacro(PortNumber, PortNumber); ///< returns the port number for serial communication virtual void SetDeviceName(std::string _arg); ///< set device name (e.g. COM1, /dev/ttyUSB0). If this is set, PortNumber will be ignored itkGetStringMacro(DeviceName); ///< returns the device name for serial communication virtual void SetBaudRate(const BaudRate _arg); ///< set baud rate for serial communication itkGetConstMacro(BaudRate, BaudRate); ///< returns the baud rate for serial communication virtual void SetDataBits(const DataBits _arg); ///< set number of data bits itkGetConstMacro(DataBits, DataBits); ///< returns the data bits for serial communication virtual void SetParity(const Parity _arg); ///< set parity mode itkGetConstMacro(Parity, Parity); ///< returns the parity mode virtual void SetStopBits(const StopBits _arg); ///< set number of stop bits itkGetConstMacro(StopBits, StopBits); ///< returns the number of stop bits virtual void SetHardwareHandshake(const HardwareHandshake _arg); ///< set use hardware handshake for serial communication itkGetConstMacro(HardwareHandshake, HardwareHandshake); ///< returns the hardware handshake setting virtual void SetIlluminationActivationRate(const IlluminationActivationRate _arg); ///< set activation rate of IR illumator for polaris itkGetConstMacro(IlluminationActivationRate, IlluminationActivationRate); ///< returns the activation rate of IR illumator for polaris virtual void SetDataTransferMode(const DataTransferMode _arg); ///< set data transfer mode to text (TX) or binary (BX). \warning: only TX is supportet at the moment itkGetConstMacro(DataTransferMode, DataTransferMode); ///< returns the data transfer mode virtual bool Beep(unsigned char count); ///< Beep the tracking device 1 to 9 times NDIErrorCode GetErrorCode(const std::string* input); ///< returns the error code for a string that contains an error code in hexadecimal format virtual bool SetOperationMode(OperationMode mode); ///< set operation mode to 6D tool tracking, 3D marker tracking or 6D&3D hybrid tracking (see OperationMode) virtual OperationMode GetOperationMode(); ///< get current operation mode /** * \brief Get 3D marker positions (operation mode must be set to MarkerTracking3D or HybridTracking) */ virtual bool GetMarkerPositions(MarkerPointContainerType* markerpositions); /** * \brief Get major revision number from tracking device * should not be called directly after starting to track **/ virtual int GetMajorFirmwareRevisionNumber(); /** * \brief Get revision number from tracking device as string * should not be called directly after starting to track **/ virtual const char* GetFirmwareRevisionNumber(); - /** + + + + protected: + + typedef std::vector NDITrackingVolumeContainerType; ///< vector of tracking volumes + typedef std::vector TrackingVolumeDimensionType; ///< List of the supported tracking volume dimensions. + + /** * \brief Get number of supported tracking volumes, a vector containing the supported volumes and * a vector containing the signed dimensions in mm. For each volume 10 boundaries are stored in the order of * the supported volumes (see AURORA API GUIDE: SFLIST p.54). **/ virtual bool GetSupportedVolumes(unsigned int* numberOfVolumes, NDITrackingVolumeContainerType* volumes, TrackingVolumeDimensionType* volumesDimensions); - /** - * \brief Sets the desired tracking volume. Returns true if the volume type could be set. Usage: ndiTracker->SetVolume(mitk::Dome); + /** + * \brief Sets the desired tracking volume. Returns true if the volume type could be set. It is set in the OpenConnection() Method and sets the tracking volume out of m_Data. * @throw mitk::IGTHardwareException Throws an IGT hardware exception if the volume could not be set. **/ - virtual bool SetVolume(NDITrackingVolume volume); - - protected: + virtual bool SetVolume(mitk::TrackingDeviceData volume); /** * \brief Add a passive 6D tool to the list of tracked tools. This method is used by AddTool * @throw mitk::IGTHardwareException Throws an exception if there are errors while adding the tool. * \warning adding tools is not possible in tracking mode, only in setup and ready. */ virtual bool InternalAddTool(NDIPassiveTool* tool); /* Methods for NDIProtocol friend class */ virtual void InvalidateAll(); ///< invalidate all tools NDIPassiveTool* GetInternalTool(std::string portHandle); ///< returns the tool object that has been assigned the port handle or NULL if no tool can be found /** * \brief free all port handles that need to be freed * * This method retrieves a list of all port handles that need to be freed (e.g. tool got disconnected) * and frees the handles at the tracking device and it removes the tools from the internal tool list * \warning This method can remove TrackingTools from the tool list! After calling this method, GetTool(i) could return * a different tool, because tool indices could have changed. * @throw mitk::IGTHardwareException Throws an exception if there are errors while communicating with the device. * \return returns NDIOKAY if everything was sucessfull, returns an error code otherwise */ NDIErrorCode FreePortHandles(); NDIErrorCode Send(const std::string* message, bool addCRC = true); ///< Send message to tracking device NDIErrorCode Receive(std::string* answer, unsigned int numberOfBytes); ///< receive numberOfBytes bytes from tracking device NDIErrorCode ReceiveByte(char* answer); ///< lightweight receive function, that reads just one byte NDIErrorCode ReceiveLine(std::string* answer); ///< receive characters until the first LF (The LF is included in the answer string) void ClearSendBuffer(); ///< empty send buffer of serial communication interface void ClearReceiveBuffer(); ///< empty receive buffer of serial communication interface const std::string CalcCRC(const std::string* input); ///< returns the CRC16 for input as a std::string public: /** * \brief TrackTools() continuously polls serial interface for new 6d tool positions until StopTracking is called. * * Continuously tracks the 6D position of all tools until StopTracking() is called. * This function is executed by the tracking thread (through StartTracking() and ThreadStartTracking()). * It should not be called directly. * @throw mitk::IGTHardwareException Throws an exception if there are errors while tracking the tools. */ virtual void TrackTools(); /** * \brief continuously polls serial interface for new 3D marker positions until StopTracking is called. * * Continuously tracks the 3D position of all markers until StopTracking() is called. * This function is executed by the tracking thread (through StartTracking() and ThreadStartTracking()). * It should not be called directly. */ virtual void TrackMarkerPositions(); /** * \brief continuously polls serial interface for new 3D marker positions and 6D tool positions until StopTracking is called. * * Continuously tracks the 3D position of all markers and the 6D position of all tools until StopTracking() is called. * This function is executed by the tracking thread (through StartTracking() and ThreadStartTracking()). * It should not be called directly. */ virtual void TrackToolsAndMarkers(); /** * \brief static start method for the tracking thread. */ static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void* data); protected: NDITrackingDevice(); ///< Constructor virtual ~NDITrackingDevice(); ///< Destructor std::string m_DeviceName;///< Device Name PortNumber m_PortNumber; ///< COM Port Number BaudRate m_BaudRate; ///< COM Port Baud Rate DataBits m_DataBits; ///< Number of Data Bits per token Parity m_Parity; ///< Parity mode for communication StopBits m_StopBits; ///< number of stop bits per token HardwareHandshake m_HardwareHandshake; ///< use hardware handshake for serial port connection - NDITrackingVolume m_NDITrackingVolume; ///< which tracking volume is currently used (if device supports multiple volumes) (\warning This parameter is not used yet) + ///< which tracking volume is currently used (if device supports multiple volumes) (\warning This parameter is not used yet) IlluminationActivationRate m_IlluminationActivationRate; ///< update rate of IR illuminator for Polaris DataTransferMode m_DataTransferMode; ///< use TX (text) or BX (binary) (\warning currently, only TX mode is supported) Tool6DContainerType m_6DTools; ///< list of 6D tools itk::FastMutexLock::Pointer m_ToolsMutex; ///< mutex for coordinated access of tool container mitk::SerialCommunication::Pointer m_SerialCommunication; ///< serial communication interface itk::FastMutexLock::Pointer m_SerialCommunicationMutex; ///< mutex for coordinated access of serial communication interface NDIProtocol::Pointer m_DeviceProtocol; ///< create and parse NDI protocol strings itk::MultiThreader::Pointer m_MultiThreader; ///< creates tracking thread that continuously polls serial interface for new tracking data int m_ThreadID; ///< ID of tracking thread OperationMode m_OperationMode; ///< tracking mode (6D tool tracking, 3D marker tracking,...) itk::FastMutexLock::Pointer m_MarkerPointsMutex; ///< mutex for marker point data container MarkerPointContainerType m_MarkerPoints; ///< container for markers (3D point tracking mode) }; } // namespace mitk #endif /* MITKNDITRACKINGDEVICE_H_HEADER_INCLUDED_C1C2FCD2 */