Skip to content

Commit 8ac5ca2

Browse files
leochan2009leochan2009
committed
EHN: UDP socket group join issue, examples for udp is working.
1 parent aa1ea65 commit 8ac5ca2

File tree

8 files changed

+63
-128
lines changed

8 files changed

+63
-128
lines changed

‎Examples/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} GREATER 1)
2525
Capability
2626
Trajectory
2727
SessionManager
28-
#TrackingDataUDPTransfer
28+
TrackingDataUDPTransfer
2929
#SampleUDPProgam
3030
)
3131
endif (${OpenIGTLink_PROTOCOL_VERSION} GREATER 1)

‎Examples/SampleUDPProgam/ServerUDPTransfer.cxx‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,15 @@
1111

1212
#include <string.h>
1313

14-
#define BUFLEN 512
15-
#define NPACK 10000
16-
#define PORT 9930
17-
14+
#ifndef BUFLEN
15+
#define BUFLEN 512
16+
#endif
17+
#ifndef NPACK
18+
#define NPACK 10000
19+
#endif
20+
#ifndef PORT
21+
#define PORT 9930
22+
#endif
1823
void diep(char *s)
1924
{
2025
perror(s);
@@ -50,4 +55,4 @@ int main(void)
5055

5156
close(s);
5257
return 0;
53-
}
58+
}

‎Examples/TrackingDataUDPTransfer/TrackingDataClientUDPTransfer.cxx‎

Lines changed: 36 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -26,131 +26,71 @@
2626
#include "igtlMessageRTPWrapper.h"
2727
#include "igtlUDPClientSocket.h"
2828

29-
class ReorderBuffer
30-
{
31-
public:
32-
ReorderBuffer(){firstPaketPos=0;filledPaketNum=0;receivedLastFrag=false;receivedFirstFrag==false;};
33-
~ReorderBuffer(){};
34-
unsigned char buffer[RTP_PAYLOAD_LENGTH*64]; // we use 6 bits for fragment number.
35-
uint32_t firstPaketPos;
36-
uint32_t filledPaketNum;
37-
bool receivedLastFrag;
38-
bool receivedFirstFrag;
39-
};
40-
4129
int ReceiveTrackingData(igtl::TrackingDataMessage::Pointer& msgData);
4230

