Skip to content

Commit 162ea9d

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

File tree

4 files changed

+55
-116
lines changed

4 files changed

+55
-116
lines changed

‎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: 35 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -26,131 +26,73 @@
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 != 3) // check number of arguments
4937
{
5038
// If not correct, print usage
5139
std::cerr << "Usage: " << argv[0] << " <hostname> <port> <fps>" << std::endl;
5240
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;
41+
std::cerr << " <port> : Port # (18944 or 18945 in Slicer default)" << std::endl;
5542
exit(0);
5643
}
5744

5845
char* hostname = argv[1];
5946
int port = atoi(argv[2]);
60-
double fps = atof(argv[3]);
61-
int interval = (int) (1000.0 / fps);
6247

6348
//------------------------------------------------------------
6449
// Establish Connection
6550

6651
igtl::UDPClientSocket::Pointer socket;
6752
socket = igtl::UDPClientSocket::New();
68-
socket->SetIPAddress("127.0.0.1");
69-
socket->SetPortNumber(port);
70-
socket->CreateUDPClient(port);
53+
int success = socket->JoinNetwork(hostname, port, 1);
54+
if (success<0)
55+
{
56+
std::cerr << "unable to join network, check if your local machine joined the host more than once. " << std::endl;
57+
exit(0);
58+
}
7159
unsigned char* bufferPKT = new unsigned char[RTP_PAYLOAD_LENGTH+RTP_HEADER_LENGTH];
7260
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();
61+
igtl::SimpleMutexLock* glock = igtl::SimpleMutexLock::New();
7662
int loop = 0;
7763
for (loop = 0; loop<100; loop++)
7864
{
7965
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())
66+
rtpWrapper->PushDataIntoPacketBuffer(bufferPKT, totMsgLen);
67+
rtpWrapper->UnWrapPacketWithTypeAndName("TDATA", "Tracker");
68+
glock->Lock();
69+
unsigned int messageNum = rtpWrapper->unWrappedMessages.size();
70+
glock->Unlock();
71+
if(messageNum)// to do: glock this session
8972
{
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
73+
igtl::TrackingDataMessage::Pointer trackingMultiPKTMSG = igtl::TrackingDataMessage::New();
74+
glock->Lock();
75+
std::map<igtl_uint32, igtl::UnWrappedMessage*>::iterator it = rtpWrapper->unWrappedMessages.begin();
76+
igtlUint8 * message = new igtlUint8[it->second->messageDataLength];
77+
int MSGLength = it->second->messageDataLength;
78+
memcpy(message, it->second->messagePackPointer, it->second->messageDataLength);
79+
delete it->second;
80+
it->second = NULL;
81+
rtpWrapper->unWrappedMessages.erase(it);
82+
glock->Unlock();
83+
igtl::MessageHeader::Pointer header = igtl::MessageHeader::New();
84+
header->InitPack();
85+
memcpy(header->GetPackPointer(), message, IGTL_HEADER_SIZE);
86+
header->Unpack();
87+
trackingMultiPKTMSG->SetMessageHeader(header);
88+
trackingMultiPKTMSG->AllocateBuffer();
89+
if (MSGLength == trackingMultiPKTMSG->GetPackSize())
11790
{
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;
91+
memcpy(trackingMultiPKTMSG->GetPackPointer(), message, MSGLength);
92+
ReceiveTrackingData(trackingMultiPKTMSG);
14993
}
15094
}
15195
}
152-
igtl::Sleep(interval);
153-
}
15496
}
15597

15698

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

165107
// Deserialize the transform data
166108
// If you want to skip CRC check, call Unpack() without argument.
167-
int c = trackingData->Unpack(1);
109+
int c = trackingData->Unpack(0);
168110

169111
if (c & igtl::MessageHeader::UNPACK_BODY) // if CRC check is OK
170112
{

‎Examples/TrackingDataUDPTransfer/TrackingDataServerUDPTransfer.cxx‎

Lines changed: 9 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,8 @@ int main(int argc, char* argv[])
5353
serverSocket = igtl::UDPServerSocket::New();
5454
int r = serverSocket->CreateUDPServer(port);
5555
serverSocket->AddClient("127.0.0.1", 18944, 1);
56+
serverSocket->AddClient("127.0.0.1", 18945, 1);
57+
5658
if (r < 0)
5759
{
5860
std::cerr << "Cannot create a server socket." << std::endl;
@@ -64,7 +66,7 @@ int main(int argc, char* argv[])
6466
igtl::MessageRTPWrapper::Pointer rtpWrapper = igtl::MessageRTPWrapper::New();
6567
//------------------------------------------------------------
6668
// loop
67-
for (int i = 0;i<100;i++)
69+
for (int i = 0;i<1000;i++)
6870
{
6971
WrapMessage(serverSocket, rtpWrapper);
7072
}
@@ -81,7 +83,7 @@ void WrapMessage(igtl::UDPServerSocket::Pointer serverSocket, igtl::MessageRTPWr
8183
//------------------------------------------------------------
8284
// Get user data
8385
igtl::MutexLock::Pointer glock = igtl::MutexLock::New();
84-
long interval = 5000;
86+
long interval = 500;
8587
std::cerr << "Interval = " << interval << " (ms)" << std::endl;
8688
//long interval = 1000;
8789
//long interval = (id + 1) * 100; // (ms)
@@ -153,22 +155,12 @@ int SendTrackingData(igtl::UDPServerSocket::Pointer& socket, igtl::TrackingDataM
153155

154156
trackingMsg->Pack();
155157
rtpWrapper->SetSSRC(1);
156-
int status = igtl::MessageRTPWrapper::PaketReady;
157-
igtl_uint8* messagePointer = (igtl_uint8*)trackingMsg->GetPackBodyPointer();
158+
int status = igtl::MessageRTPWrapper::PacketReady;
159+
igtl_uint8* messagePointer = (igtl_uint8*)trackingMsg->GetPackPointer();
158160
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-
161+
int messageLength = trackingMsg->GetPackSize();
162+
status = rtpWrapper->WrapMessageAndSend(socket, messagePointer, messageLength);
163+
172164
phi0 += 0.1;
173165
phi1 += 0.2;
174166
phi2 += 0.3;

‎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

0 commit comments

Comments
 (0)