PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
BrainLabTrackerSim.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2 Program: Plus
3 Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
4 See License.txt for details.
5 =========================================================Plus=header=end*/
6 
20 #include "PlusConfigure.h"
21 
22 #include <iostream>
23 #include <math.h>
24 #include <cstdlib>
25 #include <cstring>
26 
27 #include "igsioCommon.h"
28 
29 #include "igtlImageMessage.h"
30 #include "igtlMessageFactory.h"
31 #include "igtlMessageHeader.h"
32 #include "igtlMultiThreader.h"
33 #include "igtlOSUtil.h"
34 #include "igtlServerSocket.h"
35 #include "igtlTrackingDataMessage.h"
36 
37 
38 typedef struct
39 {
40  int nloop;
41  igtl::MutexLock::Pointer glock;
42  igtl::Socket::Pointer socket;
43  int interval;
44  int stop;
45 } ThreadData;
46 
47 //----------------------------------------------------------------------------
48 // Function to generate random matrix.
49 void GetRandomTestMatrix(igtl::Matrix4x4& matrix, float phi, float theta)
50 {
51  float position[3];
52  position[0] = 50.0 * cos(phi);
53  position[1] = 50.0 * sin(phi);
54  position[2] = 50.0 * cos(phi);
55 
56  float orientation[4];
57  orientation[0]=0.0;
58  orientation[1]=0.6666666666*cos(theta);
59  orientation[2]=0.577350269189626;
60  orientation[3]=0.6666666666*sin(theta);
61 
62  //igtl::Matrix4x4 matrix;
63  igtl::QuaternionToMatrix(orientation, matrix);
64 
65  matrix[0][3] = position[0];
66  matrix[1][3] = position[1];
67  matrix[2][3] = position[2];
68 
69  //igtl::PrintMatrix(matrix);
70 }
71 
72 //----------------------------------------------------------------------------
73 void SendTrackingData(igtl::Socket::Pointer& socket, igtl::TrackingDataMessage::Pointer& trackingMsg)
74 {
75  static float phi0 = 0.0;
76  static float theta0 = 0.0;
77 
78  // Update the tracking data with new transformation matrices
79  for (int trackingDataElementIndex=0; trackingDataElementIndex<trackingMsg->GetNumberOfTrackingDataElements(); trackingDataElementIndex++)
80  {
81  igtl::TrackingDataElement::Pointer ptr=NULL;
82  trackingMsg->GetTrackingDataElement(trackingDataElementIndex, ptr);
83  igtl::Matrix4x4 matrix;
84  GetRandomTestMatrix(matrix, phi0, theta0);
85  ptr->SetMatrix(matrix);
86  }
87 
88  static igtlUint32 messageID = 0;
89  trackingMsg->SetMessageID(messageID++);
90  trackingMsg->Pack();
91  socket->Send(trackingMsg->GetBufferPointer(), trackingMsg->GetBufferSize());
92 
93  phi0 += 0.1;
94  theta0 += 0.2;
95 }
96 
97 //----------------------------------------------------------------------------
98 void* ThreadFunction(void* ptr)
99 {
100  // Get thread information
101  igtl::MultiThreader::ThreadInfo* info = static_cast<igtl::MultiThreader::ThreadInfo*>(ptr);
102  int id = info->ThreadID;
103  std::cerr << "Thread #" << id << ": start" << std::endl;
104 
105  // Get user data
106  ThreadData* td = static_cast<ThreadData*>(info->UserData);
107  igtl::MutexLock::Pointer glock = td->glock;
108  long interval = td->interval;
109  std::cerr << "Interval = " << interval << " (ms)" << std::endl;
110 
111  igtl::Socket::Pointer& socket = td->socket;
112 
113  // Allocate TrackingData message class
114  // Reuse them in the loop to avoid reallocation in each message transfer
115  igtl::TrackingDataMessage::Pointer trackingMsg;
116  trackingMsg = igtl::TrackingDataMessage::New();
117  trackingMsg->SetDeviceName("Tracker");
118  igtl::TrackingDataElement::Pointer trackElement0;
119  trackElement0 = igtl::TrackingDataElement::New();
120  trackElement0->SetName("ReferenceStar");
121  trackElement0->SetType(igtl::TrackingDataElement::TYPE_TRACKER);
122  igtl::TrackingDataElement::Pointer trackElement1;
123  trackElement1 = igtl::TrackingDataElement::New();
124  trackElement1->SetName("Ultrasound");
125  trackElement1->SetType(igtl::TrackingDataElement::TYPE_6D);
126  igtl::TrackingDataElement::Pointer trackElement2;
127  trackElement2 = igtl::TrackingDataElement::New();
128  trackElement2->SetName("Pointer");
129  trackElement2->SetType(igtl::TrackingDataElement::TYPE_5D);
130 
131  // Start the transmission by sending only 2 tool data
132  glock->Lock();
133  std::cout << "Sending tool data: ReferenceStar, Ultrasound..." << std::endl;
134  glock->Unlock();
135  trackingMsg->AddTrackingDataElement(trackElement0);
136  trackingMsg->AddTrackingDataElement(trackElement1);
137  for (int i=0; i<500; i++)
138  {
139  glock->Lock();
140  SendTrackingData(socket, trackingMsg);
141  glock->Unlock();
142  igtl::Sleep(interval);
143  }
144 
145  // Send all tool data from now on (until stop tracking message is received)
146  glock->Lock();
147  std::cout << "Sending tool data: ReferenceStar, Ultrasound, Pointer..." << std::endl;
148  glock->Unlock();
149  trackingMsg->AddTrackingDataElement(trackElement2);
150  while (!td->stop)
151  {
152  glock->Lock();
153  SendTrackingData(socket, trackingMsg);
154  glock->Unlock();
155  igtl::Sleep(interval);
156  }
157 
158  glock->Lock();
159  std::cerr << "Thread #" << id << ": end" << std::endl;
160  glock->Unlock();
161 
162  return NULL;
163 }
164 
165 //----------------------------------------------------------------------------
166 int main(int argc, char* argv[])
167 {
168  int port = 22222;
169 
170  if (argc==2)
171  {
172  port=atoi(argv[1]);
173  }
174 
175  if (argc > 2 || port==0)
176  {
177  // There is a problem with the command-line artguments, print usage
178  std::cerr << "Usage: " << argv[0] << " <port>" << std::endl;
179  std::cerr << " <port> : Server port number (default: 22222)" << std::endl;
180  exit(0);
181  }
182 
184  igtl::MessageFactory::Pointer igtlMessageFactory = igtl::MessageFactory::New();
185 
186  std::cout << "Started BrainLab tracker simulator server at port " << port << std::endl;
187 
188  igtl::ServerSocket::Pointer serverSocket;
189  serverSocket = igtl::ServerSocket::New();
190  int r = serverSocket->CreateServer(port);
191  if (r < 0)
192  {
193  std::cerr << "Cannot create a server socket." << std::endl;
194  exit(0);
195  }
196 
197  igtl::MultiThreader::Pointer threader = igtl::MultiThreader::New();
198  igtl::MutexLock::Pointer glock = igtl::MutexLock::New();
199  ThreadData td;
200 
201  std::cout << "Waiting for connections..." << std::endl;
202 
203  while (true)
204  {
205  // Waiting for Connection
206  int threadID = -1;
207  igtl::Socket::Pointer socket;
208  socket = serverSocket->WaitForConnection(1000);
209 
210  if (socket.IsNotNull()) // if client connected
211  {
212  std::cerr << "A client is connected." << std::endl;
213 
214  // Create a message buffer to receive header
215  igtl::MessageHeader::Pointer headerMsg;
216  headerMsg = igtl::MessageHeader::New();
217  while (true)
218  {
219  // Initialize receive buffer
220  headerMsg->InitPack();
221 
222  // Receive generic header from the socket
223  bool timeout(false);
224  int rs = socket->Receive(headerMsg->GetBufferPointer(), headerMsg->GetBufferSize(), timeout);
225  if (rs == 0)
226  {
227  if (threadID >= 0)
228  {
229  td.stop = 1;
230  threader->TerminateThread(threadID);
231  threadID = -1;
232  }
233  std::cerr << "Disconnecting the client." << std::endl;
234  td.socket = NULL; // VERY IMPORTANT. Completely remove the instance.
235  socket->CloseSocket();
236  break;
237  }
238  if (rs != headerMsg->GetBufferSize())
239  {
240  continue;
241  }
242 
243  // Deserialize the header
244  headerMsg->Unpack();
245 
246  igtl::MessageBase::Pointer bodyMsg = igtlMessageFactory->CreateReceiveMessage(headerMsg);
247  if( bodyMsg.IsNull() )
248  {
249  LOG_ERROR("Unable to receive message of type: " << headerMsg->GetMessageType());
250  continue;
251  }
252 
253  // Check data type and receive data body
254  if (dynamic_cast<igtl::StartTrackingDataMessage*>(bodyMsg.GetPointer()))
255  {
256  std::cerr << "Received a STT_TDATA message." << std::endl;
257 
258  igtl::StartTrackingDataMessage::Pointer startTracking = dynamic_cast<igtl::StartTrackingDataMessage*>(bodyMsg.GetPointer());
259 
260  bool timeout(false);
261  socket->Receive(startTracking->GetBufferBodyPointer(), startTracking->GetBufferBodySize(), timeout);
262  const int enableCrcCheck=1;
263  int unpackSuccess = startTracking->Unpack(enableCrcCheck);
264  if (unpackSuccess & igtl::MessageHeader::UNPACK_BODY)
265  {
266  td.interval = startTracking->GetResolution();
267  td.glock = glock;
268  td.socket = socket;
269  td.stop = 0;
270  threadID = threader->SpawnThread((igtl::ThreadFunctionType) &ThreadFunction, &td);
271  }
272  }
273  else if (dynamic_cast<igtl::StopTrackingDataMessage*>(bodyMsg.GetPointer()))
274  {
275  socket->Skip(headerMsg->GetBodySizeToRead(), 0);
276  std::cerr << "Received a STP_TDATA message." << std::endl;
277  if (threadID >= 0)
278  {
279  td.stop = 1;
280  threader->TerminateThread(threadID);
281  threadID = -1;
282  std::cerr << "Disconnecting the client." << std::endl;
283  td.socket = NULL; // VERY IMPORTANT. Completely remove the instance.
284  socket->CloseSocket();
285  }
286  break;
287  }
288  else
289  {
290  std::cerr << "Receiving : " << headerMsg->GetMessageType() << std::endl;
291  socket->Skip(headerMsg->GetBodySizeToRead(), 0);
292  }
293  }
294  }
295  }
296 
297  // Close connection (the example code actually never reaches to this section ...)
298  serverSocket->CloseSocket();
299 }
void SendTrackingData(igtl::Socket::Pointer &socket, igtl::TrackingDataMessage::Pointer &trackingMsg)
for i
int main(int argc, char *argv[])
void GetRandomTestMatrix(igtl::Matrix4x4 &matrix, float phi, float theta)
int port
Definition: phidget22.h:2454
void * ThreadFunction(void *ptr)
double * position
Definition: phidget22.h:3303