4331
int main(int argc, char* argv[])
4432
{
4533
//------------------------------------------------------------
4634
// Parse Arguments
4735

48-
if (argc != 4) // check number of arguments
36+
if (argc != 2) // check number of arguments
4937
{
5038
// If not correct, print usage
5139
std::cerr << "Usage: " << argv[0] << " <hostname> <port> <fps>" << std::endl;
52-
std::cerr << " <hostname> : IP or host name" << std::endl;
53-
std::cerr << " <port> : Port # (18944 in Slicer default)" << std::endl;
54-
std::cerr << " <fps> : Frequency (fps) to send coordinate" << std::endl;
40+
std::cerr << " <port> : Port # (18944 or 18945 in Slicer default)" << std::endl;
5541
exit(0);
5642
}
5743

58-
char* hostname = argv[1];
59-
int port = atoi(argv[2]);
60-
double fps = atof(argv[3]);
61-
int interval = (int) (1000.0 / fps);
44+
int port = atoi(argv[1]);
6245

6346
//------------------------------------------------------------
6447
// Establish Connection
6548

6649
igtl::UDPClientSocket::Pointer socket;
6750
socket = igtl::UDPClientSocket::New();
68-
socket->SetIPAddress("127.0.0.1");
69-
socket->SetPortNumber(port);
70-
socket->CreateUDPClient(port);
51+
int success = socket->JoinNetwork("127.0.0.1", port, 1);
52+
if (success<0)
53+
{
54+
std::cerr << "unable to join network, check if your local machine joined the host more than once. " << std::endl;
55+
exit(0);
56+
}
7157
unsigned char* bufferPKT = new unsigned char[RTP_PAYLOAD_LENGTH+RTP_HEADER_LENGTH];
7258
igtl::MessageRTPWrapper::Pointer rtpWrapper = igtl::MessageRTPWrapper::New();
73-
igtl::TrackingDataMessage::Pointer trackingMultiPKTMSG = igtl::TrackingDataMessage::New();
74-
//std::vector<ReorderBuffer> reorderBufferVec(10, ReorderBuffer();
75-
ReorderBuffer reorderBuffer = ReorderBuffer();
59+
igtl::SimpleMutexLock* glock = igtl::SimpleMutexLock::New();
7660
int loop = 0;
7761
for (loop = 0; loop<100; loop++)
7862
{
7963
int totMsgLen = socket->ReadSocket(bufferPKT, RTP_PAYLOAD_LENGTH+RTP_HEADER_LENGTH);
80-
if (totMsgLen>12)
81-
{
82-
// Set up the RTP header:
83-
igtl_uint32 rtpHdr, timeIncrement;
84-
rtpHdr = *((igtl_uint32*)bufferPKT);
85-
//bool rtpMarkerBit = (rtpHdr&0x00800000) != 0;
86-
timeIncrement = *(igtl_uint32*)(bufferPKT+4);
87-
igtl_uint32 SSRC = *(igtl_uint32*)(bufferPKT+8);
88-
if(igtl_is_little_endian())
64+
rtpWrapper->PushDataIntoPacketBuffer(bufferPKT, totMsgLen);
65+
rtpWrapper->UnWrapPacketWithTypeAndName("TDATA", "Tracker");
66+
glock->Lock();
67+
unsigned int messageNum = rtpWrapper->unWrappedMessages.size();
68+
glock->Unlock();
69+
if(messageNum)// to do: glock this session
8970
{
90-
rtpHdr = BYTE_SWAP_INT32(rtpHdr);
91-
timeIncrement = BYTE_SWAP_INT32(timeIncrement);
92-
SSRC = BYTE_SWAP_INT32(SSRC);
93-
}
94-
int curPackedMSGLocation = RTP_HEADER_LENGTH;
95-
while(curPackedMSGLocation<totMsgLen)
96-
{
97-
igtl_uint8 fragmentNumber = *(bufferPKT + curPackedMSGLocation);
98-
curPackedMSGLocation++;
99-
igtl::MessageHeader::Pointer header = igtl::MessageHeader::New();
100-
header->AllocatePack();
101-
memcpy(header->GetPackPointer(), bufferPKT + curPackedMSGLocation, IGTL_HEADER_SIZE);
102-
curPackedMSGLocation += IGTL_HEADER_SIZE;
103-
header->Unpack();
104-
if(fragmentNumber==0X00) // fragment doesn't exist
105-
{
106-
107-
if (strcmp(header->GetDeviceType(),"TDATA")==0)
108-
{
109-
igtl::TrackingDataMessage::Pointer trackingMSG = igtl::TrackingDataMessage::New();
110-
trackingMSG->SetMessageHeader(header);
111-
trackingMSG->AllocatePack();
112-
memcpy(trackingMSG->GetPackBodyPointer(), bufferPKT + curPackedMSGLocation, header->GetBodySizeToRead());
113-
}
114-
curPackedMSGLocation += header->GetBodySizeToRead();
115-
}
116-
else
71+
igtl::TrackingDataMessage::Pointer trackingMultiPKTMSG = igtl::TrackingDataMessage::New();
72+
glock->Lock();
73+
std::map<igtl_uint32, igtl::UnWrappedMessage*>::iterator it = rtpWrapper->unWrappedMessages.begin();
74+
igtlUint8 * message = new igtlUint8[it->second->messageDataLength];
75+
int MSGLength = it->second->messageDataLength;
76+
memcpy(message, it->second->messagePackPointer, it->second->messageDataLength);
77+
delete it->second;
78+
it->second = NULL;
79+
rtpWrapper->unWrappedMessages.erase(it);
80+
glock->Unlock();
81+
igtl::MessageHeader::Pointer header = igtl::MessageHeader::New();
82+
header->InitPack();
83+
memcpy(header->GetPackPointer(), message, IGTL_HEADER_SIZE);
84+
header->Unpack();
85+
trackingMultiPKTMSG->SetMessageHeader(header);
86+
trackingMultiPKTMSG->AllocateBuffer();
87+
if (MSGLength == trackingMultiPKTMSG->GetPackSize())
11788
{
118-
if (strcmp(header->GetDeviceType(),"TDATA")==0)
119-
{
120-
int bodyMsgLength = (RTP_PAYLOAD_LENGTH-IGTL_HEADER_SIZE-1);//this is the length of the body within a full fragment paket
121-
int totFragNumber = -1;
122-
if(fragmentNumber==0X80)// To do, fix the issue when later fragment arrives earlier than the beginning fragment
123-
{
124-
trackingMultiPKTMSG->SetMessageHeader(header);
125-
trackingMultiPKTMSG->AllocatePack();
126-
memcpy(reorderBuffer.buffer, bufferPKT + curPackedMSGLocation, totMsgLen-curPackedMSGLocation);
127-
reorderBuffer.firstPaketPos = totMsgLen-curPackedMSGLocation;
128-
}
129-
else if(fragmentNumber>0XE0)// this is the last fragment
130-
{
131-
totFragNumber = fragmentNumber - 0XE0 + 1;
132-
memcpy(reorderBuffer.buffer+reorderBuffer.firstPaketPos+(totFragNumber-2)*bodyMsgLength, bufferPKT + RTP_HEADER_LENGTH+IGTL_HEADER_SIZE+1, totMsgLen-(RTP_HEADER_LENGTH+IGTL_HEADER_SIZE+1));
133-
reorderBuffer.receivedLastFrag = true;
134-
}
135-
else
136-
{
137-
int curFragNumber = fragmentNumber - 0X80;
138-
memcpy(reorderBuffer.buffer+reorderBuffer.firstPaketPos+(curFragNumber-1)*bodyMsgLength, bufferPKT + RTP_HEADER_LENGTH+IGTL_HEADER_SIZE+1, totMsgLen-(RTP_HEADER_LENGTH+IGTL_HEADER_SIZE+1));
139-
}
140-
reorderBuffer.filledPaketNum++;
141-
if(reorderBuffer.receivedLastFrag == true && reorderBuffer.filledPaketNum == totFragNumber)
142-
{
143-
memcpy(trackingMultiPKTMSG->GetPackBodyPointer(), reorderBuffer.buffer, header->GetBodySizeToRead());
144-
ReceiveTrackingData(trackingMultiPKTMSG);
145-
reorderBuffer.filledPaketNum = 0;
146-
}
147-
}
148-
break;
89+
memcpy(trackingMultiPKTMSG->GetPackPointer(), message, MSGLength);
90+
ReceiveTrackingData(trackingMultiPKTMSG);
14991
}
15092
}
15193
}
152-
igtl::Sleep(interval);
153-
}
15494
}
15595

