/
MOOSUDPLink.cpp
185 lines (133 loc) · 3.65 KB
/
MOOSUDPLink.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
/*
* MOOSUDPLink.cpp
* MOOS
*
* Created by pnewman on 16/05/2009.
* Copyright 2009 Oxford University. All rights reserved.
*
*/
#include "MOOSUDPLink.h"
#include "MOOS/libMOOS/Comms/XPCUdpSocket.h"
#include "MOOS/libMOOS/Utils/MOOSException.h"
#define MAX_UDP_PKT_SIZE 65536
bool _ListenCB(void * pParam)
{
CMOOSUDPLink* pMe = (CMOOSUDPLink* )pParam;
return pMe->ListenLoop();
}
CMOOSUDPLink::CMOOSUDPLink()
{
m_pUDPSocket = NULL;
m_nLocalPort = -1;
}
bool CMOOSUDPLink::Run(int nLocalPort)
{
m_nLocalPort = nLocalPort;
//input socket
m_pUDPSocket = new XPCUdpSocket(m_nLocalPort);
//its likely we want tp broadcast
m_pUDPSocket->vSetBroadcast(true);
if(nLocalPort!=-1)
{
try
{
m_pUDPSocket->vBindSocket();
}
catch (XPCException e)
{
MOOSTrace(e.sGetException());
return false;
}
m_ListenThread.Initialise(_ListenCB,this);
m_ListenThread.Start();
}
return true;
}
bool CMOOSUDPLink::Post(CMOOSMsg & M,const std::string & sHost, long int nPort)
{
if(m_pUDPSocket==NULL)
return MOOSFail("CMOOSUDPLink::Post No functioning UDP socket - has run been called\n");
MOOSMSG_LIST OutBox;
OutBox.push_back(M);
CMOOSCommPkt P;
P.Serialize(OutBox, true, false);
try
{
if(P.GetStreamLength()>MAX_UDP_PKT_SIZE)
{
//too big
throw XPCException("serialised packet is too big to send ....ask for an upgrade");
}
int nSent = m_pUDPSocket->iSendMessageTo(P.Stream(),P.GetStreamLength(),nPort,sHost);
if(nSent!=P.GetStreamLength())
{
//something abd happened..
throw XPCException("failed to send mailbox as datagram");
}
else
{
//MOOSTrace("sent %d bytes\n", nSent);
}
}
catch(XPCException e)
{
MOOSTrace("CMOOSUdpLink::PostLoop Exception caught %s\n",e.sGetException());
}
return true;
}
bool CMOOSUDPLink::Fetch(MOOSMSG_LIST & M)
{
m_InLock.Lock();
M.splice(M.begin(),m_InBox);
m_InLock.UnLock();
return true;
}
bool CMOOSUDPLink::ListenLoop()
{
//MOOSTrace("listening for incoming data packets on port %d\n",m_nLocalPort);
unsigned char Buff[MAX_UDP_PKT_SIZE];
while(!m_ListenThread.IsQuitRequested())
{
int nRead = m_pUDPSocket->iRecieveMessage((unsigned char*)Buff,sizeof(Buff));
CMOOSCommPkt P;
try
{
ReadPktFromArray(Buff, nRead, P);
m_InLock.Lock();
{
if(P.Serialize(m_InBox,false))
{
MOOSTrace("%d MOOSMsg's held\n",m_InBox.size());
}
else
{
MOOSTrace("failed to serialise CommPacket to Mailbox");
}
}
m_InLock.UnLock();
}
catch (CMOOSException e)
{
MOOSTrace("failed to serialise udp datagram to a CommPacket");
}
}
return true;
}
bool CMOOSUDPLink::ReadPktFromArray(unsigned char *pBuffer,int nBytes, CMOOSCommPkt& PktRx)
{
int nRqd=0;
int q = 0;
while((nRqd=PktRx.GetBytesRequired())!=0)
{
if(q+nRqd<=nBytes)
{
PktRx.Fill(pBuffer+q,nRqd);
q+=nRqd;
}
else
{
throw CMOOSException("UDP Packet was too small! Gross Error");
}
}
return true;
}