From d2bff14b70bb86ff9ccdc37b5bb378f53b7f68b4 Mon Sep 17 00:00:00 2001 From: Matt Clarkson Date: Fri, 11 Jul 2014 11:06:24 +0100 Subject: [PATCH 01/12] #47: Placeholder igtlFullTrackingDataWithErrorMessage type --- Source/CMakeLists.txt | 2 + .../igtlFullTrackingDataWithErrorMessage.cxx | 134 ++++++++++++++++++ Source/igtlFullTrackingDataWithErrorMessage.h | 86 +++++++++++ 3 files changed, 222 insertions(+) create mode 100644 Source/igtlFullTrackingDataWithErrorMessage.cxx create mode 100644 Source/igtlFullTrackingDataWithErrorMessage.h diff --git a/Source/CMakeLists.txt b/Source/CMakeLists.txt index df08e6ba..607366fd 100644 --- a/Source/CMakeLists.txt +++ b/Source/CMakeLists.txt @@ -121,6 +121,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtlutil/igtl_polydata.c igtlColorTableMessage.cxx igtlCapabilityMessage.cxx + igtlFullTrackingDataWithErrorMessage.cxx igtlImageMetaMessage.cxx igtlLabelMetaMessage.cxx igtlPointMessage.cxx @@ -152,6 +153,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtlutil/igtl_polydata.h igtlColorTableMessage.h igtlCapabilityMessage.h + igtlFullTrackingDataWithErrorMessage.h igtlImageMetaMessage.h igtlLabelMetaMessage.h igtlPointMessage.h diff --git a/Source/igtlFullTrackingDataWithErrorMessage.cxx b/Source/igtlFullTrackingDataWithErrorMessage.cxx new file mode 100644 index 00000000..47c9e303 --- /dev/null +++ b/Source/igtlFullTrackingDataWithErrorMessage.cxx @@ -0,0 +1,134 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C++ + Web page: http://openigtlink.org/ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#include "igtlFullTrackingDataWithErrorMessage.h" + +#include "igtl_header.h" +#include "igtl_tdata.h" + +// Disable warning C4996 (strncpy() may be unsafe) in Windows. +#define _CRT_SECURE_NO_WARNINGS + +#include +#include + +namespace igtl { + +//---------------------------------------------------------------------- +FullTrackingDataWithErrorMessage::FullTrackingDataWithErrorMessage() + : TrackingDataMessage() +{ + this->m_DefaultBodyType = "FULLDATA"; +} + + +//---------------------------------------------------------------------- +FullTrackingDataWithErrorMessage::~FullTrackingDataWithErrorMessage() +{ +} + + +//---------------------------------------------------------------------- +int FullTrackingDataWithErrorMessage::GetBodyPackSize() +{ +/* + // The body size sum of the header size and status message size. + return IGTL_TDATA_ELEMENT_SIZE * this->m_TrackingDataList.size(); +*/ + return 1; +} + + +//---------------------------------------------------------------------- +int FullTrackingDataWithErrorMessage::PackBody() +{ +/* + // allocate pack + AllocatePack(); + + igtl_tdata_element* element; + element = (igtl_tdata_element*)this->m_Body; + std::vector::iterator iter; + + for (iter = this->m_TrackingDataList.begin(); iter != this->m_TrackingDataList.end(); iter ++) + { + strncpy((char*)element->name, (*iter)->GetName(), IGTL_TDATA_LEN_NAME); + element->type = (*iter)->GetType(); + + Matrix4x4 matrix; + (*iter)->GetMatrix(matrix); + for (int i = 0; i < 3; i ++) + { + element->transform[i] = matrix[i][0]; + element->transform[i+3] = matrix[i][1]; + element->transform[i+6] = matrix[i][2]; + element->transform[i+9] = matrix[i][3]; + } + element ++; + } + + igtl_tdata_convert_byte_order((igtl_tdata_element*)this->m_Body, this->m_TrackingDataList.size()); +*/ + return 1; +} + + +//---------------------------------------------------------------------- +int FullTrackingDataWithErrorMessage::UnpackBody() +{ +/* + this->m_TrackingDataList.clear(); + + igtl_tdata_element* element = (igtl_tdata_element*) this->m_Body; + int nElement = igtl_tdata_get_data_n(this->m_BodySizeToRead); + + igtl_tdata_convert_byte_order(element, nElement); + + char strbuf[128]; + for (int i = 0; i < nElement; i ++) + { + TrackingDataElement::Pointer elemClass = TrackingDataElement::New(); + + // Add '\n' at the end of each string + // (neccesary for a case, where a string reaches the maximum length.) + strbuf[IGTL_TDATA_LEN_NAME] = '\n'; + strncpy(strbuf, (char*)element->name, IGTL_TDATA_LEN_NAME); + elemClass->SetName((const char*)strbuf); + elemClass->SetType(element->type); + + Matrix4x4 matrix; + IdentityMatrix(matrix); + for (int j = 0; j < 3; j ++) + { + matrix[j][0] = element->transform[j]; + matrix[j][1] = element->transform[j+3]; + matrix[j][2] = element->transform[j+6]; + matrix[j][3] = element->transform[j+9]; + } + elemClass->SetMatrix(matrix); + + this->m_TrackingDataList.push_back(elemClass); + + element ++; + } +*/ + return 1; +} + +} // namespace igtl + + + + + diff --git a/Source/igtlFullTrackingDataWithErrorMessage.h b/Source/igtlFullTrackingDataWithErrorMessage.h new file mode 100644 index 00000000..35c56236 --- /dev/null +++ b/Source/igtlFullTrackingDataWithErrorMessage.h @@ -0,0 +1,86 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C++ + Web page: http://openigtlink.org/ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#ifndef __igtlFullTrackingDataWithErrorMessage_h +#define __igtlFullTrackingDataWithErrorMessage_h + +#include "igtlTrackingDataMessage.h" +#include "igtlMacro.h" + +namespace igtl +{ + +class IGTLCommon_EXPORT TrackingDataWithErrorElement: public TrackingDataElement +{ +public: + typedef TrackingDataWithErrorElement Self; + typedef TrackingDataElement Superclass; + typedef SmartPointer Pointer; + typedef SmartPointer ConstPointer; + + igtlTypeMacro(igtl::TrackingDataWithErrorElement, igtl::TrackingDataElement); + igtlNewMacro(igtl::TrackingDataWithErrorElement); + +public: + igtlGetConstMacro(Error, igtlFloat32); + igtlSetMacro(Error, igtlFloat32); + +protected: + TrackingDataWithErrorElement() : m_Error(0) {} + ~TrackingDataWithErrorElement() {} + +protected: + + /// Some tracking systems (e.g. NDI Certus) give the tracking error. + igtlFloat32 m_Error; +}; + + +class IGTLCommon_EXPORT FullTrackingDataWithErrorMessage: public TrackingDataMessage +{ +public: + typedef FullTrackingDataWithErrorMessage Self; + typedef TrackingDataMessage Superclass; + typedef SmartPointer Pointer; + typedef SmartPointer ConstPointer; + + igtlTypeMacro(igtl::FullTrackingDataWithErrorMessage, igtl::TrackingDataMessage); + igtlNewMacro(igtl::FullTrackingDataWithErrorMessage); + +public: + + /// Gets the tracking data element specified by 'index'. + void GetTrackingDataElement(int index, TrackingDataWithErrorElement::Pointer& elem); + +protected: + FullTrackingDataWithErrorMessage(); + ~FullTrackingDataWithErrorMessage(); + +protected: + + virtual int GetBodyPackSize(); + virtual int PackBody(); + virtual int UnpackBody(); + + /// The list of trakcing data elements. + std::vector m_TrackingDataList; + +}; + +} // namespace igtl + +#endif // __igtlFullTrackingDataWithErrorMessage_h + + + From 70552c2a16d7a79eb8161eab969c2d38ff5f06d6 Mon Sep 17 00:00:00 2001 From: Steve Thompson Date: Thu, 31 Oct 2013 11:22:39 +0000 Subject: [PATCH 02/12] #47: fixed bug in igtl_message_dump --- Source/igtlutil/igtl_util.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Source/igtlutil/igtl_util.c b/Source/igtlutil/igtl_util.c index 51cde514..2b95d8e7 100644 --- a/Source/igtlutil/igtl_util.c +++ b/Source/igtlutil/igtl_util.c @@ -233,7 +233,8 @@ void igtl_export igtl_message_dump_hex(FILE* stream, const void* message, int by for (j = 0; j < cols_in_last_row; j ++) { - fprintf(stream, "%02x ", p[i]); + fprintf(stream, "%02x ", *p); + p++; } fprintf(stream, "\n"); From 6cf01efaf1909e2d82511ff4090566d04ea7b472 Mon Sep 17 00:00:00 2001 From: Steve Thompson Date: Thu, 31 Oct 2013 17:09:52 +0000 Subject: [PATCH 03/12] #47: created new message types, to enable backward compatability --- Source/igtlutil/igtl_fulltdata.c | 99 ++++++++++++++++++++++++++++ Source/igtlutil/igtl_fulltdata.h | 90 +++++++++++++++++++++++++ Source/igtlutil/igtl_fulltransform.c | 45 +++++++++++++ Source/igtlutil/igtl_fulltransform.h | 43 ++++++++++++ 4 files changed, 277 insertions(+) create mode 100644 Source/igtlutil/igtl_fulltdata.c create mode 100644 Source/igtlutil/igtl_fulltdata.h create mode 100644 Source/igtlutil/igtl_fulltransform.c create mode 100644 Source/igtlutil/igtl_fulltransform.h diff --git a/Source/igtlutil/igtl_fulltdata.c b/Source/igtlutil/igtl_fulltdata.c new file mode 100644 index 00000000..7fb6d4bf --- /dev/null +++ b/Source/igtlutil/igtl_fulltdata.c @@ -0,0 +1,99 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#include +#include + +#include "igtl_tdata.h" +#include "igtl_util.h" + +void igtl_export igtl_tdata_convert_byte_order(igtl_tdata_element* tdatalist, int nitem) +{ + igtl_tdata_element* elem; + int i; + int j; + igtl_int32* tmp; + + for (i = 0; i < nitem; i ++) + { + elem = &(tdatalist[i]); + if (igtl_is_little_endian()) + { + for (j = 0; j < 12; j ++) + { + tmp = (igtl_int32*)&(elem->transform[j]); + *tmp = BYTE_SWAP_INT32(*tmp); + } + tmp = ( igtl_int32*)&(elem->error); + *tmp = BYTE_SWAP_INT32(*tmp); + } + + } +} + + +void igtl_export igtl_stt_tdata_convert_byte_order(igtl_stt_tdata* stt_tdata) +{ + igtl_int32* tmp; + + if (igtl_is_little_endian()) + { + tmp = (igtl_int32*)&(stt_tdata->resolution); + *tmp = BYTE_SWAP_INT32(*tmp); + } +} + + +void igtl_export igtl_rts_tdata_convert_byte_order(igtl_rts_tdata* rts_tdata) +{ + // do nothing +} + + +igtl_uint64 igtl_export igtl_tdata_get_crc(igtl_tdata_element* tdatalist, int nitem) +{ + igtl_tdata_element* elem; + int i; + igtl_uint64 crc; + + crc = crc64(0, 0, 0); + for (i = 0; i < nitem; i ++) + { + elem = &(tdatalist[i]); + crc = crc64((unsigned char*) elem, IGTL_TDATA_ELEMENT_SIZE, crc); + } + + return crc; +} + + +igtl_uint64 igtl_export igtl_stt_tdata_get_crc(igtl_stt_tdata* stt_tdata) +{ + igtl_uint64 crc; + + crc = crc64(0, 0, 0); + crc = crc64((unsigned char*) stt_tdata, IGTL_STT_TDATA_SIZE, crc); + return crc; +} + + +igtl_uint64 igtl_export igtl_rts_tdata_get_crc(igtl_rts_tdata* rts_tdata) +{ + igtl_uint64 crc; + + crc = crc64(0, 0, 0); + crc = crc64((unsigned char*) rts_tdata, IGTL_RTS_TDATA_SIZE, crc); + return crc; +} + diff --git a/Source/igtlutil/igtl_fulltdata.h b/Source/igtlutil/igtl_fulltdata.h new file mode 100644 index 00000000..79845f99 --- /dev/null +++ b/Source/igtlutil/igtl_fulltdata.h @@ -0,0 +1,90 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#ifndef __IGTL_TDATA_H +#define __IGTL_TDATA_H + +#include "igtl_win32header.h" +#include "igtl_util.h" +#include "igtl_types.h" + +#define IGTL_TDATA_ELEMENT_SIZE 74 +#define IGTL_STT_TDATA_SIZE 36 +#define IGTL_RTS_TDATA_SIZE 1 + +#define IGTL_TDATA_LEN_NAME 20 /* Maximum length of tracking instrument name */ +#define IGTL_STT_TDATA_LEN_COORDNAME 32 /* Maximum length of coordinate system name */ + +#define IGTL_TDATA_TYPE_TRACKER 1 /* Tracker */ +#define IGTL_TDATA_TYPE_6D 2 /* 6D instrument (regular instrument) */ +#define IGTL_TDATA_TYPE_3D 3 /* 3D instrument (only tip of the instrument defined) */ +#define IGTL_TDATA_TYPE_5D 4 /* 5D instrument (tip and handle are defined, + but not the normal vector) */ + + +#ifdef __cplusplus +extern "C" { +#endif + +#pragma pack(1) /* For 1-byte boundary in memroy */ + +/** Status data header for OpenIGTLinik protocol */ +typedef struct { + char name[IGTL_TDATA_LEN_NAME]; /* Name of instrument / tracker */ + igtl_uint8 type; /* Tracking data type (1-4) */ + igtl_uint8 reserved; /* Reserved byte */ + igtl_float32 transform[12]; /* same as TRANSFORM */ + igtl_float32 error; /* the tracking error */ +} igtl_tdata_element; + + +typedef struct { + igtl_int32 resolution; /* Minimum time between two frames. Use 0 for as fast as possible. */ + /* If e.g. 50 ms is specified, the maximum update rate will be 20 Hz. */ + char coord_name[IGTL_STT_TDATA_LEN_COORDNAME]; /* Name of the coordinate system */ +} igtl_stt_tdata; + +typedef struct { + igtl_int8 status; /* 0: Success 1: Error */ +} igtl_rts_tdata; + +#pragma pack() + +/** igtl_tdata_get_data_size(n) calculates the size of body based on the number + * of tdatas. The size of body is used in the message header.*/ +#define igtl_tdata_get_data_size(n) ((n) * IGTL_TDATA_ELEMENT_SIZE) + +/** igtl_tdata_get_data_n(size) calculates the number of tdatas in the body, based on + * the body size. This function may be used when a client program parses a TDATA message. */ +#define igtl_tdata_get_data_n(size) ((size) / IGTL_TDATA_ELEMENT_SIZE) + +/** Converts endianness of each element in an array of + * igtl_tdata_element from host byte order to network byte order, + * or vice versa.*/ +void igtl_export igtl_tdata_convert_byte_order(igtl_tdata_element* tdatalist, int nelem); +void igtl_export igtl_stt_tdata_convert_byte_order(igtl_stt_tdata* stt_tdata); +void igtl_export igtl_rts_tdata_convert_byte_order(igtl_rts_tdata* rts_tdata); + +/** Calculates CRC of TDATA, STT_TDATA and RTS_TDATA messages.*/ +igtl_uint64 igtl_export igtl_tdata_get_crc(igtl_tdata_element* tdatalist, int nelem); +igtl_uint64 igtl_export igtl_stt_tdata_get_crc(igtl_stt_tdata* stt_tdata); +igtl_uint64 igtl_export igtl_rts_tdata_get_crc(igtl_rts_tdata* rts_tdata); + +#ifdef __cplusplus +} +#endif + +#endif /* __IGTL_TDATA_H */ + + diff --git a/Source/igtlutil/igtl_fulltransform.c b/Source/igtlutil/igtl_fulltransform.c new file mode 100644 index 00000000..27426179 --- /dev/null +++ b/Source/igtlutil/igtl_fulltransform.c @@ -0,0 +1,45 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#include +#include "igtl_transform.h" +#include "igtl_util.h" +/*#include "crc32.h"*/ + +#include + +void igtl_export igtl_transform_convert_byte_order(igtl_float32* transform) +{ + int i; + igtl_uint32 tmp[12]; + + if (igtl_is_little_endian()) { + memcpy(tmp, transform, sizeof(igtl_uint32)*12); + for (i = 0; i < 12; i ++) { + tmp[i] = BYTE_SWAP_INT32(tmp[i]); + } + memcpy(transform, tmp, sizeof(igtl_uint32)*12); + } +} + + +igtl_uint64 igtl_export igtl_transform_get_crc(igtl_float32* transform) +{ + + igtl_uint64 crc = crc64(0, 0, 0); + + crc = crc64((unsigned char*)transform, sizeof(igtl_float32)*12, crc); + + return crc; +} diff --git a/Source/igtlutil/igtl_fulltransform.h b/Source/igtlutil/igtl_fulltransform.h new file mode 100644 index 00000000..182ff525 --- /dev/null +++ b/Source/igtlutil/igtl_fulltransform.h @@ -0,0 +1,43 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#ifndef __IGTL_TRANSFORM_H +#define __IGTL_TRANSFORM_H + +#include "igtl_win32header.h" +#include "igtl_util.h" + +#define IGTL_TRANSFORM_SIZE 48 + +#ifdef __cplusplus +extern "C" { +#endif + +/* +typedef igtl_float32[12] transform; +*/ + +/** Converts endianness of each member variable + * in igtl_image_header from host byte order to network byte order, + * or vice versa. */ +void igtl_export igtl_transform_convert_byte_order(igtl_float32* transform); + +/** Calculates CRC of transform data. */ +igtl_uint64 igtl_export igtl_transform_get_crc(igtl_float32* transform); + +#ifdef __cplusplus +} +#endif +#endif /*__IGTL_TRANSFORM_H*/ + From 7d67b9d4901e023bc2c82c449275648db2492680 Mon Sep 17 00:00:00 2001 From: Steve Thompson Date: Thu, 31 Oct 2013 17:12:33 +0000 Subject: [PATCH 04/12] #47: created tests for new message types, to enable backward compatability --- Testing/igtlutil/igtl_fulltdata_test.c | 150 ++++++++++++++++++++ Testing/igtlutil/igtl_test_data_fulltdata.h | 80 +++++++++++ 2 files changed, 230 insertions(+) create mode 100644 Testing/igtlutil/igtl_fulltdata_test.c create mode 100644 Testing/igtlutil/igtl_test_data_fulltdata.h diff --git a/Testing/igtlutil/igtl_fulltdata_test.c b/Testing/igtlutil/igtl_fulltdata_test.c new file mode 100644 index 00000000..92f081a4 --- /dev/null +++ b/Testing/igtlutil/igtl_fulltdata_test.c @@ -0,0 +1,150 @@ +/*========================================================================= + + Program: OpenIGTLink Library + Module: $RCSfile: $ + Language: C + Date: $Date: $ + Version: $Revision: $ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#include +#include "igtl_types.h" +#include "igtl_header.h" +#include "igtl_tdata.h" +#include "igtl_util.h" + +/* include test tdata data and serialized tdata message */ +#include "igtl_test_data_tdata.h" + +#define EXIT_SUCCESS 0 +#define EXIT_FAILURE 1 + +#define TEST_TDATA_NUM 3 + +#pragma pack(1) +struct tdata_message { + igtl_header header; + igtl_tdata_element tlist[TEST_TDATA_NUM]; +}; +#pragma pack() + + +int main( int argc, char * argv [] ) +{ + + struct tdata_message message; + int r; + int s; + + // Test structure size + if (sizeof(message) != IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM) + { + fprintf(stdout, "Invalid size of tdata message structure.\n"); + return EXIT_FAILURE; + } + + /* Tracking data 0 */ + strncpy((char*)&(message.tlist[0].name), "Tracker0", 20); + message.tlist[0].type = IGTL_TDATA_TYPE_6D; + message.tlist[0].reserved = 0; + message.tlist[0].transform[0] = -0.954892f; + message.tlist[0].transform[1] = 0.196632f; + message.tlist[0].transform[2] = -0.222525f; + message.tlist[0].transform[3] = -0.196632f; + message.tlist[0].transform[4] = 0.142857f; + message.tlist[0].transform[5] = 0.970014f; + message.tlist[0].transform[6] = 0.222525f; + message.tlist[0].transform[7] = 0.970014f; + message.tlist[0].transform[8] = -0.0977491f; + message.tlist[0].transform[9] = 46.0531f; + message.tlist[0].transform[10] = 19.4709f; + message.tlist[0].transform[11] = 46.0531f; + message.tlist[0].error = 46.0531f; + + /* Tracking data 1 */ + strncpy((char*)&(message.tlist[1].name), "Tracker1", 20); + message.tlist[1].type = IGTL_TDATA_TYPE_6D; + message.tlist[1].reserved = 0; + message.tlist[1].transform[0] = -0.954892f; + message.tlist[1].transform[1] = 0.196632f; + message.tlist[1].transform[2] = -0.222525f; + message.tlist[1].transform[3] = -0.196632f; + message.tlist[1].transform[4] = 0.142857f; + message.tlist[1].transform[5] = 0.970014f; + message.tlist[1].transform[6] = 0.222525f; + message.tlist[1].transform[7] = 0.970014f; + message.tlist[1].transform[8] = -0.0977491f; + message.tlist[1].transform[9] = 46.0531f; + message.tlist[1].transform[10] = 19.4709f; + message.tlist[1].transform[11] = 46.0531f; + message.tlist[1].error = 46.0531f; + + /* Tracking data 2 */ + strncpy((char*)&(message.tlist[2].name), "Tracker2", 20); + message.tlist[2].type = IGTL_TDATA_TYPE_6D; + message.tlist[2].reserved = 0; + message.tlist[2].transform[0] = -0.954892f; + message.tlist[2].transform[1] = 0.196632f; + message.tlist[2].transform[2] = -0.222525f; + message.tlist[2].transform[3] = -0.196632f; + message.tlist[2].transform[4] = 0.142857f; + message.tlist[2].transform[5] = 0.970014f; + message.tlist[2].transform[6] = 0.222525f; + message.tlist[2].transform[7] = 0.970014f; + message.tlist[2].transform[8] = -0.0977491f; + message.tlist[2].transform[9] = 46.0531f; + message.tlist[2].transform[10] = 19.4709f; + message.tlist[2].transform[11] = 46.0531f; + message.tlist[2].error = 46.0531f; + + /* Swap byte order if necessary */ + igtl_tdata_convert_byte_order(message.tlist, TEST_TDATA_NUM); + + /* Create OpenIGTLink header */ + message.header.version = 1; + strncpy( (char*)&(message.header.name), "TDATA", 12 ); + strncpy( (char*)&(message.header.device_name), "DeviceName", 20 ); + message.header.timestamp = 1234567890; + message.header.body_size = IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM; + message.header.crc = igtl_tdata_get_crc(message.tlist, TEST_TDATA_NUM); + igtl_header_convert_byte_order( &(message.header) ); + + /* Dumping data -- for debugging */ + /* + FILE *fp; + fp = fopen("tdata.bin", "w"); + fwrite(&(message), IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM, 1, fp); + fclose(fp); + */ + + + /* Compare the serialized byte array with the gold standard */ + r = memcmp((const void*)&message, (const void*)test_tdata_message, + (size_t)(IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM)); + + if (r == 0) + { + return EXIT_SUCCESS; + } + else + { + /* Print first 256 bytes as HEX values in STDERR for debug */ + s = IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM; + if (s > 256) + { + // s = 256; + } + + fprintf(stdout, "\n===== First %d bytes of the test message =====\n", s); + igtl_message_dump_hex(stdout, (const void*)&message, s); + + return EXIT_FAILURE; + } +} diff --git a/Testing/igtlutil/igtl_test_data_fulltdata.h b/Testing/igtlutil/igtl_test_data_fulltdata.h new file mode 100644 index 00000000..412382b2 --- /dev/null +++ b/Testing/igtlutil/igtl_test_data_fulltdata.h @@ -0,0 +1,80 @@ +/*========================================================================= + + Program: OpenIGTLLink Library -- Dummy tdata data + Module: $RCSfile: $ + Language: C + Date: $Date: $ + Version: $Revision: $ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#ifndef __IGTL_TEST_DATA_TDATA_H +#define __IGTL_TEST_DATA_TDATA_H + +char test_tdata_message[] = { + /*------- OpenIGTLink message header --------*/ + 0x00, 0x01, /* Version number */ + 0x54, 0x44, 0x41, 0x54, 0x41, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* TDATA */ + 0x44, 0x65, 0x76, 0x69, 0x63, 0x65, 0x4e, 0x61, + 0x6d, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Device name */ + 0x00, 0x00, 0x00, 0x00, 0x49, 0x96, 0x02, 0xd2, /* Time stamp */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xde, /* Body size */ + 0x83, 0x78, 0xb3, 0x9f, 0x73, 0x60, 0xd6, 0xf7, /* CRC */ + + /*---------- TDATA message body ------------*/ + /* Tracker data 0 */ + 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Name */ + 0x02, /* Instrument type */ + 0x00, + 0xBF, 0x74, 0x73, 0xCD, 0x3E, 0x49, 0x59, 0xE6, /* tx, ty */ + 0xBE, 0x63, 0xDD, 0x98, 0xBE, 0x49, 0x59, 0xE6, /* tz, sx */ + 0x3E, 0x12, 0x49, 0x1B, 0x3F, 0x78, 0x52, 0xD6, /* sy, sz */ + 0x3E, 0x63, 0xDD, 0x98, 0x3F, 0x78, 0x52, 0xD6, /* nx, ny */ + 0xBD, 0xC8, 0x30, 0xAE, 0x42, 0x38, 0x36, 0x60, /* nz, px */ + 0x41, 0x9B, 0xC4, 0x67, 0x42, 0x38, 0x36, 0x60, /* py, pz */ + 0x42, 0x38, 0x36, 0x60, /* error */ + + /* Tracker data 1 */ + 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x31, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Name */ + 0x02, /* Instrument type */ + 0x00, + 0xBF, 0x74, 0x73, 0xCD, 0x3E, 0x49, 0x59, 0xE6, /* tx, ty */ + 0xBE, 0x63, 0xDD, 0x98, 0xBE, 0x49, 0x59, 0xE6, /* tz, sx */ + 0x3E, 0x12, 0x49, 0x1B, 0x3F, 0x78, 0x52, 0xD6, /* sy, sz */ + 0x3E, 0x63, 0xDD, 0x98, 0x3F, 0x78, 0x52, 0xD6, /* nx, ny */ + 0xBD, 0xC8, 0x30, 0xAE, 0x42, 0x38, 0x36, 0x60, /* nz, px */ + 0x41, 0x9B, 0xC4, 0x67, 0x42, 0x38, 0x36, 0x60, /* py, pz */ + 0x42, 0x38, 0x36, 0x60, /* error */ + + /* Tracker data 1 */ + 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x32, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Name */ + 0x02, /* Instrument type */ + 0x00, + 0xBF, 0x74, 0x73, 0xCD, 0x3E, 0x49, 0x59, 0xE6, /* tx, ty */ + 0xBE, 0x63, 0xDD, 0x98, 0xBE, 0x49, 0x59, 0xE6, /* tz, sx */ + 0x3E, 0x12, 0x49, 0x1B, 0x3F, 0x78, 0x52, 0xD6, /* sy, sz */ + 0x3E, 0x63, 0xDD, 0x98, 0x3F, 0x78, 0x52, 0xD6, /* nx, ny */ + 0xBD, 0xC8, 0x30, 0xAE, 0x42, 0x38, 0x36, 0x60, /* nz, px */ + 0x41, 0x9B, 0xC4, 0x67, 0x42, 0x38, 0x36, 0x60, /* py, pz */ + 0x42, 0x38, 0x36, 0x60, /* error */ + +}; + +#endif /* IGTL_TEST_DATA_TDATA_H */ + + + From 5e05346f707f783c47d4bda8dd92d93abb049968 Mon Sep 17 00:00:00 2001 From: Steve Thompson Date: Thu, 31 Oct 2013 20:09:16 +0000 Subject: [PATCH 05/12] #47: separated tdata and full tdata --- Source/igtlutil/CMakeLists.txt | 2 ++ Source/igtlutil/igtl_fulltdata.c | 49 ++++---------------------------- Source/igtlutil/igtl_fulltdata.h | 48 +++++++------------------------ 3 files changed, 18 insertions(+), 81 deletions(-) diff --git a/Source/igtlutil/CMakeLists.txt b/Source/igtlutil/CMakeLists.txt index d90b36a4..e6649d2c 100644 --- a/Source/igtlutil/CMakeLists.txt +++ b/Source/igtlutil/CMakeLists.txt @@ -39,6 +39,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtl_lbmeta.h igtl_point.h igtl_tdata.h + igtl_fulltdata.h igtl_qtdata.h igtl_trajectory.h igtl_unit.h @@ -56,6 +57,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtl_lbmeta.c igtl_point.c igtl_tdata.c + igtl_fulltdata.c igtl_qtdata.c igtl_trajectory.c igtl_unit.c diff --git a/Source/igtlutil/igtl_fulltdata.c b/Source/igtlutil/igtl_fulltdata.c index 7fb6d4bf..6d0fcf2b 100644 --- a/Source/igtlutil/igtl_fulltdata.c +++ b/Source/igtlutil/igtl_fulltdata.c @@ -15,12 +15,12 @@ #include #include -#include "igtl_tdata.h" +#include "igtl_fulltdata.h" #include "igtl_util.h" -void igtl_export igtl_tdata_convert_byte_order(igtl_tdata_element* tdatalist, int nitem) +void igtl_export igtl_fulltdata_convert_byte_order(igtl_fulltdata_element* tdatalist, int nitem) { - igtl_tdata_element* elem; + igtl_fulltdata_element* elem; int i; int j; igtl_int32* tmp; @@ -43,27 +43,9 @@ void igtl_export igtl_tdata_convert_byte_order(igtl_tdata_element* tdatalist, in } -void igtl_export igtl_stt_tdata_convert_byte_order(igtl_stt_tdata* stt_tdata) +igtl_uint64 igtl_export igtl_fulltdata_get_crc(igtl_fulltdata_element* tdatalist, int nitem) { - igtl_int32* tmp; - - if (igtl_is_little_endian()) - { - tmp = (igtl_int32*)&(stt_tdata->resolution); - *tmp = BYTE_SWAP_INT32(*tmp); - } -} - - -void igtl_export igtl_rts_tdata_convert_byte_order(igtl_rts_tdata* rts_tdata) -{ - // do nothing -} - - -igtl_uint64 igtl_export igtl_tdata_get_crc(igtl_tdata_element* tdatalist, int nitem) -{ - igtl_tdata_element* elem; + igtl_fulltdata_element* elem; int i; igtl_uint64 crc; @@ -71,29 +53,10 @@ igtl_uint64 igtl_export igtl_tdata_get_crc(igtl_tdata_element* tdatalist, int ni for (i = 0; i < nitem; i ++) { elem = &(tdatalist[i]); - crc = crc64((unsigned char*) elem, IGTL_TDATA_ELEMENT_SIZE, crc); + crc = crc64((unsigned char*) elem, IGTL_FULLTDATA_ELEMENT_SIZE, crc); } return crc; } -igtl_uint64 igtl_export igtl_stt_tdata_get_crc(igtl_stt_tdata* stt_tdata) -{ - igtl_uint64 crc; - - crc = crc64(0, 0, 0); - crc = crc64((unsigned char*) stt_tdata, IGTL_STT_TDATA_SIZE, crc); - return crc; -} - - -igtl_uint64 igtl_export igtl_rts_tdata_get_crc(igtl_rts_tdata* rts_tdata) -{ - igtl_uint64 crc; - - crc = crc64(0, 0, 0); - crc = crc64((unsigned char*) rts_tdata, IGTL_RTS_TDATA_SIZE, crc); - return crc; -} - diff --git a/Source/igtlutil/igtl_fulltdata.h b/Source/igtlutil/igtl_fulltdata.h index 79845f99..8f9af8ab 100644 --- a/Source/igtlutil/igtl_fulltdata.h +++ b/Source/igtlutil/igtl_fulltdata.h @@ -12,26 +12,12 @@ =========================================================================*/ -#ifndef __IGTL_TDATA_H -#define __IGTL_TDATA_H +#ifndef __IGTL_FULLTDATA_H +#define __IGTL_FULLTDATA_H -#include "igtl_win32header.h" -#include "igtl_util.h" -#include "igtl_types.h" - -#define IGTL_TDATA_ELEMENT_SIZE 74 -#define IGTL_STT_TDATA_SIZE 36 -#define IGTL_RTS_TDATA_SIZE 1 - -#define IGTL_TDATA_LEN_NAME 20 /* Maximum length of tracking instrument name */ -#define IGTL_STT_TDATA_LEN_COORDNAME 32 /* Maximum length of coordinate system name */ - -#define IGTL_TDATA_TYPE_TRACKER 1 /* Tracker */ -#define IGTL_TDATA_TYPE_6D 2 /* 6D instrument (regular instrument) */ -#define IGTL_TDATA_TYPE_3D 3 /* 3D instrument (only tip of the instrument defined) */ -#define IGTL_TDATA_TYPE_5D 4 /* 5D instrument (tip and handle are defined, - but not the normal vector) */ +#include "igtl_tdata.h" +#define IGTL_FULLTDATA_ELEMENT_SIZE 74 #ifdef __cplusplus extern "C" { @@ -46,45 +32,31 @@ typedef struct { igtl_uint8 reserved; /* Reserved byte */ igtl_float32 transform[12]; /* same as TRANSFORM */ igtl_float32 error; /* the tracking error */ -} igtl_tdata_element; - +} igtl_fulltdata_element; -typedef struct { - igtl_int32 resolution; /* Minimum time between two frames. Use 0 for as fast as possible. */ - /* If e.g. 50 ms is specified, the maximum update rate will be 20 Hz. */ - char coord_name[IGTL_STT_TDATA_LEN_COORDNAME]; /* Name of the coordinate system */ -} igtl_stt_tdata; - -typedef struct { - igtl_int8 status; /* 0: Success 1: Error */ -} igtl_rts_tdata; #pragma pack() /** igtl_tdata_get_data_size(n) calculates the size of body based on the number * of tdatas. The size of body is used in the message header.*/ -#define igtl_tdata_get_data_size(n) ((n) * IGTL_TDATA_ELEMENT_SIZE) +#define igtl_fulltdata_get_data_size(n) ((n) * IGTL_FULLTDATA_ELEMENT_SIZE) /** igtl_tdata_get_data_n(size) calculates the number of tdatas in the body, based on * the body size. This function may be used when a client program parses a TDATA message. */ -#define igtl_tdata_get_data_n(size) ((size) / IGTL_TDATA_ELEMENT_SIZE) +#define igtl_fulltdata_get_data_n(size) ((size) / IGTL_FULLTDATA_ELEMENT_SIZE) /** Converts endianness of each element in an array of * igtl_tdata_element from host byte order to network byte order, * or vice versa.*/ -void igtl_export igtl_tdata_convert_byte_order(igtl_tdata_element* tdatalist, int nelem); -void igtl_export igtl_stt_tdata_convert_byte_order(igtl_stt_tdata* stt_tdata); -void igtl_export igtl_rts_tdata_convert_byte_order(igtl_rts_tdata* rts_tdata); +void igtl_export igtl_fulltdata_convert_byte_order(igtl_fulltdata_element* tdatalist, int nelem); /** Calculates CRC of TDATA, STT_TDATA and RTS_TDATA messages.*/ -igtl_uint64 igtl_export igtl_tdata_get_crc(igtl_tdata_element* tdatalist, int nelem); -igtl_uint64 igtl_export igtl_stt_tdata_get_crc(igtl_stt_tdata* stt_tdata); -igtl_uint64 igtl_export igtl_rts_tdata_get_crc(igtl_rts_tdata* rts_tdata); +igtl_uint64 igtl_export igtl_fulltdata_get_crc(igtl_fulltdata_element* tdatalist, int nelem); #ifdef __cplusplus } #endif -#endif /* __IGTL_TDATA_H */ +#endif /* __IGTL_FULLTDATA_H */ From b513e507f99a628ce29382604a828be020fed5e2 Mon Sep 17 00:00:00 2001 From: Steve Thompson Date: Thu, 31 Oct 2013 20:10:45 +0000 Subject: [PATCH 06/12] #47: added unit tests for full tdata --- Testing/igtlutil/CMakeLists.txt | 3 +++ Testing/igtlutil/igtl_fulltdata_test.c | 22 ++++++++++----------- Testing/igtlutil/igtl_test_data_fulltdata.h | 4 ++-- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/Testing/igtlutil/CMakeLists.txt b/Testing/igtlutil/CMakeLists.txt index 42de4157..38e31dd9 100644 --- a/Testing/igtlutil/CMakeLists.txt +++ b/Testing/igtlutil/CMakeLists.txt @@ -15,6 +15,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") ADD_EXECUTABLE(igtl_lbmeta_test igtl_lbmeta_test.c) ADD_EXECUTABLE(igtl_point_test igtl_point_test.c) ADD_EXECUTABLE(igtl_tdata_test igtl_tdata_test.c) + ADD_EXECUTABLE(igtl_fulltdata_test igtl_fulltdata_test.c) ADD_EXECUTABLE(igtl_trajectory_test igtl_trajectory_test.c) ADD_EXECUTABLE(igtl_sensor_test igtl_sensor_test.c) ADD_EXECUTABLE(igtl_string_test igtl_string_test.c) @@ -37,6 +38,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") TARGET_LINK_LIBRARIES(igtl_lbmeta_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_point_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_tdata_test OpenIGTLink) + TARGET_LINK_LIBRARIES(igtl_fulltdata_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_trajectory_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_sensor_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_string_test OpenIGTLink) @@ -59,6 +61,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") ADD_TEST( igtl_lbmeta_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_lbmeta_test ) ADD_TEST( igtl_point_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_point_test ) ADD_TEST( igtl_tdata_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_tdata_test ) + ADD_TEST( igtl_fulltdata_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_fulltdata_test ) ADD_TEST( igtl_trajectory_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_trajectory_test ) ADD_TEST( igtl_sensor_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_sensor_test ) ADD_TEST( igtl_string_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_string_test ) diff --git a/Testing/igtlutil/igtl_fulltdata_test.c b/Testing/igtlutil/igtl_fulltdata_test.c index 92f081a4..b069bc24 100644 --- a/Testing/igtlutil/igtl_fulltdata_test.c +++ b/Testing/igtlutil/igtl_fulltdata_test.c @@ -17,11 +17,11 @@ #include #include "igtl_types.h" #include "igtl_header.h" -#include "igtl_tdata.h" +#include "igtl_fulltdata.h" #include "igtl_util.h" /* include test tdata data and serialized tdata message */ -#include "igtl_test_data_tdata.h" +#include "igtl_test_data_fulltdata.h" #define EXIT_SUCCESS 0 #define EXIT_FAILURE 1 @@ -31,7 +31,7 @@ #pragma pack(1) struct tdata_message { igtl_header header; - igtl_tdata_element tlist[TEST_TDATA_NUM]; + igtl_fulltdata_element tlist[TEST_TDATA_NUM]; }; #pragma pack() @@ -44,9 +44,9 @@ int main( int argc, char * argv [] ) int s; // Test structure size - if (sizeof(message) != IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM) + if (sizeof(message) != IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM) { - fprintf(stdout, "Invalid size of tdata message structure.\n"); + fprintf(stdout, "Invalid size of full tdata message structure.\n"); return EXIT_FAILURE; } @@ -105,15 +105,15 @@ int main( int argc, char * argv [] ) message.tlist[2].error = 46.0531f; /* Swap byte order if necessary */ - igtl_tdata_convert_byte_order(message.tlist, TEST_TDATA_NUM); + igtl_fulltdata_convert_byte_order(message.tlist, TEST_TDATA_NUM); /* Create OpenIGTLink header */ message.header.version = 1; - strncpy( (char*)&(message.header.name), "TDATA", 12 ); + strncpy( (char*)&(message.header.name), "FULLTDATA", 12 ); strncpy( (char*)&(message.header.device_name), "DeviceName", 20 ); message.header.timestamp = 1234567890; - message.header.body_size = IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM; - message.header.crc = igtl_tdata_get_crc(message.tlist, TEST_TDATA_NUM); + message.header.body_size = IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM; + message.header.crc = igtl_fulltdata_get_crc(message.tlist, TEST_TDATA_NUM); igtl_header_convert_byte_order( &(message.header) ); /* Dumping data -- for debugging */ @@ -127,7 +127,7 @@ int main( int argc, char * argv [] ) /* Compare the serialized byte array with the gold standard */ r = memcmp((const void*)&message, (const void*)test_tdata_message, - (size_t)(IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM)); + (size_t)(IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM)); if (r == 0) { @@ -136,7 +136,7 @@ int main( int argc, char * argv [] ) else { /* Print first 256 bytes as HEX values in STDERR for debug */ - s = IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM; + s = IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM; if (s > 256) { // s = 256; diff --git a/Testing/igtlutil/igtl_test_data_fulltdata.h b/Testing/igtlutil/igtl_test_data_fulltdata.h index 412382b2..8bf80263 100644 --- a/Testing/igtlutil/igtl_test_data_fulltdata.h +++ b/Testing/igtlutil/igtl_test_data_fulltdata.h @@ -20,8 +20,8 @@ char test_tdata_message[] = { /*------- OpenIGTLink message header --------*/ 0x00, 0x01, /* Version number */ - 0x54, 0x44, 0x41, 0x54, 0x41, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, /* TDATA */ + 0x46, 0x55, 0x4c, 0x4c, 0x54, 0x44, 0x41, 0x54, + 0x41, 0x00, 0x00, 0x00, /* FULLTDATA */ 0x44, 0x65, 0x76, 0x69, 0x63, 0x65, 0x4e, 0x61, 0x6d, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* Device name */ From 4c9d7678836f274b4f55b888f6aa6bc498b5d85f Mon Sep 17 00:00:00 2001 From: Steve Thompson Date: Fri, 1 Nov 2013 12:01:57 +0000 Subject: [PATCH 07/12] #47: updated unit test for full data --- Testing/igtlutil/igtl_fulltdata_test.c | 69 +++++++++++++-------- Testing/igtlutil/igtl_test_data_fulltdata.h | 48 +++++++------- 2 files changed, 69 insertions(+), 48 deletions(-) diff --git a/Testing/igtlutil/igtl_fulltdata_test.c b/Testing/igtlutil/igtl_fulltdata_test.c index b069bc24..6cc6c6f3 100644 --- a/Testing/igtlutil/igtl_fulltdata_test.c +++ b/Testing/igtlutil/igtl_fulltdata_test.c @@ -57,15 +57,20 @@ int main( int argc, char * argv [] ) message.tlist[0].transform[0] = -0.954892f; message.tlist[0].transform[1] = 0.196632f; message.tlist[0].transform[2] = -0.222525f; - message.tlist[0].transform[3] = -0.196632f; - message.tlist[0].transform[4] = 0.142857f; - message.tlist[0].transform[5] = 0.970014f; - message.tlist[0].transform[6] = 0.222525f; - message.tlist[0].transform[7] = 0.970014f; - message.tlist[0].transform[8] = -0.0977491f; - message.tlist[0].transform[9] = 46.0531f; - message.tlist[0].transform[10] = 19.4709f; - message.tlist[0].transform[11] = 46.0531f; + message.tlist[0].transform[3] = 0.0f; + message.tlist[0].transform[4] = -0.196632f; + message.tlist[0].transform[5] = 0.142857f; + message.tlist[0].transform[6] = 0.970014f; + message.tlist[0].transform[7] = 0.0f; + message.tlist[0].transform[8] = 0.222525f; + message.tlist[0].transform[9] = 0.970014f; + message.tlist[0].transform[10] = -0.0977491f; + message.tlist[0].transform[11] = 0.0f; + message.tlist[0].transform[12] = 46.0531f; + message.tlist[0].transform[13] = 19.4709f; + message.tlist[0].transform[14] = 46.0531f; + message.tlist[0].transform[15] = 1.0f; + message.tlist[0].error = 46.0531f; /* Tracking data 1 */ @@ -75,15 +80,19 @@ int main( int argc, char * argv [] ) message.tlist[1].transform[0] = -0.954892f; message.tlist[1].transform[1] = 0.196632f; message.tlist[1].transform[2] = -0.222525f; - message.tlist[1].transform[3] = -0.196632f; - message.tlist[1].transform[4] = 0.142857f; - message.tlist[1].transform[5] = 0.970014f; - message.tlist[1].transform[6] = 0.222525f; - message.tlist[1].transform[7] = 0.970014f; - message.tlist[1].transform[8] = -0.0977491f; - message.tlist[1].transform[9] = 46.0531f; - message.tlist[1].transform[10] = 19.4709f; - message.tlist[1].transform[11] = 46.0531f; + message.tlist[1].transform[3] = 0.0f; + message.tlist[1].transform[4] = -0.196632f; + message.tlist[1].transform[5] = 0.142857f; + message.tlist[1].transform[6] = 0.970014f; + message.tlist[1].transform[7] = 0.0f; + message.tlist[1].transform[8] = 0.222525f; + message.tlist[1].transform[9] = 0.970014f; + message.tlist[1].transform[10] = -0.0977491f; + message.tlist[1].transform[11] = 0.0f; + message.tlist[1].transform[12] = 46.0531f; + message.tlist[1].transform[13] = 19.4709f; + message.tlist[1].transform[14] = 46.0531f; + message.tlist[1].transform[15] = 1.0f; message.tlist[1].error = 46.0531f; /* Tracking data 2 */ @@ -93,15 +102,19 @@ int main( int argc, char * argv [] ) message.tlist[2].transform[0] = -0.954892f; message.tlist[2].transform[1] = 0.196632f; message.tlist[2].transform[2] = -0.222525f; - message.tlist[2].transform[3] = -0.196632f; - message.tlist[2].transform[4] = 0.142857f; - message.tlist[2].transform[5] = 0.970014f; - message.tlist[2].transform[6] = 0.222525f; - message.tlist[2].transform[7] = 0.970014f; - message.tlist[2].transform[8] = -0.0977491f; - message.tlist[2].transform[9] = 46.0531f; - message.tlist[2].transform[10] = 19.4709f; - message.tlist[2].transform[11] = 46.0531f; + message.tlist[2].transform[3] = 0.0f; + message.tlist[2].transform[4] = -0.196632f; + message.tlist[2].transform[5] = 0.142857f; + message.tlist[2].transform[6] = 0.970014f; + message.tlist[2].transform[7] = 0.0f; + message.tlist[2].transform[8] = 0.222525f; + message.tlist[2].transform[9] = 0.970014f; + message.tlist[2].transform[10] = -0.0977491f; + message.tlist[2].transform[11] = 0.0f; + message.tlist[2].transform[12] = 46.0531f; + message.tlist[2].transform[13] = 19.4709f; + message.tlist[2].transform[14] = 46.0531f; + message.tlist[2].transform[15] = 1.0f; message.tlist[2].error = 46.0531f; /* Swap byte order if necessary */ @@ -145,6 +158,8 @@ int main( int argc, char * argv [] ) fprintf(stdout, "\n===== First %d bytes of the test message =====\n", s); igtl_message_dump_hex(stdout, (const void*)&message, s); + fprintf(stdout, "\n===== First %d bytes of the gs message =====\n", s); + igtl_message_dump_hex(stdout, (const void*)&test_tdata_message, s); return EXIT_FAILURE; } } diff --git a/Testing/igtlutil/igtl_test_data_fulltdata.h b/Testing/igtlutil/igtl_test_data_fulltdata.h index 8bf80263..dd298881 100644 --- a/Testing/igtlutil/igtl_test_data_fulltdata.h +++ b/Testing/igtlutil/igtl_test_data_fulltdata.h @@ -26,8 +26,8 @@ char test_tdata_message[] = { 0x6d, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* Device name */ 0x00, 0x00, 0x00, 0x00, 0x49, 0x96, 0x02, 0xd2, /* Time stamp */ - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xde, /* Body size */ - 0x83, 0x78, 0xb3, 0x9f, 0x73, 0x60, 0xd6, 0xf7, /* CRC */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0e, /* Body size */ + 0x87, 0x6d, 0x4f, 0xb9, 0x60, 0x77, 0x63, 0xc2, /* CRC */ /*---------- TDATA message body ------------*/ /* Tracker data 0 */ @@ -36,12 +36,14 @@ char test_tdata_message[] = { 0x00, 0x00, 0x00, 0x00, /* Name */ 0x02, /* Instrument type */ 0x00, - 0xBF, 0x74, 0x73, 0xCD, 0x3E, 0x49, 0x59, 0xE6, /* tx, ty */ - 0xBE, 0x63, 0xDD, 0x98, 0xBE, 0x49, 0x59, 0xE6, /* tz, sx */ - 0x3E, 0x12, 0x49, 0x1B, 0x3F, 0x78, 0x52, 0xD6, /* sy, sz */ - 0x3E, 0x63, 0xDD, 0x98, 0x3F, 0x78, 0x52, 0xD6, /* nx, ny */ - 0xBD, 0xC8, 0x30, 0xAE, 0x42, 0x38, 0x36, 0x60, /* nz, px */ - 0x41, 0x9B, 0xC4, 0x67, 0x42, 0x38, 0x36, 0x60, /* py, pz */ + 0xbf, 0x74, 0x73, 0xcd, 0x3e, 0x49, 0x59, 0xe6, /*4x4 Matrix*/ + 0xbe, 0x63, 0xdd, 0x98, 0x00, 0x00, 0x00, 0x00, + 0xbe, 0x49, 0x59, 0xe6, 0x3e, 0x12, 0x49, 0x1b, + 0x3f, 0x78, 0x52, 0xd6, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x63, 0xdd, 0x98, 0x3f, 0x78, 0x52, 0xd6, + 0xbd, 0xc8, 0x30, 0xae, 0x00, 0x00, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, 0x41, 0x9b, 0xc4, 0x67, + 0x42, 0x38, 0x36, 0x60, 0x3f, 0x80, 0x00, 0x00, 0x42, 0x38, 0x36, 0x60, /* error */ /* Tracker data 1 */ @@ -50,26 +52,30 @@ char test_tdata_message[] = { 0x00, 0x00, 0x00, 0x00, /* Name */ 0x02, /* Instrument type */ 0x00, - 0xBF, 0x74, 0x73, 0xCD, 0x3E, 0x49, 0x59, 0xE6, /* tx, ty */ - 0xBE, 0x63, 0xDD, 0x98, 0xBE, 0x49, 0x59, 0xE6, /* tz, sx */ - 0x3E, 0x12, 0x49, 0x1B, 0x3F, 0x78, 0x52, 0xD6, /* sy, sz */ - 0x3E, 0x63, 0xDD, 0x98, 0x3F, 0x78, 0x52, 0xD6, /* nx, ny */ - 0xBD, 0xC8, 0x30, 0xAE, 0x42, 0x38, 0x36, 0x60, /* nz, px */ - 0x41, 0x9B, 0xC4, 0x67, 0x42, 0x38, 0x36, 0x60, /* py, pz */ + 0xbf, 0x74, 0x73, 0xcd, 0x3e, 0x49, 0x59, 0xe6, /*4x4 Matrix*/ + 0xbe, 0x63, 0xdd, 0x98, 0x00, 0x00, 0x00, 0x00, + 0xbe, 0x49, 0x59, 0xe6, 0x3e, 0x12, 0x49, 0x1b, + 0x3f, 0x78, 0x52, 0xd6, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x63, 0xdd, 0x98, 0x3f, 0x78, 0x52, 0xd6, + 0xbd, 0xc8, 0x30, 0xae, 0x00, 0x00, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, 0x41, 0x9b, 0xc4, 0x67, + 0x42, 0x38, 0x36, 0x60, 0x3f, 0x80, 0x00, 0x00, 0x42, 0x38, 0x36, 0x60, /* error */ - /* Tracker data 1 */ + /* Tracker data 2 */ 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* Name */ 0x02, /* Instrument type */ 0x00, - 0xBF, 0x74, 0x73, 0xCD, 0x3E, 0x49, 0x59, 0xE6, /* tx, ty */ - 0xBE, 0x63, 0xDD, 0x98, 0xBE, 0x49, 0x59, 0xE6, /* tz, sx */ - 0x3E, 0x12, 0x49, 0x1B, 0x3F, 0x78, 0x52, 0xD6, /* sy, sz */ - 0x3E, 0x63, 0xDD, 0x98, 0x3F, 0x78, 0x52, 0xD6, /* nx, ny */ - 0xBD, 0xC8, 0x30, 0xAE, 0x42, 0x38, 0x36, 0x60, /* nz, px */ - 0x41, 0x9B, 0xC4, 0x67, 0x42, 0x38, 0x36, 0x60, /* py, pz */ + 0xbf, 0x74, 0x73, 0xcd, 0x3e, 0x49, 0x59, 0xe6, /*4x4 Matrix*/ + 0xbe, 0x63, 0xdd, 0x98, 0x00, 0x00, 0x00, 0x00, + 0xbe, 0x49, 0x59, 0xe6, 0x3e, 0x12, 0x49, 0x1b, + 0x3f, 0x78, 0x52, 0xd6, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x63, 0xdd, 0x98, 0x3f, 0x78, 0x52, 0xd6, + 0xbd, 0xc8, 0x30, 0xae, 0x00, 0x00, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, 0x41, 0x9b, 0xc4, 0x67, + 0x42, 0x38, 0x36, 0x60, 0x3f, 0x80, 0x00, 0x00, 0x42, 0x38, 0x36, 0x60, /* error */ }; From fedcfc60354df5f7f8c1b0b4b05c80f329b4009c Mon Sep 17 00:00:00 2001 From: Matt Clarkson Date: Fri, 11 Jul 2014 12:10:57 +0100 Subject: [PATCH 08/12] #47: Implemented unit test for igtl_fulltdata --- Source/CMakeLists.txt | 1 + Source/igtlutil/igtl_fulltdata.c | 2 +- Source/igtlutil/igtl_fulltdata.h | 11 +++++++---- Testing/igtlutil/igtl_fulltdata_test.c | 14 ++++++++------ Testing/igtlutil/igtl_test_data_fulltdata.h | 8 ++++---- 5 files changed, 21 insertions(+), 15 deletions(-) diff --git a/Source/CMakeLists.txt b/Source/CMakeLists.txt index 607366fd..3635cdba 100644 --- a/Source/CMakeLists.txt +++ b/Source/CMakeLists.txt @@ -110,6 +110,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtlutil/igtl_lbmeta.c igtlutil/igtl_point.c igtlutil/igtl_tdata.c + igtlutil/igtl_fulltdata.c igtlutil/igtl_qtdata.c igtlutil/igtl_trajectory.c igtlutil/igtl_unit.c diff --git a/Source/igtlutil/igtl_fulltdata.c b/Source/igtlutil/igtl_fulltdata.c index 6d0fcf2b..9a439693 100644 --- a/Source/igtlutil/igtl_fulltdata.c +++ b/Source/igtlutil/igtl_fulltdata.c @@ -30,7 +30,7 @@ void igtl_export igtl_fulltdata_convert_byte_order(igtl_fulltdata_element* tdata elem = &(tdatalist[i]); if (igtl_is_little_endian()) { - for (j = 0; j < 12; j ++) + for (j = 0; j < 16; j ++) { tmp = (igtl_int32*)&(elem->transform[j]); *tmp = BYTE_SWAP_INT32(*tmp); diff --git a/Source/igtlutil/igtl_fulltdata.h b/Source/igtlutil/igtl_fulltdata.h index 8f9af8ab..d7185a3e 100644 --- a/Source/igtlutil/igtl_fulltdata.h +++ b/Source/igtlutil/igtl_fulltdata.h @@ -15,9 +15,12 @@ #ifndef __IGTL_FULLTDATA_H #define __IGTL_FULLTDATA_H -#include "igtl_tdata.h" +#include "igtl_win32header.h" +#include "igtl_util.h" +#include "igtl_types.h" -#define IGTL_FULLTDATA_ELEMENT_SIZE 74 +#define IGTL_FULLTDATA_ELEMENT_SIZE 90 +#define IGTL_FULLTDATA_LEN_NAME 20 /* Maximum length of tracking instrument name */ #ifdef __cplusplus extern "C" { @@ -27,10 +30,10 @@ extern "C" { /** Status data header for OpenIGTLinik protocol */ typedef struct { - char name[IGTL_TDATA_LEN_NAME]; /* Name of instrument / tracker */ + char name[IGTL_FULLTDATA_LEN_NAME]; /* Name of instrument / tracker */ igtl_uint8 type; /* Tracking data type (1-4) */ igtl_uint8 reserved; /* Reserved byte */ - igtl_float32 transform[12]; /* same as TRANSFORM */ + igtl_float32 transform[16]; /* same as TRANSFORM */ igtl_float32 error; /* the tracking error */ } igtl_fulltdata_element; diff --git a/Testing/igtlutil/igtl_fulltdata_test.c b/Testing/igtlutil/igtl_fulltdata_test.c index 6cc6c6f3..2b493df0 100644 --- a/Testing/igtlutil/igtl_fulltdata_test.c +++ b/Testing/igtlutil/igtl_fulltdata_test.c @@ -14,13 +14,15 @@ =========================================================================*/ +#include #include #include "igtl_types.h" #include "igtl_header.h" +#include "igtl_tdata.h" #include "igtl_fulltdata.h" #include "igtl_util.h" -/* include test tdata data and serialized tdata message */ +/* include test fulltdata data and serialized fulltdata message */ #include "igtl_test_data_fulltdata.h" #define EXIT_SUCCESS 0 @@ -29,7 +31,7 @@ #define TEST_TDATA_NUM 3 #pragma pack(1) -struct tdata_message { +struct fulltdata_message { igtl_header header; igtl_fulltdata_element tlist[TEST_TDATA_NUM]; }; @@ -39,14 +41,14 @@ struct tdata_message { int main( int argc, char * argv [] ) { - struct tdata_message message; + struct fulltdata_message message; int r; int s; // Test structure size if (sizeof(message) != IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM) { - fprintf(stdout, "Invalid size of full tdata message structure.\n"); + fprintf(stdout, "Invalid size of fulltdata message structure.\n"); return EXIT_FAILURE; } @@ -139,7 +141,7 @@ int main( int argc, char * argv [] ) /* Compare the serialized byte array with the gold standard */ - r = memcmp((const void*)&message, (const void*)test_tdata_message, + r = memcmp((const void*)&message, (const void*)test_fulltdata_message, (size_t)(IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM)); if (r == 0) @@ -159,7 +161,7 @@ int main( int argc, char * argv [] ) igtl_message_dump_hex(stdout, (const void*)&message, s); fprintf(stdout, "\n===== First %d bytes of the gs message =====\n", s); - igtl_message_dump_hex(stdout, (const void*)&test_tdata_message, s); + igtl_message_dump_hex(stdout, (const void*)&test_fulltdata_message, s); return EXIT_FAILURE; } } diff --git a/Testing/igtlutil/igtl_test_data_fulltdata.h b/Testing/igtlutil/igtl_test_data_fulltdata.h index dd298881..1d558455 100644 --- a/Testing/igtlutil/igtl_test_data_fulltdata.h +++ b/Testing/igtlutil/igtl_test_data_fulltdata.h @@ -14,10 +14,10 @@ =========================================================================*/ -#ifndef __IGTL_TEST_DATA_TDATA_H -#define __IGTL_TEST_DATA_TDATA_H +#ifndef __IGTL_TEST_DATA_FULLTDATA_H +#define __IGTL_TEST_DATA_FULLTDATA_H -char test_tdata_message[] = { +char test_fulltdata_message[] = { /*------- OpenIGTLink message header --------*/ 0x00, 0x01, /* Version number */ 0x46, 0x55, 0x4c, 0x4c, 0x54, 0x44, 0x41, 0x54, @@ -80,7 +80,7 @@ char test_tdata_message[] = { }; -#endif /* IGTL_TEST_DATA_TDATA_H */ +#endif /* __IGTL_TEST_DATA_FULLTDATA_H */ From 3c6b9bc5eb896e9b6083b8cde244a3ef72e59745 Mon Sep 17 00:00:00 2001 From: Matt Clarkson Date: Fri, 11 Jul 2014 12:14:58 +0100 Subject: [PATCH 09/12] #47: Add fulltdata into CMake include config --- Source/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Source/CMakeLists.txt b/Source/CMakeLists.txt index 3635cdba..912107d0 100644 --- a/Source/CMakeLists.txt +++ b/Source/CMakeLists.txt @@ -143,6 +143,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtlutil/igtl_lbmeta.h igtlutil/igtl_point.h igtlutil/igtl_tdata.h + igtlutil/igtl_fulltdata.h igtlutil/igtl_qtdata.h igtlutil/igtl_trajectory.h igtlutil/igtl_unit.h From 85bd51a72ca58a09921e1886dbf4dc74ed8d06c7 Mon Sep 17 00:00:00 2001 From: Matt Clarkson Date: Fri, 11 Jul 2014 13:21:31 +0100 Subject: [PATCH 10/12] #47: Implemented igtlFullTrackingDataWithErrorMessage from previous placeholder --- .../igtlFullTrackingDataWithErrorMessage.cxx | 102 +++++++++++------- Source/igtlFullTrackingDataWithErrorMessage.h | 3 - 2 files changed, 66 insertions(+), 39 deletions(-) diff --git a/Source/igtlFullTrackingDataWithErrorMessage.cxx b/Source/igtlFullTrackingDataWithErrorMessage.cxx index 47c9e303..c28af3a7 100644 --- a/Source/igtlFullTrackingDataWithErrorMessage.cxx +++ b/Source/igtlFullTrackingDataWithErrorMessage.cxx @@ -15,7 +15,8 @@ #include "igtlFullTrackingDataWithErrorMessage.h" #include "igtl_header.h" -#include "igtl_tdata.h" +#include "igtl_fulltdata.h" +#include "igtlMacro.h" // Disable warning C4996 (strncpy() may be unsafe) in Windows. #define _CRT_SECURE_NO_WARNINGS @@ -29,7 +30,7 @@ namespace igtl { FullTrackingDataWithErrorMessage::FullTrackingDataWithErrorMessage() : TrackingDataMessage() { - this->m_DefaultBodyType = "FULLDATA"; + this->m_DefaultBodyType = "FULLTDATA"; } @@ -42,44 +43,51 @@ FullTrackingDataWithErrorMessage::~FullTrackingDataWithErrorMessage() //---------------------------------------------------------------------- int FullTrackingDataWithErrorMessage::GetBodyPackSize() { -/* // The body size sum of the header size and status message size. - return IGTL_TDATA_ELEMENT_SIZE * this->m_TrackingDataList.size(); -*/ - return 1; + return IGTL_FULLTDATA_ELEMENT_SIZE * this->m_TrackingDataList.size(); } //---------------------------------------------------------------------- int FullTrackingDataWithErrorMessage::PackBody() { -/* // allocate pack AllocatePack(); - igtl_tdata_element* element; - element = (igtl_tdata_element*)this->m_Body; + igtl_fulltdata_element* element; + element = (igtl_fulltdata_element*)this->m_Body; std::vector::iterator iter; for (iter = this->m_TrackingDataList.begin(); iter != this->m_TrackingDataList.end(); iter ++) + { + TrackingDataWithErrorElement* elem = dynamic_cast((*iter).GetPointer()); + if (elem == NULL) { - strncpy((char*)element->name, (*iter)->GetName(), IGTL_TDATA_LEN_NAME); + igtlExceptionMacro("Found list item that is not a TrackingDataWithErrorElement"); + } + + strncpy((char*)element->name, (*iter)->GetName(), IGTL_FULLTDATA_LEN_NAME); element->type = (*iter)->GetType(); Matrix4x4 matrix; (*iter)->GetMatrix(matrix); - for (int i = 0; i < 3; i ++) - { + for (int i = 0; i < 4; i ++) + { element->transform[i] = matrix[i][0]; - element->transform[i+3] = matrix[i][1]; - element->transform[i+6] = matrix[i][2]; - element->transform[i+9] = matrix[i][3]; - } - element ++; + element->transform[i+4] = matrix[i][1]; + element->transform[i+8] = matrix[i][2]; + element->transform[i+12] = matrix[i][3]; } + + float error; + error = elem->GetError(); + element->error = error; + + element ++; + } - igtl_tdata_convert_byte_order((igtl_tdata_element*)this->m_Body, this->m_TrackingDataList.size()); -*/ + igtl_fulltdata_convert_byte_order((igtl_fulltdata_element*)this->m_Body, this->m_TrackingDataList.size()); + return 1; } @@ -87,45 +95,67 @@ int FullTrackingDataWithErrorMessage::PackBody() //---------------------------------------------------------------------- int FullTrackingDataWithErrorMessage::UnpackBody() { -/* + this->m_TrackingDataList.clear(); - igtl_tdata_element* element = (igtl_tdata_element*) this->m_Body; - int nElement = igtl_tdata_get_data_n(this->m_BodySizeToRead); + igtl_fulltdata_element* element = (igtl_fulltdata_element*) this->m_Body; + int nElement = igtl_fulltdata_get_data_n(this->m_BodySizeToRead); - igtl_tdata_convert_byte_order(element, nElement); + igtl_fulltdata_convert_byte_order(element, nElement); char strbuf[128]; for (int i = 0; i < nElement; i ++) - { - TrackingDataElement::Pointer elemClass = TrackingDataElement::New(); + { + igtl::TrackingDataWithErrorElement::Pointer elemClass = TrackingDataWithErrorElement::New(); // Add '\n' at the end of each string // (neccesary for a case, where a string reaches the maximum length.) - strbuf[IGTL_TDATA_LEN_NAME] = '\n'; - strncpy(strbuf, (char*)element->name, IGTL_TDATA_LEN_NAME); + strbuf[IGTL_FULLTDATA_LEN_NAME] = '\n'; + strncpy(strbuf, (char*)element->name, IGTL_FULLTDATA_LEN_NAME); elemClass->SetName((const char*)strbuf); elemClass->SetType(element->type); Matrix4x4 matrix; IdentityMatrix(matrix); - for (int j = 0; j < 3; j ++) - { + for (int j = 0; j < 4; j ++) + { matrix[j][0] = element->transform[j]; - matrix[j][1] = element->transform[j+3]; - matrix[j][2] = element->transform[j+6]; - matrix[j][3] = element->transform[j+9]; - } + matrix[j][1] = element->transform[j+4]; + matrix[j][2] = element->transform[j+8]; + matrix[j][3] = element->transform[j+12]; + } elemClass->SetMatrix(matrix); - this->m_TrackingDataList.push_back(elemClass); + float error; + error = element->error; + elemClass->SetError(error); + + igtl::TrackingDataElement::Pointer pointerToBase = igtl::TrackingDataElement::New(); + pointerToBase = elemClass.GetPointer(); + + this->m_TrackingDataList.push_back(pointerToBase); element ++; - } -*/ + } + return 1; } +//---------------------------------------------------------------------- +void FullTrackingDataWithErrorMessage::GetTrackingDataElement(int index, TrackingDataWithErrorElement::Pointer& elem) +{ + if (index >= 0 && index < (int)this->m_TrackingDataList.size()) + { + TrackingDataWithErrorElement* element = dynamic_cast(this->m_TrackingDataList[index].GetPointer()); + if (element == NULL) + { + igtlExceptionMacro("Found list item that is not a TrackingDataWithErrorElement"); + } + igtl::TrackingDataWithErrorElement::Pointer tmp = element; + elem = tmp; + } +} + } // namespace igtl diff --git a/Source/igtlFullTrackingDataWithErrorMessage.h b/Source/igtlFullTrackingDataWithErrorMessage.h index 35c56236..b1697cd1 100644 --- a/Source/igtlFullTrackingDataWithErrorMessage.h +++ b/Source/igtlFullTrackingDataWithErrorMessage.h @@ -73,9 +73,6 @@ class IGTLCommon_EXPORT FullTrackingDataWithErrorMessage: public TrackingDataMes virtual int PackBody(); virtual int UnpackBody(); - /// The list of trakcing data elements. - std::vector m_TrackingDataList; - }; } // namespace igtl From 92576b996cc50c45b567833e2a2c1e5f0b0ad0da Mon Sep 17 00:00:00 2001 From: Matt Clarkson Date: Fri, 11 Jul 2014 13:51:26 +0100 Subject: [PATCH 11/12] #47: Implement Client/Server example for FullTrackingDataWithError --- Examples/CMakeLists.txt | 1 + .../FullTrackingDataWithError/CMakeLists.txt | 16 +++ .../FullTrackingDataWithErrorServer.cxx | 136 ++++++++++++++++++ Examples/Receiver/ReceiveClient.cxx | 52 +++++++ 4 files changed, 205 insertions(+) create mode 100644 Examples/FullTrackingDataWithError/CMakeLists.txt create mode 100644 Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt index 356342bb..7641172b 100644 --- a/Examples/CMakeLists.txt +++ b/Examples/CMakeLists.txt @@ -27,6 +27,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") Capability Trajectory SessionManager + FullTrackingDataWithError ) endif (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") diff --git a/Examples/FullTrackingDataWithError/CMakeLists.txt b/Examples/FullTrackingDataWithError/CMakeLists.txt new file mode 100644 index 00000000..1a66af8f --- /dev/null +++ b/Examples/FullTrackingDataWithError/CMakeLists.txt @@ -0,0 +1,16 @@ +PROJECT(FullTrackingDataWithError) + +cmake_minimum_required(VERSION 2.4) +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 NEW) +endif(COMMAND cmake_policy) + +find_package(OpenIGTLink REQUIRED) + +include(${OpenIGTLink_USE_FILE}) + +ADD_EXECUTABLE(FullTrackingDataWithErrorServer FullTrackingDataWithErrorServer.cxx) +TARGET_LINK_LIBRARIES(FullTrackingDataWithErrorServer OpenIGTLink) + + + diff --git a/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx b/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx new file mode 100644 index 00000000..4f0910fe --- /dev/null +++ b/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx @@ -0,0 +1,136 @@ +/*========================================================================= + + Program: Open IGT Link -- Example for Full Tracking Data With Errors + Module: $RCSfile: $ + Language: C++ + Date: $Date: $ + Version: $Revision: $ + + Copyright (c) Insight Software Consortium. All rights reserved. + + This software is distributed WITHOUT ANY WARRANTY; without even + the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + PURPOSE. See the above copyright notices for more information. + +=========================================================================*/ + +#include +#include +#include + +#include "igtlOSUtil.h" +#include "igtlTrackingDataMessage.h" +#include "igtlFullTrackingDataWithErrorMessage.h" +#include "igtlServerSocket.h" + +void GetRandomTestMatrix(igtl::Matrix4x4& matrix); + +int main(int argc, char* argv[]) +{ + //------------------------------------------------------------ + // Parse Arguments + + if (argc != 3) // check number of arguments + { + // If not correct, print usage + std::cerr << "Usage: " << argv[0] << " " << std::endl; + std::cerr << " : Port # (18944 in Slicer default)" << std::endl; + std::cerr << " : Frequency (fps) to send coordinate" << std::endl; + exit(0); + } + + int port = atoi(argv[1]); + double fps = atof(argv[2]); + int interval = (int) (1000.0 / fps); + + igtl::FullTrackingDataWithErrorMessage::Pointer msg; + msg = igtl::FullTrackingDataWithErrorMessage::New(); + + igtl::TrackingDataWithErrorElement::Pointer elem; + elem = igtl::TrackingDataWithErrorElement::New(); + elem->SetType(igtl::TrackingDataElement::TYPE_6D); + elem->SetName("Tracker"); + + igtl::TrackingDataElement::Pointer elemBase; + elemBase = dynamic_cast(elem.GetPointer()); + + igtl::ServerSocket::Pointer serverSocket; + serverSocket = igtl::ServerSocket::New(); + int r = serverSocket->CreateServer(port); + + if (r < 0) + { + std::cerr << "Cannot create a server socket." << std::endl; + exit(0); + } + + igtl::Socket::Pointer socket; + + while (1) + { + //------------------------------------------------------------ + // Waiting for Connection + socket = serverSocket->WaitForConnection(1000); + + if (socket.IsNotNull()) // if client connected + { + //------------------------------------------------------------ + // loop + for (int i = 0; i < 100; i ++) + { + igtl::Matrix4x4 matrix; + GetRandomTestMatrix(matrix); + + msg->ClearTrackingDataElements(); + + elem->SetMatrix(matrix); + elem->SetError(i); + + msg->AddTrackingDataElement(elemBase); + + msg->Pack(); + + socket->Send(msg->GetPackPointer(), msg->GetPackSize()); + igtl::Sleep(interval); // wait + } + } + } + + //------------------------------------------------------------ + // Close connection (The example code never reachs to this section ...) + + socket->CloseSocket(); + +} + + +void GetRandomTestMatrix(igtl::Matrix4x4& matrix) +{ + float position[3]; + float orientation[4]; + + // random position + static float phi = 0.0; + position[0] = 50.0 * cos(phi); + position[1] = 50.0 * sin(phi); + position[2] = 50.0 * cos(phi); + phi = phi + 0.2; + + // random orientation + static float theta = 0.0; + orientation[0]=0.0; + orientation[1]=0.6666666666*cos(theta); + orientation[2]=0.577350269189626; + orientation[3]=0.6666666666*sin(theta); + theta = theta + 0.1; + + //igtl::Matrix4x4 matrix; + igtl::QuaternionToMatrix(orientation, matrix); + + matrix[0][3] = position[0]; + matrix[1][3] = position[1]; + matrix[2][3] = position[2]; + + igtl::PrintMatrix(matrix); +} + diff --git a/Examples/Receiver/ReceiveClient.cxx b/Examples/Receiver/ReceiveClient.cxx index 56e58922..06a03555 100644 --- a/Examples/Receiver/ReceiveClient.cxx +++ b/Examples/Receiver/ReceiveClient.cxx @@ -33,6 +33,7 @@ #include "igtlTrajectoryMessage.h" #include "igtlStringMessage.h" #include "igtlTrackingDataMessage.h" +#include "igtlFullTrackingDataWithErrorMessage.h" #include "igtlQuaternionTrackingDataMessage.h" #include "igtlCapabilityMessage.h" #endif // OpenIGTLink_PROTOCOL_VERSION >= 2 @@ -47,6 +48,7 @@ int ReceiveStatus(igtl::Socket * socket, igtl::MessageHeader::Pointer& header); int ReceiveTrajectory(igtl::Socket * socket, igtl::MessageHeader::Pointer& header); int ReceiveString(igtl::Socket * socket, igtl::MessageHeader::Pointer& header); int ReceiveTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header); + int ReceiveFullTrackingDataWithError(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header); int ReceiveQuaternionTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header); int ReceiveCapability(igtl::Socket * socket, igtl::MessageHeader * header); #endif //OpenIGTLink_PROTOCOL_VERSION >= 2 @@ -161,6 +163,10 @@ int main(int argc, char* argv[]) { ReceiveTrackingData(socket, headerMsg); } + else if (strcmp(headerMsg->GetDeviceType(), "FULLTDATA") == 0) + { + ReceiveFullTrackingDataWithError(socket, headerMsg); + } else if (strcmp(headerMsg->GetDeviceType(), "QTDATA") == 0) { ReceiveQuaternionTrackingData(socket, headerMsg); @@ -497,6 +503,52 @@ int ReceiveTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader return 0; } + +int ReceiveFullTrackingDataWithError(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header) +{ + std::cerr << "Receiving FULLTDATA data type." << std::endl; + + // Create a message buffer to receive transform data + igtl::FullTrackingDataWithErrorMessage::Pointer trackingData; + trackingData = igtl::FullTrackingDataWithErrorMessage::New(); + trackingData->SetMessageHeader(header); + trackingData->AllocatePack(); + + // Receive body from the socket + socket->Receive(trackingData->GetPackBodyPointer(), trackingData->GetPackBodySize()); + + // Deserialize the transform data + // If you want to skip CRC check, call Unpack() without argument. + int c = trackingData->Unpack(1); + + if (c & igtl::MessageHeader::UNPACK_BODY) // if CRC check is OK + { + int nElements = trackingData->GetNumberOfTrackingDataElements(); + for (int i = 0; i < nElements; i ++) + { + igtl::TrackingDataWithErrorElement::Pointer trackingElement; + trackingData->GetTrackingDataElement(i, trackingElement); + + igtl::Matrix4x4 matrix; + trackingElement->GetMatrix(matrix); + + float errorMeasure; + errorMeasure = trackingElement->GetError(); + + std::cerr << "========== Element #" << i << " ==========" << std::endl; + std::cerr << " Name : " << trackingElement->GetName() << std::endl; + std::cerr << " Type : " << (int) trackingElement->GetType() << std::endl; + std::cerr << " Matrix : " << std::endl; + igtl::PrintMatrix(matrix); + std::cerr << " Error : " << trackingElement->GetError() << std::endl; + std::cerr << "================================" << std::endl << std::endl; + } + return 1; + } + return 0; +} + + int ReceiveQuaternionTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header) { std::cerr << "Receiving QTDATA data type." << std::endl; From 49edde4156185851d76437de46218c62c87a5a7a Mon Sep 17 00:00:00 2001 From: Matt Clarkson Date: Fri, 11 Jul 2014 14:02:57 +0100 Subject: [PATCH 12/12] #47: Testing sending full matrix --- .../FullTrackingDataWithErrorServer.cxx | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx b/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx index 4f0910fe..4a06934f 100644 --- a/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx +++ b/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx @@ -81,6 +81,13 @@ int main(int argc, char* argv[]) igtl::Matrix4x4 matrix; GetRandomTestMatrix(matrix); + // Geometrically a bit meaningless, + // but the whole point of this class is to transmit + // the full matrix. So we want to see these numbers at the client end. + matrix[3][0] = i; + matrix[3][1] = i; + matrix[3][2] = i; + msg->ClearTrackingDataElements(); elem->SetMatrix(matrix);