15696

@@ -164,7 +104,7 @@ int ReceiveTrackingData(igtl::TrackingDataMessage::Pointer& msgData)
164104

165105
// Deserialize the transform data
166106
// If you want to skip CRC check, call Unpack() without argument.
167-
int c = trackingData->Unpack(1);
107+
int c = trackingData->Unpack(0);
168108

169109
if (c & igtl::MessageHeader::UNPACK_BODY) // if CRC check is OK
170110
{

‎Examples/TrackingDataUDPTransfer/TrackingDataServerUDPTransfer.cxx‎

Lines changed: 12 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -40,19 +40,19 @@ int main(int argc, char* argv[])
4040
//------------------------------------------------------------
4141
// Parse Arguments
4242

43-
if (argc != 2) // check number of arguments
43+
if (argc != 1) // check number of arguments
4444
{
4545
// If not correct, print usage
46-
std::cerr << "Usage: " << argv[0] << " <port>" << std::endl;
47-
std::cerr << " <port> : Port # (18944 in Slicer default)" << std::endl;
46+
std::cerr << "Usage: No augments" << std::endl;
4847
exit(0);
4948
}
5049

51-
int port = atoi(argv[1]);
5250
igtl::UDPServerSocket::Pointer serverSocket;
5351
serverSocket = igtl::UDPServerSocket::New();
54-
int r = serverSocket->CreateUDPServer(port);
52+
int r = serverSocket->CreateUDPServer();
5553
serverSocket->AddClient("127.0.0.1", 18944, 1);
54+
serverSocket->AddClient("127.0.0.1", 18945, 1);
55+
5656
if (r < 0)
5757
{
5858
std::cerr << "Cannot create a server socket." << std::endl;
@@ -64,7 +64,7 @@ int main(int argc, char* argv[])
6464
igtl::MessageRTPWrapper::Pointer rtpWrapper = igtl::MessageRTPWrapper::New();
6565
//------------------------------------------------------------
6666
// loop
67-
for (int i = 0;i<100;i++)
67+
for (int i = 0;i<1000;i++)
6868
{
6969
WrapMessage(serverSocket, rtpWrapper);
7070
}
@@ -81,7 +81,7 @@ void WrapMessage(igtl::UDPServerSocket::Pointer serverSocket, igtl::MessageRTPWr
8181
//------------------------------------------------------------
8282
// Get user data
8383
igtl::MutexLock::Pointer glock = igtl::MutexLock::New();
84-
long interval = 5000;
84+
long interval = 500;
8585
std::cerr << "Interval = " << interval << " (ms)" << std::endl;
8686
//long interval = 1000;
8787
//long interval = (id + 1) * 100; // (ms)
@@ -153,22 +153,12 @@ int SendTrackingData(igtl::UDPServerSocket::Pointer& socket, igtl::TrackingDataM
153153

154154
trackingMsg->Pack();
155155
rtpWrapper->SetSSRC(1);
156-
int status = igtl::MessageRTPWrapper::PaketReady;
157-
igtl_uint8* messagePointer = (igtl_uint8*)trackingMsg->GetPackBodyPointer();
156+
int status = igtl::MessageRTPWrapper::PacketReady;
157+
igtl_uint8* messagePointer = (igtl_uint8*)trackingMsg->GetPackPointer();
158158
rtpWrapper->SetMSGHeader((igtl_uint8*)trackingMsg->GetPackPointer());
159-
int messageLength = trackingMsg->GetPackBodySize();
160-
do
161-
{
162-
status = rtpWrapper->WrapMessage(messagePointer, messageLength);
163-
if (status == igtl::MessageRTPWrapper::WaitingForFragment || status == igtl::MessageRTPWrapper::PaketReady)
164-
{
165-
socket->WriteSocket(rtpWrapper->GetPackPointer(), rtpWrapper->GetPackedMSGLocation());
166-
messagePointer += rtpWrapper->GetCurMSGLocation();
167-
messageLength = trackingMsg->GetPackBodySize() - rtpWrapper->GetCurMSGLocation();
168-
}
169-
}while(status!=igtl::MessageRTPWrapper::PaketReady);
170-
socket->WriteSocket(rtpWrapper->GetPackPointer(), RTP_PAYLOAD_LENGTH+RTP_HEADER_LENGTH);
171-
159+
int messageLength = trackingMsg->GetPackSize();
160+
status = rtpWrapper->WrapMessageAndSend(socket, messagePointer, messageLength);
161+
172162
phi0 += 0.1;
173163
phi1 += 0.2;
174164
phi2 += 0.3;

‎Source/VideoStreaming/igtlVideoStreamIGTLinkServer.cxx‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -99,7 +99,7 @@ int VideoStreamIGTLinkServer::StartUDPServer ()
9999
{
100100
this->serverUDPSocket->CloseSocket();
101101
}
102-
r = this->serverUDPSocket->CreateUDPServer(this->serverPortNumber);
102+
r = this->serverUDPSocket->CreateUDPServer();
103103
if (r < 0)
104104
{
105105
std::cerr << "Cannot create a server socket." << std::endl;

‎Source/igtlUDPClientSocket.cxx‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,8 @@ int UDPClientSocket::JoinNetwork(const char* groupIPAddr, int portNum, bool join
4545
{
4646
this->SetIPAddress(groupIPAddr);
4747
this->SetPortNumber(portNum);
48-
this->m_SocketDescriptor = this->CreateUDPClientSocket();
4948
this->SetJoinGroup(joinGroup);
49+
this->m_SocketDescriptor = this->CreateUDPClientSocket();
5050
return this->m_SocketDescriptor;
5151
}
5252

‎Source/igtlUDPServerSocket.cxx‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -120,7 +120,7 @@ int UDPServerSocket::WriteSocket(unsigned char* buffer, unsigned bufferSize)
120120
return numByteSend;
121121
}
122122

123-
int UDPServerSocket::CreateUDPServer(int port)
123+
int UDPServerSocket::CreateUDPServer()
124124
{
125125
if (this->m_SocketDescriptor != -1)
126126
{

‎Source/igtlUDPServerSocket.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -106,7 +106,7 @@ class IGTLCommon_EXPORT UDPServerSocket : public GeneralSocket
106106
// Description:
107107
// Creates a UDP server socket at a given port and binds to it.
108108
// Returns -1 on error. 0 on success.
109-
int CreateUDPServer(int port);
109+
int CreateUDPServer();
110110

111111
// Desciption:
112112
// Write the data to all clients

0 commit comments

Comments
 (0)