1
//---------------------------------------------------------------------------
3
// Project: OpenWalnut ( http://www.openwalnut.org )
5
// Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
6
// For more information see http://www.openwalnut.org/copying
8
// This file is part of OpenWalnut.
10
// OpenWalnut is free software: you can redistribute it and/or modify
11
// it under the terms of the GNU Lesser General Public License as published by
12
// the Free Software Foundation, either version 3 of the License, or
13
// (at your option) any later version.
15
// OpenWalnut is distributed in the hope that it will be useful,
16
// but WITHOUT ANY WARRANTY; without even the implied warranty of
17
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18
// GNU Lesser General Public License for more details.
20
// You should have received a copy of the GNU Lesser General Public License
21
// along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
23
//---------------------------------------------------------------------------
29
#include "WIGTLinkRemote.h"
31
#include "core/dataHandler/WDataSet.h"
32
#include "core/dataHandler/WValueSet.h"
33
#include "core/dataHandler/WGridRegular3D.h"
34
#include "core/dataHandler/WDataSetScalar.h"
35
#include "core/common/WIOTools.h"
36
#include "core/common/WLogger.h"
38
#include "igtlOSUtil.h"
39
#include "igtlImageMessage.h"
40
#include "igtlImageMetaMessage.h"
41
#include "igtlServerSocket.h"
42
#include "igtlTransformMessage.h"
44
WIGTLinkRemote::WIGTLinkRemote()
45
: checkCRC( false ), port( 0 )
47
receiversCondition.reset( new WCondition );
48
statusCondition.reset( new WCondition );
51
WIGTLinkRemote::~WIGTLinkRemote()
53
receiversMutex.lock();
54
//receiveQueue.clear();
55
receiversCondition->notify();
56
receiversMutex.unlock();
57
if( socket.IsNotNull() )
59
socket->CloseSocket();
63
void WIGTLinkRemote::createSocketAndWaitForConnection( uint32_t port )
68
void WIGTLinkRemote::createSocketAndConnect( std::string hostname, uint32_t port )
70
std::cout << "setting up connection to: \"" << hostname << ":" << port << "\"" << std::endl;
72
socket = igtl::ClientSocket::New();
73
int r = socket->ConnectToServer( hostname.c_str(), port );
76
throw WException( "Cannot connect to server" );
78
std::cout << "connection established" << std::endl;
79
statusCondition->notify(); // link established
82
void WIGTLinkRemote::threadMain()
87
void WIGTLinkRemote::listenLoop()
89
igtl::ServerSocket::Pointer serverSocket;
90
serverSocket = igtl::ServerSocket::New();
91
serverSocket->CreateServer( port );
93
// wait for connection
94
while( !m_shutdownFlag() && socket.IsNull() )
96
socket = serverSocket->WaitForConnection( 2000 );
98
if( !m_shutdownFlag() && socket.IsNotNull() )
100
statusCondition->notify(); // passive connection established
104
void WIGTLinkRemote::readDataLoop()
106
igtl::MessageHeader::Pointer headerMsg;
107
headerMsg = igtl::MessageHeader::New();
109
headerMsg->InitPack();
116
WAssert( socket.IsNotNull(), "Something failed when setting up the socket" );
118
while( !m_shutdownFlag() )
122
std::cout << "Waiting for data." << std::endl;
123
int r = socket->Receive( headerMsg->GetPackPointer(), headerMsg->GetPackSize() );
127
throw WException( "Socket Failed" );
129
std::cout << "got something." << std::endl;
131
if( r != headerMsg->GetPackSize() )
133
throw WException( "Invalid data size" );
136
// deserialize the header
137
headerMsg->Unpack( checkCRC ? 1:0 );
140
if( strcmp( headerMsg->GetDeviceType(), "TRANSFORM" ) == 0 )
142
//debugLog() << "Received TRANSFORM";
143
receiveTransform( headerMsg );
145
else if( strcmp( headerMsg->GetDeviceType(), "IMAGE" ) == 0 )
147
std::cout << "got an image!!!" << std::endl;
148
WDataSetScalarSPtr ds = receiveImage( headerMsg );
149
receiversMutex.lock();
150
receiveQueue.push( ds );
151
receiversCondition->notify();
152
receiversMutex.unlock();
154
else if( strcmp( headerMsg->GetDeviceType(), "POSITION" ) == 0 )
156
//readPosition( headerMsg );
157
socket->Skip( headerMsg->GetBodySizeToRead(), 0 );
159
else if( strcmp( headerMsg->GetDeviceType(), "STATUS" ) == 0 )
161
//readStatus( headerMsg );
162
socket->Skip( headerMsg->GetBodySizeToRead(), 0 );
166
//debugLog() << "Unknown header received";
167
socket->Skip( headerMsg->GetBodySizeToRead(), 0 );
170
catch( const std::exception &e )
172
if( socket.IsNotNull() )
174
socket->CloseSocket();
178
// we are in client mode and the connection failed
179
// notify the main program and quit this thread
180
// we have to start a new connection if this happens
185
// just continue listening
186
socket = igtl::ClientSocket::Pointer();
193
void WIGTLinkRemote::receiveTransform( igtl::MessageHeader::Pointer headerMsg )
195
// TODO(mario): OpenWalnut needs a nice interface for transforms to display tracking devices etc
197
// message body handler for transform
198
igtl::TransformMessage::Pointer transMsg;
199
transMsg = igtl::TransformMessage::New();
200
transMsg->SetMessageHeader( headerMsg );
201
transMsg->AllocatePack();
203
socket->Receive( transMsg->GetPackBodyPointer(), transMsg->GetPackBodySize() );
205
// deserialize the message body
206
int c = transMsg->Unpack( checkCRC ? 1:0 ); // 1 indicates to perform CRC
207
if( c & igtl::MessageHeader::UNPACK_BODY )
209
// if CRC check is OK, read transform data
210
igtl::Matrix4x4 matrix;
211
transMsg->GetMatrix( matrix );
213
// TODO(mario): add better debugging
214
igtl::PrintMatrix( matrix );
218
WDataSetScalarSPtr WIGTLinkRemote::receiveImage( igtl::MessageHeader::Pointer headerMsg )
220
igtl::ImageMessage::Pointer imgMsg;
221
imgMsg = igtl::ImageMessage::New();
222
imgMsg->SetMessageHeader( headerMsg );
223
imgMsg->AllocatePack();
225
// receive data from socket
226
socket->Receive( imgMsg->GetPackBodyPointer(), imgMsg->GetPackBodySize() );
228
// deserialize the data
229
int c = imgMsg->Unpack( checkCRC ? 1:0 ); // 1 stands for do CRC
231
if( c & igtl::MessageHeader::UNPACK_BODY )
240
scalarType = imgMsg->GetScalarType();
241
imgMsg->GetDimensions( size );
242
imgMsg->GetSpacing( spacing );
243
imgMsg->GetSubVolume( svsize, svoffset );
244
imgMsg->GetMatrix( mat );
246
WMatrix<double> owmat( 4, 4 );
247
for( int i = 0; i < 4; ++i )
249
for( int j = 0; j < 4; ++j )
251
owmat( i, j ) = mat[ i ][ j ];
254
WGridRegular3D::SPtr grid( new WGridRegular3D( size[ 0 ], size[ 1 ], size[ 2 ], WGridTransformOrtho( owmat ) ) );
256
boost::shared_ptr< WValueSetBase > valueSet = createValueSet( imgMsg );
258
WDataSetScalarSPtr ds( new WDataSetScalar( valueSet, grid ) );
259
// if( length( imgMsg->GetDeviceName() > 0 ) )
261
// ds->setFilename( imgMsg->GetDeviceName() );
268
std::cerr << "decoding image failed" << std::endl;
270
return WDataSetScalarSPtr ();
273
void WIGTLinkRemote::injectMessage()
277
void WIGTLinkRemote::sendTransform( const std::string& name, const WMatrix<double> & matrix )
279
if( socket.IsNotNull() )
281
igtl::TransformMessage::Pointer transMsg;
282
transMsg = igtl::TransformMessage::New();
283
transMsg->SetDeviceName( name.c_str() );
284
igtl::Matrix4x4 igtlMatrix;
285
for( int i = 0; i < 4; ++i )
287
for( int j = 0; j < 4; ++j )
289
igtlMatrix[ i ][ j ] = matrix( i, j );
292
transMsg->SetMatrix( igtlMatrix );
296
socket->Send( transMsg->GetPackPointer(), transMsg->GetPackSize() );
302
int convertTypeOWtoIGTL( int type )
306
case igtl::ImageMessage::TYPE_INT8:
308
case igtl::ImageMessage::TYPE_UINT8:
310
case igtl::ImageMessage::TYPE_INT16:
312
case igtl::ImageMessage::TYPE_UINT16:
314
case igtl::ImageMessage::TYPE_INT32:
316
case igtl::ImageMessage::TYPE_UINT32:
319
// TODO(mario): throw exception?
325
void WIGTLinkRemote::sendImageMetaData( const std::vector < WDataSetScalarSPtr >& dataSets )
327
igtl::ImageMetaMessage::Pointer imgMetaMsg;
328
imgMetaMsg = igtl::ImageMetaMessage::New();
330
imgMetaMsg->SetDeviceName( "OpenWalnut" );
332
// create meta data for each data set
333
for( size_t i = 0; i < dataSets.size(); ++i )
335
igtl::ImageMetaElement::Pointer imgMeta;
336
imgMeta = igtl::ImageMetaElement::New();
337
imgMeta->SetName( dataSets[ i ]->getFilename().c_str() );
338
imgMeta->SetDeviceName( dataSets[ i ]->getFilename().c_str() );
339
imgMeta->SetModality( "UNKNOWN_MODALITY" );
340
imgMeta->SetPatientName( dataSets[ i ]->getFilename().c_str() );
341
imgMeta->SetPatientID( "PATIENT_ID_0" );
343
igtl::TimeStamp::Pointer ts0;
344
ts0 = igtl::TimeStamp::New();
345
ts0->SetTime( 123456.78 );
347
imgMeta->SetTimeStamp( ts0 );
348
boost::shared_ptr < WGridRegular3D > g3dr( boost::shared_dynamic_cast < WGridRegular3D > ( dataSets[ i ]->getGrid() ) );
349
imgMeta->SetSize( g3dr->getNbCoordsX(), g3dr->getNbCoordsY(), g3dr->getNbCoordsZ() );
350
imgMeta->SetScalarType( convertTypeOWtoIGTL( dataSets[ i ]->getValueSet()->getDataType() ) );
351
imgMetaMsg->AddImageMetaElement( imgMeta );
355
socket->Send( imgMetaMsg->GetPackPointer(), imgMetaMsg->GetPackSize() );
361
size_t getRawSizeT( boost::shared_ptr < WValueSetBase > valueSet )
363
typedef typename DataTypeRT<DT>::type type;
364
boost::shared_ptr < WValueSet < type > > v = boost::shared_dynamic_cast < WValueSet < type > >( valueSet );
365
WAssert( v, "Type cast failed" );
366
return valueSet->rawSize() * sizeof( type );
370
const void* getRawPtrT( boost::shared_ptr < WValueSetBase > valueSet )
372
typedef typename DataTypeRT<DT>::type type;
373
boost::shared_ptr < WValueSet < type > > v;
374
v = boost::shared_dynamic_cast < WValueSet < type > >( valueSet );
375
WAssert( v, "Type cast failed" );
376
const void* ptr = v->rawData();
377
WAssert( ptr != 0, "Trying to query raw data, got null pointer" );
381
size_t getRawSize( boost::shared_ptr < WValueSetBase > valueSet )
383
int type = valueSet->getDataType();
384
#define CASE( A ) case A: return getRawSizeT < A > ( valueSet );
387
CASE( W_DT_UNSIGNED_CHAR );
389
CASE( W_DT_SIGNED_SHORT ); // INT16
390
CASE( W_DT_SIGNED_INT ); // INT32
397
CASE( W_DT_FLOAT128 );
399
throw WException( "Not implemented for given data type" );
404
const void* getRawPtr( boost::shared_ptr < WValueSetBase > valueSet )
406
int type = valueSet->getDataType();
407
#define CASE( A ) case A: return getRawPtrT < A > ( valueSet );
410
CASE( W_DT_UNSIGNED_CHAR );
412
CASE( W_DT_SIGNED_SHORT ); // INT16
413
CASE( W_DT_SIGNED_INT ); // INT32
420
CASE( W_DT_FLOAT128 );
422
throw WException( "Not implemented for given data type" );
428
void WIGTLinkRemote::sendImageData( WDataSetScalarSPtr dataSetScalar )
430
boost::shared_ptr< WValueSetBase > valueSet = dataSetScalar->getValueSet();
431
//size_t rawSize = valueSet->rawSize();
434
switch( valueSet->getDataType() )
437
scalarType = igtl::ImageMessage::TYPE_FLOAT32;
440
scalarType = igtl::ImageMessage::TYPE_FLOAT64;
443
scalarType = igtl::ImageMessage::TYPE_UINT16;
446
scalarType = igtl::ImageMessage::TYPE_UINT32;
448
case W_DT_UNSIGNED_CHAR:
449
scalarType = igtl::ImageMessage::TYPE_UINT8;
452
scalarType = igtl::ImageMessage::TYPE_INT8;
456
throw WException( "Unsupported scalar type: not supported by igtl?" );
458
case W_DT_SIGNED_INT:
459
scalarType = igtl::ImageMessage::TYPE_INT32;
461
case W_DT_SIGNED_SHORT:
462
scalarType = igtl::ImageMessage::TYPE_INT16;
466
throw WException( "Unsupported scalar type" );
470
throw WException( "RGB not supported" );
473
case W_DT_COMPLEX128:
474
case W_DT_COMPLEX256:
475
throw WException( "Complex types are not supported, yet" );
478
throw WException( "W_DT_NONE should never occur as a type." );
481
throw WException( "W_DT_ALL should never occur as a type." );
485
if( socket.IsNotNull() )
488
boost::shared_ptr < WGridRegular3D > g3dr( boost::shared_dynamic_cast < WGridRegular3D > ( dataSetScalar->getGrid() ) );
489
size[ 0 ] = g3dr->getNbCoordsX();
490
size[ 1 ] = g3dr->getNbCoordsY();
491
size[ 2 ] = g3dr->getNbCoordsZ();
493
int svsize[ 3 ] = { size[ 0 ], size[ 1 ], size[ 2 ]};
494
int svoffset[] = { 0, 0, 0 };
496
igtl::ImageMessage::Pointer imgMsg = igtl::ImageMessage::New();
497
imgMsg->SetDimensions( size );
498
imgMsg->SetSpacing( g3dr->getOffsetX(), g3dr->getOffsetY(), g3dr->getOffsetZ() );
499
imgMsg->SetDeviceName( "OpenWalnut" );
500
imgMsg->SetSubVolume( svsize, svoffset );
501
imgMsg->SetScalarType( scalarType );
502
imgMsg->AllocateScalars();
504
size_t rawsize = Ugly::getRawSize( valueSet );
505
std::cout << "Transfering " << rawsize << " = " << imgMsg->GetImageSize() << " bytes of data." << std::endl;
506
memcpy( imgMsg->GetScalarPointer(), Ugly::getRawPtr( valueSet ), rawsize );
508
igtl::Matrix4x4 matrix;
509
igtl::IdentityMatrix( matrix );
510
imgMsg->SetMatrix( matrix ); // TODO(mario): get the matrix from the data set
513
socket->Send( imgMsg->GetPackPointer(), imgMsg->GetPackSize() );
517
boost::shared_ptr < WValueSetBase > WIGTLinkRemote::createValueSet( const igtl::ImageMessage::Pointer& imgMsg )
519
boost::shared_ptr<WValueSetBase> valueSet;
521
imgMsg->GetDimensions( size );
522
size_t sz = size[ 0 ] * size[ 1 ] * size[ 2 ];
523
#define CASE( igtltype, ctype, owtype )\
526
boost::shared_ptr < std::vector < ctype > > values( new std::vector < ctype >( sz ) );\
527
memcpy( ( void* )&( ( *values )[ 0 ] ), imgMsg->GetScalarPointer(), sizeof( ctype ) * sz );\
528
valueSet.reset( new WValueSet < ctype >( 0, 1, values, owtype ) );\
532
switch( imgMsg->GetScalarType() )
534
CASE( igtl::ImageMessage::TYPE_INT8, int8_t, W_DT_INT8 );
535
CASE( igtl::ImageMessage::TYPE_UINT8, uint8_t, W_DT_UINT8 );
536
CASE( igtl::ImageMessage::TYPE_INT16, int16_t, W_DT_INT16 );
537
CASE( igtl::ImageMessage::TYPE_UINT16, uint16_t, W_DT_UINT16 );
538
CASE( igtl::ImageMessage::TYPE_INT32, int32_t, W_DT_SIGNED_INT );
539
CASE( igtl::ImageMessage::TYPE_UINT32, uint32_t, W_DT_UINT32 );
542
// TODO(mario): throw exception?