int SnmpRouted::setup(fd_set& set)
{
FD_ZERO(&set);
int maxfd = listener.getfd();
FD_SET(maxfd, &set);
for(int i = 0; i < MaxRequest; i++)
{
UdpSocket *dest = reqTable[i].dest;
if(dest)
{
FD_SET(dest->getfd(), &set);
if(i > maxfd) maxfd = i;
}
}
return maxfd+1;
}
int main()
{
UdpSocket sock;
char msg[100];
int msgLen;
string reply="Server msg";
struct sockaddr_in clientSockAddr;
socklen_t sockLen;
sock.initServer(5000);
cout<<"Initialized the UDP server"<<endl;
msgLen = sock.serverRecv(msg,sizeof(msg),&clientSockAddr,&sockLen);
msg[msgLen] = 0;
cout<<"Client reply:"<<endl;
cout<<msg<<endl;
sock.serverSend(reply.c_str(),reply.length(),&clientSockAddr,sizeof(clientSockAddr));
}
// Test1: server unreliable message receive -----------------------------------
void serverUnreliable( UdpSocket &sock, const int max, int message[] ) {
cerr << "server unreliable test:" << endl;
// receive message[] max times
for ( int i = 0; i < max; i++ ) {
sock.recvFrom( ( char * ) message, MSGSIZE ); // udp message receive
cerr << message[0] << endl; // print out message
}
}
static typename T::VALUE_TYPE Execute(const typename T::VALUE_TYPE& value)
{
UdpSocket socket;
socket.bind(0, 0);
TestUdpSenderRunnable<T> testSender(socket.getSocketAddrInfo().port, value);
Thread thread;
thread.start(testSender);
UdpSocketInputStream<1450> inStream(socket);
typename T::VALUE_TYPE resultValue;
inStream >> resultValue;
thread.join();
return resultValue;
}
int my_send()
{
printf("==== Send\n");
char * msg = "hello";
UdpSocket sender;
DataBuffer buffer(1024);
buffer.set_data_size(1024);
buffer.set_data((YETI_Byte *)msg, strlen(msg));
IpAddress address;
address.resolve_name("localhost");
SocketAddress socket_address(address, 9123);
YETI_Result result = sender.send(buffer, &socket_address);
if (YETI_FAILED(result)) {
fprintf(stderr, "send() failed (%d)\n", result);
return result;
}
return 0;
}
// Test 1: client unreliable message send -------------------------------------
void clientUnreliable( UdpSocket &sock, const int max, int message[] ) {
cerr << "client: unreliable test:" << endl;
// transfer message[] max times
for ( int i = 0; i < max; i++ ) {
message[0] = i; // message[0] has a sequence #
sock.sendTo( ( char * )message, MSGSIZE ); // udp message send
cerr << "message = " << message[0] << endl;
}
}
void serverEarlyRetrans(UdpSocket &sock, const int max, int message[],
int windowSize) {
cerr << "Server early retrans test:" << endl;
int ack; // Holds the acknowledgment
bool serverArray[max]; // Holds messages that server needs from client
int sequence = 0; // The sequence # counter
// Initialize all values in serverArray to false
for (int i = 0; i < max; i++)
serverArray[i] = false;
while (sequence < max) {
// Read the message from the client
sock.recvFrom((char *)message, MSGSIZE);
// Got needed message from client
if (message[0] == sequence) {
// Set the serverArray at sequence
serverArray[sequence] = true;
// Find the lowest number the server needs
while (sequence < max) {
if (serverArray[sequence] == false) {
break;
}
sequence++;
}
// Send an ACK for previous seq #
ack = sequence - 1;
}
else {
// Set the serverArray at message
serverArray[message[0]] = true;
// Send the number that is still needed
ack = sequence;
}
// Send the acknowledgment
sock.ackTo((char *)&ack, sizeof(ack));
}
}
void serverReliable( UdpSocket &sock, const int max, int message[] ) {
cerr << "server reliable test:" << endl;
// receive message[] max times
for ( int i = 0; i < max; ) {
sock.recvFrom( ( char * ) message, MSGSIZE ); // udp message receive
if ( message[0] >= max ) {
cerr << "ERROR: " << message[0] << " is larger than max\n";
break;
}
// Drops the message[] if its a duplicate
if ( message[0] == i ) {
sock.ackTo( ( char * ) message, MSGSIZE); // send ACK to client
//cerr << " ACKed: " << message[0] << endl;
i++;
}
}
}
/*
* obj UdpSocket类型的指针
*/
void *UdpSocket::_runRecving(void* obj) {
UdpSocket *udpSocket = (UdpSocket *)obj;
struct sockaddr_in addr;
socklen_t addrLen = sizeof(struct sockaddr_in);
int errorNum;
char data[MAX_BUF_SIZE];
while (udpSocket->m_running) {
bzero(&addr, sizeof(struct sockaddr_in));
if((errorNum = recvfrom(udpSocket->m_socket, data,
MAX_BUF_SIZE, 0,
(struct sockaddr*)&addr, &addrLen)) < 0) {
printf("error num %d\n", errorNum);
continue;
}
data[errorNum] = '\0';
udpSocket->recvHandler(&addr, data);
}
return((void*)0);
}
void UdpSendThread::Main() {
SocketThreadOperation *op = mSocketThreadOperation;
UdpSocket *s = (UdpSocket*)op->mSocket;
ErrorType error;
StopWatch sw;
sw.Start();
while (!ShouldEnd()) {
error = s->Send(op->mIPAddress, op->mByteStream, Math::ClampLong(
op->mTimeoutMS - sw.GetElapsedMilliseconds(), 0L, 16L));
if (0L == op->mTimeoutMS ||
(error && kErrorTimedOut != error) ||
(0L < op->mTimeoutMS &&
sw.GetElapsedMilliseconds() >= op->mTimeoutMS))
{
if (error)
Error::Throw(error, String("[%s(%p, %p)]",
FastFunctionName, this, op));
s->OnSend(op->mID, op->mIPAddress, error);
break;
}
}
}
int main(int argc, char* argv[])
{
/* Concept:
1) UdpSocket listens for input data.
2) Data is passed to H264Decoder which than parses
the NALU files and passes everything to ffmpeg.
The decoded data is than passed to the SdlViewer instance;
3) The SdlViewer uses SDL with OpenGL acceleration to show the received frames.
*/
for(int i = 1; i < argc; ++i) {
std::string value(argv[i]);
if (value == "--fullscreen") {
fullscreen = true;
}else if(value == "--oculus"){
viewer = new OculusViewer(DEFAULT_WIDTH, DEFAULT_HEIGHT);
}else if (value == "--help" || value == "-h") {
cout << "--help no idea what exactly this parameter does" << endl
<< "--fullscreen open fullscreen opengl context instead of vga window" << endl
<< "--oculus adjust screen window for Oculus Rift DK2" << endl;
return 0;
}
}
if(viewer == nullptr){
viewer = new SdlViewer(DEFAULT_WIDTH, DEFAULT_HEIGHT);
}
input.initClient(cfg.getValueOfKey<string>("TARGET_IP").c_str(), TARGET_PORT);
input.setInitCallback(&init);
input.send(PROTOCOL_TYPE_INIT, nullptr, 0);
viewer->setInputCallback(&inputCallback);
viewer->setPositionCallback(&positionCallback);
viewer->show(fullscreen);
input.send(PROTOCOL_TYPE_CLOSE, nullptr, 0);
input.close();
return 0;
}
void run()
{
provInit();
for (;;)
{
socket1.process();
provTmr();
usleep(10000);
provCounterAdd(0, 1);
provCounterSendState();
}
}
void SnmpRouted::routeRequest()
{
try
{
Request req;
req.dest = new UdpSocket(destAddr);
req.expTime = time(0)+RequestTimeout;
if(req.dest)
{
req.dest->connect();
int nbytes = listener.bytesToRead(sizeof(dataGram));
listener.read(dataGram, nbytes);
req.origAddr = listener.getSender();
req.dest->write(dataGram, nbytes);
int fd = req.dest->getfd();
reqTable[fd] = req;
}
}
catch(NetErr& e)
{
syslog(LOG_ERR, "Request not routed: %s", e.errm);
}
}
int clientStopWait( UdpSocket &sock, const int max, int message[] ) {
cerr << "Stop-and-wait test:" << endl;
Timer t;
int retransmits = 0;
for ( int i = 0; i < max; i++ ) { // transfer message[] max times
message[0] = i; // message[0] has a sequence #
sock.sendTo((char*) message, MSGSIZE); // send message
t.start(); // start timer
long lap;
while ((lap = t.lap()) < 1500) {
if(sock.pollRecvFrom() > 0) { // if ack received before timer ends
sock.recvFrom((char*)message, MSGSIZE);
break;
}
}
if (lap >= 1500) { // timeout, so retransmit
i--;
retransmits++;
cerr << retransmits << endl;
}
}
return retransmits;
}
void serverEarlyRetrans( UdpSocket &sock, const int max, int message[], int windowSize ) {
cerr << "server sliding window:" << endl;
int seq = 0;
int lastSeq = -1;
int windowLoc = 0;
vector<int> window(windowSize);
for(int i = 0; i < windowSize; i++) { //init window
window[i] = -1;
}
while(seq < max) {
while( sock.pollRecvFrom() <=0 ){} // pause until message
sock.recvFrom( ( char * ) message, MSGSIZE ); // udp message receive
seq = *message; // get the seq # from beginning of msg
int index = seq % windowSize;
if (index < windowLoc || index >= windowSize) { // seq is outside window
ackPkt(lastSeq, sock); // ack last contiguous packet.
continue; // drop packet
} else if(index == windowLoc) { // seq is next contiguous packet
window[index] = seq; // store seq in 1st slot
lastSeq = seq;
while(window[windowLoc] > -1) { // while contiguous
lastSeq = window[windowLoc]; // store value in lastSeq
window[windowLoc] = -1; // clean up value to -1
windowLoc++; // increment window
windowLoc = windowLoc % windowSize; // mod window if needed
if (window[windowLoc] == -1) { // if next slot is empty
ackPkt(lastSeq, sock); // ack last contiguous packet
}
}
} else { // seq is within window, so store
window[index] = seq; // store seq in correct slot
ackPkt(lastSeq, sock); // ack last contiguous packet
}
if (seq == max-1)
break;
}
}
int main(){
if(!winSockDll.Init()){
std::cout<<"Couldn't load winsock"<<std::endl;
system("pause");
return 1;
}
connections = (Address*)calloc(64, sizeof(Address));
addr = Address("127.0.0.1", 2222);
sConnect = UdpSocket();
sConnect.Bind(addr);
std::cout << "Server started..." << std::endl;
char* buffer = new char[512];
bool clientexist = false;
while(true){
clientexist = false;
ZeroMemory(buffer, 512);
sConnect.Receive(buffer, 512, &storeAddr);
for(int i = 0; i < Counter; i++){
if(storeAddr.Equals(connections[i])){
std::cout << "Client["<<i<<"]: "<<buffer<<std::endl;
clientexist = true;
}
}
if(!clientexist){
connections[Counter] = storeAddr;
std::cout<<"New Client: "<<Counter<<std::endl;
std::cout << "Client["<<Counter<<"]: "<< buffer << std::endl;
Counter++;
}
}
winSockDll.Cleanup();
return 0;
}
ExternalCommunicationsSender( UdpSocket& externalSocket,
IpEndpointName remoteServerEndpoint,
int localToRemotePort,
const char *userName, const char *userPassword,
const char *groupName, const char *groupPassword )
: lastAliveSentTime_( 0 )
, externalSocket_( externalSocket )
, remoteServerEndpoint_( remoteServerEndpoint )
, localToServerEndpoint_(
externalSocket.LocalEndpointFor( remoteServerEndpoint ).address,
localToRemotePort )
, userName_( userName )
, userPassword_( userPassword )
, groupName_( groupName )
, groupPassword_( groupPassword )
{
PrepareAliveBuffer();
PreparePingBuffer();
}
void init(int mode, int lWidth, int lHeigth, int rWidth, int rHeight){
if(decoder != nullptr) decoder->releaseObserver();
cout << "LW : " << lWidth << endl;
cout << "LH : " << lHeigth << endl;
cout << "RW : " << rWidth << endl;
cout << "RH : " << rHeight << endl;
if(mode == (int) MODE_VERTICALCONCAT){
cout << "MODE_VERTICALCONCAT" << endl;
}else if(mode == (int) MODE_LEFTRESIZED){
cout << "MODE_LEFTRESIZED" << endl;
}else if(mode == (int) MODE_RIGHTRESIZED){
cout << "MODE_RIGHTRESIZED" << endl;
}else if(mode == (int) MODE_LEFTBLURRED){
cout << "MODE_LEFTBLURRED" << endl;
}else if(mode == (int) MODE_RIGHTBLURRED){
cout << "MODE_RIGHTBLURRED" << endl;
}else if(mode == (int) MODE_SINGLE){
cout << "MODE_SINGLE" << endl;
}else if(mode == (int) MODE_INTERLEAVING){
cout << "MODE_INTERLEAVING" << endl;
}else{
return;
}
cout << "Hoop1" << endl;
cout << "Vw" << viewer << endl;
viewer->updateSize(lWidth, lHeigth, rWidth, rHeight);
cout << "Hoop2" << endl;
// TODO: DELETE OLD DECODER
decoder = new MultiH264Decoder(mode);
cout << "Hoop3" << endl;
decoder->setDecoderObserver(0, viewer);
cout << "Hoop4" << endl;
input.setInputObserver(0, decoder);
cout << "Hoop5" << endl;
}
int main()
{
UdpSocket s;
Host h("localhost", 2001);
if (s.sendString("hello world", h) == AbstractSocket::Done)
cout << "Done" << endl;
else
perror("Error");
if (s.sendInt32(666, h) == AbstractSocket::Done)
cout << "Done" << endl;
else
perror("Error");
if (s.sendInt8('&', h) == AbstractSocket::Done)
cout << "Done" << endl;
else
perror("Error");
size_t ts = 21;
if (s.sendCharArray("this is a char array", ts, h) == AbstractSocket::Done)
cout << "Done" << endl;
else
perror("Error");
Frame f;
f << "This is a frame " << 666 << '\n';
if (s.sendFrame(f, h) != AbstractSocket::Done)
perror("error");
else
cout << "done" << endl;
s.close();
}
otError Udp::RemoveSocket(UdpSocket &aSocket)
{
if (mSockets == &aSocket)
{
mSockets = mSockets->GetNext();
}
else
{
for (UdpSocket *socket = mSockets; socket; socket = socket->GetNext())
{
if (socket->GetNext() == &aSocket)
{
socket->SetNext(aSocket.GetNext());
break;
}
}
}
aSocket.SetNext(NULL);
return OT_ERROR_NONE;
}
value _UdpSocket_GetMaxMsgSize(value a) {
UdpSocket* s = (UdpSocket*) val_data(a);
return alloc_int(s->GetMaxMsgSize());
}
otError Udp::HandleMessage(Message &aMessage, MessageInfo &aMessageInfo)
{
otError error = OT_ERROR_NONE;
UdpHeader udpHeader;
uint16_t payloadLength;
uint16_t checksum;
payloadLength = aMessage.GetLength() - aMessage.GetOffset();
// check length
VerifyOrExit(payloadLength >= sizeof(UdpHeader), error = OT_ERROR_PARSE);
// verify checksum
checksum = Ip6::ComputePseudoheaderChecksum(aMessageInfo.GetPeerAddr(), aMessageInfo.GetSockAddr(),
payloadLength, kProtoUdp);
checksum = aMessage.UpdateChecksum(checksum, aMessage.GetOffset(), payloadLength);
#ifndef FUZZING_BUILD_MODE_UNSAFE_FOR_PRODUCTION
VerifyOrExit(checksum == 0xffff, error = OT_ERROR_DROP);
#endif
VerifyOrExit(aMessage.Read(aMessage.GetOffset(), sizeof(udpHeader), &udpHeader) == sizeof(udpHeader),
error = OT_ERROR_PARSE);
aMessage.MoveOffset(sizeof(udpHeader));
aMessageInfo.mPeerPort = udpHeader.GetSourcePort();
aMessageInfo.mSockPort = udpHeader.GetDestinationPort();
// find socket
for (UdpSocket *socket = mSockets; socket; socket = socket->GetNext())
{
if (socket->GetSockName().mPort != udpHeader.GetDestinationPort())
{
continue;
}
if (socket->GetSockName().mScopeId != 0 &&
socket->GetSockName().mScopeId != aMessageInfo.mInterfaceId)
{
continue;
}
if (!aMessageInfo.GetSockAddr().IsMulticast() &&
!socket->GetSockName().GetAddress().IsUnspecified() &&
socket->GetSockName().GetAddress() != aMessageInfo.GetSockAddr())
{
continue;
}
// verify source if connected socket
if (socket->GetPeerName().mPort != 0)
{
if (socket->GetPeerName().mPort != udpHeader.GetSourcePort())
{
continue;
}
if (!socket->GetPeerName().GetAddress().IsUnspecified() &&
socket->GetPeerName().GetAddress() != aMessageInfo.GetPeerAddr())
{
continue;
}
}
socket->HandleUdpReceive(aMessage, aMessageInfo);
}
exit:
return error;
}
void provSendPacket(const void* buffer, int len)
{
socket1.sendData("127.0.0.1", 9999, buffer, len);
}
void PTrackingBridge::exec() const
{
vector<Point32> averagedVelocities, positions, standardDeviations, velocities;
vector<int> heights, identities, widths;
UdpSocket receiverSocket;
InetAddress sender;
stringstream s;
string dataReceived, token;
float timeout;
int iterations, ret;
bool binding;
if (agentPort != -1)
{
binding = receiverSocket.bind(agentPort);
if (!binding)
{
ERR("Error during the binding operation, exiting..." << endl);
exit(-1);
}
}
else
{
ERR("Agent id not set, please check the launch file." << endl);
exit(-1);
}
INFO("PTracking bridge bound on port: " << agentPort << endl);
iterations = 0;
timeout = 0.05;
while (true)
{
ret = receiverSocket.recv(dataReceived,sender,timeout);
if (ret == -1)
{
ERR("Error in receiving message from: " << sender.toString());
continue;
}
averagedVelocities.clear();
heights.clear();
identities.clear();
positions.clear();
standardDeviations.clear();
velocities.clear();
widths.clear();
s.clear();
s.str("");
s << dataReceived;
/// Splitting message (each estimation is separated by the ';' character).
while (getline(s,token,';'))
{
Point32 averagedVelocity, position, standardDeviation, velocity;
stringstream tokenStream;
int height, identity, width;
tokenStream << token;
tokenStream >> identity >> position.x >> position.y >> standardDeviation.x >> standardDeviation.y >> width >> height >> velocity.x >> velocity.y >> averagedVelocity.x >> averagedVelocity.y;
identities.push_back(identity);
positions.push_back(position);
standardDeviations.push_back(standardDeviation);
widths.push_back(width);
heights.push_back(height);
velocities.push_back(velocity);
averagedVelocities.push_back(averagedVelocity);
}
TargetEstimations targetEstimations;
targetEstimations.header.seq = iterations++;
targetEstimations.header.stamp = ros::Time::now();
targetEstimations.identities = identities;
targetEstimations.positions = positions;
targetEstimations.standardDeviations = standardDeviations;
targetEstimations.widths = widths;
targetEstimations.heights = heights;
targetEstimations.velocities = velocities;
targetEstimations.averagedVelocities = averagedVelocities;
publisherTargetEstimations.publish(targetEstimations);
timeout = identities.size() > 0 ? 0.2 : 0.0;
}
}
ServiceManager::ServiceManager(uint32 _tcpAcptrSvcNum, uint32 _udpAcptrSvcNum, uint32 _sslAcptrSvcNum,
uint32 _tcpSvcNum, uint32 _udpSvcNum, uint32 _sslSvcNum,
uint32 _tcpAcptrThreadNum, uint32 _udpAcptrThreadNum, uint32 _sslAcptrThreadNum, uint32 _contextBase,
uint32 _perClrTcpSktNum, uint32 _tcpSktClrNum, uint32 _perClrUdpSubSktNum, uint32 _udpSubSktClrNum,
uint32 _perClrUdpSktNum, uint32 _udpSktClrNum, IFreeListShrPtr _packetFreeList,
IPacketParserShrPtr _pktParser, bool _isIp4)
: m_tcpAcptrSvcNum(_tcpAcptrSvcNum)
, m_udpAcptrSvcNum(_udpAcptrSvcNum)
, m_sslAcptrSvcNum(_sslAcptrSvcNum)
, m_tcpSvcNum(_tcpSvcNum)
, m_udpSvcNum(_udpSvcNum)
, m_sslSvcNum(_sslSvcNum)
, m_tcpAcptrThreadNum(_tcpAcptrThreadNum == 0 ? 1 : _tcpAcptrThreadNum)
, m_udpAcptrThreadNum(_udpAcptrThreadNum == 0 ? 1 : _udpAcptrThreadNum)
, m_sslAcptrThreadNum(_sslAcptrThreadNum == 0 ? 1 : _sslAcptrThreadNum)
, m_perClrTcpSktNum(_perClrTcpSktNum == 0 ? 1 : _perClrTcpSktNum)
, m_tcpSktClrNum(_tcpSktClrNum == 0 ? 1 : _tcpSktClrNum)
, m_perClrUdpSubSktNum(_perClrUdpSubSktNum == 0 ? 1 : _perClrUdpSubSktNum)
, m_udpSubSktClrNum(_udpSubSktClrNum == 0 ? 1 : _udpSubSktClrNum)
, m_perClrUdpSktNum(_perClrUdpSktNum == 0 ? 1 : _perClrUdpSktNum)
, m_udpSktClrNum(_udpSktClrNum == 0 ? 1 : _udpSktClrNum)
, m_usingTcp(m_tcpAcptrSvcNum > 0 && m_tcpSvcNum > 0)
, m_usingUdp(m_udpAcptrSvcNum > 0 && m_udpSvcNum > 0)
, m_usingSSL(m_sslAcptrSvcNum > 0 && m_sslSvcNum > 0)
, m_contexts(_contextBase)
, m_netInfoMgr((m_tcpSvcNum+m_udpSvcNum+_contextBase)/3+1)
, m_tcpAcptrSvcs(m_tcpAcptrSvcNum, NumChooser(0, (m_tcpAcptrSvcNum == 0 ? 1 : m_tcpAcptrSvcNum) - 1, 24249, false))
, m_sslAcptrSvcs(m_sslAcptrSvcNum, NumChooser(0, (m_sslAcptrSvcNum == 0 ? 1 : m_sslAcptrSvcNum) - 1, 21139, false))
, m_udpAcptrSvcs(m_udpAcptrSvcNum, NumChooser(0, (m_udpAcptrSvcNum == 0 ? 1 : m_udpAcptrSvcNum) - 1, 23449, false))
, m_tcpSvcs(m_tcpSvcNum, NumChooser(0, (m_tcpSvcNum == 0 ? 1 : m_tcpSvcNum) - 1, 14249, false))
, m_sslSvcs(m_sslSvcNum, NumChooser(0, (m_sslSvcNum == 0 ? 1 : m_sslSvcNum) - 1, 1222, false))
, m_udpSvcs(m_udpSvcNum, NumChooser(0, (m_udpSvcNum == 0 ? 1 : m_udpSvcNum) - 1, 54249, false))
, m_isIp4(_isIp4)
, m_currMaxSnIndex(0)
{
if (_pktParser)
{
m_pktParser = _pktParser;
}
else
{
m_pktParser = IPacketParserShrPtr(new PacketParser);
}
m_contexts.setHash();
for (uint32 i=0; i<m_tcpAcptrSvcs.size(); ++i)
{
m_tcpAcptrSvcs[i] = new TcpAcceptorService(this, m_tcpAcptrThreadNum, m_isIp4);
}
for (uint32 i=0; i<m_sslAcptrSvcs.size(); ++i)
{
m_sslAcptrSvcs[i] = new SSLAcceptorService(this, m_sslAcptrThreadNum, m_isIp4);
}
for (uint32 i=0; i<m_udpAcptrSvcs.size(); ++i)
{
m_udpAcptrSvcs[i] = new UdpAcceptorService(this, m_udpAcptrThreadNum, m_isIp4);
}
for (uint32 i=0; i<m_tcpSvcs.size(); ++i)
{
m_tcpSvcs[i] = new TcpService(this, m_isIp4);
}
for (uint32 i=0; i<m_sslSvcs.size(); ++i)
{
m_sslSvcs[i] = new SSLService(this, m_isIp4);
}
for (uint32 i=0; i<m_udpSvcs.size(); ++i)
{
m_udpSvcs[i] = new UdpService(this, m_isIp4);
}
// create and init tcp socket free list
IFreeListShrPtr tcpSktFl(new FreeList<TcpSocket>(m_tcpSktClrNum, m_perClrTcpSktNum));
uint32 maxTcpNum = m_tcpSktClrNum*m_perClrTcpSktNum;
for (uint32 ix=0, i=0; (i<maxTcpNum && m_usingTcp); ++i)
{
ix = (m_currMaxSnIndex+i) % m_tcpSvcs.size();
TcpSocket* tcpSkt = (TcpSocket*)tcpSktFl->get(i);
if (tcpSkt)
tcpSkt->init(ISessionShrPtr(new Session(m_currMaxSnIndex+i, &m_netInfoMgr, m_currMaxSnIndex+i,
m_pktParser->create())), m_tcpSvcs[ix], this);
}
sTcpSktFList.set(tcpSktFl);
if (m_usingTcp)
{
m_currMaxSnIndex += maxTcpNum;
}
// create and init udp sub socket free list
IFreeListShrPtr udpSubSktFl(new FreeList<UdpSubSocket>(m_udpSubSktClrNum, m_perClrUdpSubSktNum));
uint32 maxUdpSubNum = m_udpSubSktClrNum*m_perClrUdpSubSktNum;
for (uint32 ix=0, i=0; (i<maxUdpSubNum && m_usingUdp); ++i)
{
ix = (m_currMaxSnIndex+i) % m_udpSvcs.size();
UdpSubSocket* udpSubSkt = (UdpSubSocket*)udpSubSktFl->get(i);
if (udpSubSkt)
udpSubSkt->init(ISessionShrPtr(new Session(m_currMaxSnIndex+i, &m_netInfoMgr, m_currMaxSnIndex+i,
m_pktParser->create())), m_udpSvcs[ix], this, m_isIp4);
}
sUdpSubSktFList.set(udpSubSktFl);
if (m_usingUdp)
{
m_currMaxSnIndex += maxUdpSubNum;
}
// create and init udp socket free list
IFreeListShrPtr udpSktFl(new FreeList<UdpSocket>(m_udpSktClrNum, m_perClrUdpSktNum));
uint32 maxUdpNum = m_udpSktClrNum*m_perClrUdpSktNum;
for (uint32 ix=0, i=0; (i<maxUdpNum && m_usingUdp); ++ix, ++i)
{
if (ix >= m_udpSvcs.size())
ix = 0;
UdpSocket* udpSkt = (UdpSocket*)udpSktFl->get(i);
if (udpSkt)
udpSkt->init(m_udpSvcs[ix], 0, m_isIp4);
}
sUdpSktFList.set(udpSktFl);
if (_packetFreeList)
{
sPkFList.set(_packetFreeList);
}
else
{
sPkFList.set(IFreeListShrPtr(new FreeList<NetPacket>(10, 100)));
}
}
ServiceManager::~ServiceManager()
{
uint32 maxTcpNum = m_tcpSktClrNum*m_perClrTcpSktNum;
for (uint32 i=0; (i<maxTcpNum && m_usingTcp); ++i)
{
TcpSocket* tcpSkt = (TcpSocket*)sTcpSktFList.get()->get(i);
if (tcpSkt)
{
tcpSkt->close();
tcpSkt->clean();
}
}
HashMapShrPtr hm = sSSLSktFList.getFls();
if (hm)
{
uint32 size = hm->arraySize();
for (uint32 i=0; i<size; ++i)
{
hm->lockWrite((ContextId)i, (ContextId)i);
std::map<ContextId, IFreeListShrPtr>& hmMap = hm->map(i);
for (std::map<ContextId, IFreeListShrPtr>::iterator itr=hmMap.begin(); itr!=hmMap.end(); ++itr)
{
uint32 maxSSLSktNum = itr->second->getClr() * itr->second->getPer();
for (uint32 j=0; (j<maxSSLSktNum && m_usingSSL); ++j)
{
SSLSocket* sslSkt = (SSLSocket*)itr->second->get(j);
if (sslSkt)
{
sslSkt->socket()->lowest_layer().close();
sslSkt->clean();
}
}
}
hm->unlockWrite((ContextId)i, (ContextId)i);
}
}
hm = HashMapShrPtr();
uint32 maxUdpSubNum = m_udpSubSktClrNum*m_perClrUdpSubSktNum;
for (uint32 i=0; (i<maxUdpSubNum && m_usingUdp); ++i)
{
UdpSubSocket* udpSubSkt = (UdpSubSocket*)sUdpSubSktFList.get()->get(i);
if (udpSubSkt)
{
udpSubSkt->close();
udpSubSkt->clean();
}
}
uint32 maxUdpNum = m_udpSktClrNum*m_perClrUdpSktNum;
for (uint32 i=0; (i<maxUdpNum && m_usingUdp); ++i)
{
UdpSocket* udpSkt = (UdpSocket*)sUdpSktFList.get()->get(i);
if (udpSkt)
{
udpSkt->socket()->close();
udpSkt->clean();
}
}
m_contexts.clear();
for (uint32 i=0; i<m_tcpAcptrSvcs.size(); ++i)
{
if (m_tcpAcptrSvcs[i])
delete m_tcpAcptrSvcs[i];
}
for (uint32 i=0; i<m_sslAcptrSvcs.size(); ++i)
{
if (m_sslAcptrSvcs[i])
delete m_sslAcptrSvcs[i];
}
for (uint32 i=0; i<m_udpAcptrSvcs.size(); ++i)
{
if (m_udpAcptrSvcs[i])
delete m_udpAcptrSvcs[i];
}
for (uint32 i=0; i<m_tcpSvcs.size(); ++i)
{
if (m_tcpSvcs[i])
delete m_tcpSvcs[i];
}
for (uint32 i=0; i<m_sslSvcs.size(); ++i)
{
if (m_sslSvcs[i])
delete m_sslSvcs[i];
}
for (uint32 i=0; i<m_udpSvcs.size(); ++i)
{
if (m_udpSvcs[i])
delete m_udpSvcs[i];
}
}
void PTracker::exec(const ObjectSensorReading& visualReading)
{
static UdpSocket senderSocket;
vector<ObjectSensorReading> observations;
string dataToSend;
int ret;
#ifdef DEBUG_MODE
INFO("[PTracker (" << agentId << ")] - ***********************************" << endl);
INFO("[PTracker (" << agentId << ")] - \tNEW ITERATION (" << ++counterResult << ")" << endl);
INFO("[PTracker (" << agentId << ")] - ***********************************" << endl);
#else
INFO(".");
#endif
currentTimestamp.setToNow();
updateTargetVector(visualReading);
agentPose.x = 0.0;
agentPose.y = 0.0;
agentPose.theta = 0.0;
objectSensorReading.setObservationsAgentPose(agentPose);
objectSensorReading.setObservations(targetVector,currentTargetIndex,lastCurrentTargetIndex,LAST_N_TARGET_PERCEPTIONS,worldX,worldY);
observations.push_back(objectSensorReading);
processor.processReading(agentPose,initialTimestamp,currentTimestamp,observations);
const EstimationsSingleAgent& estimationsWithModel = objectParticleFilter.getEstimationsWithModel();
updateTargetPosition(estimationsWithModel);
bestParticles = updateBestParticles(estimationsWithModel);
if (estimatedTargetModels.size() > 0)
{
dataToSend = "Agent ";
AgentPacket agentPacket;
agentPacket.dataPacket.ip = agentAddress;
agentPacket.dataPacket.port = agentPort;
agentPacket.dataPacket.agentPose = agentPose;
agentPacket.dataPacket.estimatedTargetModels = estimatedTargetModels;
agentPacket.dataPacket.particlesTimestamp = currentTimestamp.getMsFromMidnight();
dataToSend += agentPacket.toString();
if ((Timestamp() - lastTimeInformationSent).getMs() > (1000.0 / messageFrequency))
{
sendEstimationsToAgents(dataToSend);
lastTimeInformationSent.setToNow();
}
ObjectSensorReadingMultiAgent objectSensorReadingMultiAgent;
objectSensorReadingMultiAgent.setAgent(agentAddress,agentPort);
objectSensorReadingMultiAgent.setSensor(objectParticleFilter.getSensor());
objectSensorReadingMultiAgent.setEstimationsWithModels(estimatedTargetModels);
objectSensorReadingMultiAgent.setEstimationsTimestamp(currentTimestamp.getMsFromMidnight());
mutex.lock();
observationsMultiAgent.push_back(objectSensorReadingMultiAgent);
mutex.unlock();
}
initialTimestamp = currentTimestamp;
++iterationCounter;
if (iterationCounter == 1)
{
iterationCounter = 0;
initialTimestampMas = currentTimestamp;
mutex.lock();
multiAgentProcessor.processReading(observationsMultiAgent);
estimatedTargetModelsMultiAgent = objectParticleFilterMultiAgent.getEstimationsWithModel();
observationsMultiAgent.clear();
mutex.unlock();
dataToSend = prepareDataForViewer();
ret = senderSocket.send(dataToSend,InetAddress(pViewerAddress,pViewerPort));
if (ret == -1)
{
ERR("Error when sending message to PViewer." << endl);
}
if (rosBridgeEnabled)
{
stringstream s;
for (EstimationsMultiAgent::const_iterator it = estimatedTargetModelsMultiAgent.begin(); it != estimatedTargetModelsMultiAgent.end(); ++it)
{
s << it->first << " " << it->second.first.first.observation.getCartesian().x << " " << it->second.first.first.observation.getCartesian().y << " " << it->second.first.second.x << " " << it->second.first.second.y << " "
<< it->second.first.first.model.width << " " << it->second.first.first.model.height << " " << it->second.first.first.model.velocity.x << " " << it->second.first.first.model.velocity.y << " "
<< it->second.first.first.model.averagedVelocity.x << " " << it->second.first.first.model.averagedVelocity.y << " ; ";
}
ret = senderSocket.send(s.str().substr(0,s.str().size() - 3),InetAddress(rosBridgeAddress,rosBridgePort));
if (ret == -1)
{
ERR("Error when sending message to the ros node bridge." << endl);
}
}
}
lastCurrentTargetIndex = currentTargetIndex;
#ifdef DEBUG_MODE
for (EstimationsMultiAgent::const_iterator it = estimatedTargetModelsMultiAgent.begin(); it != estimatedTargetModelsMultiAgent.end(); ++it)
{
WARN("[PTracker (" << agentId << ")] - target estimation global frame -> (" << it->first << ",[" << it->second.first.first.observation.getCartesian().x << ","
<< it->second.first.first.observation.getCartesian().y << "],"
<< "velocity = [" << it->second.first.first.model.velocity.x << ","
<< it->second.first.first.model.velocity.y << "],"
<< "averaged velocity = [" << it->second.first.first.model.averagedVelocity.x << ","
<< it->second.first.first.model.averagedVelocity.y << "])" << endl);
}
INFO("Time: " << (Timestamp() - currentTimestamp).getMs() << endl);
#endif
}
value _UdpSocket_SetTTL(value a, value b) {
UdpSocket* s = (UdpSocket*) val_data(a);
return alloc_bool(s->SetTTL(val_int(b)));
}
ConnectionPointer<Connection> ConnectionManager2::getConnWithConfig(quint8 type, const QHash<QString, QVariant> &cfg)
{
this->refresh();
#ifdef HAVE_LIBYB
// We want to receive first USB enumeration events,
// so that connections created here can be opened
QApplication::processEvents();
#endif
Connection *enumCon = NULL;
for(int i = 0; i < m_conns.size(); ++i)
{
if(m_conns[i]->getType() != type)
continue;
switch(type)
{
case CONNECTION_SERIAL_PORT:
{
SerialPort *sp = (SerialPort*)m_conns[i];
if(sp->deviceName() == cfg["device_name"])
{
if(!sp->removable())
enumCon = sp;
else if(sp->baudRate() == cfg["baud_rate"])
return ConnectionPointer<Connection>::fromPtr(sp);
}
break;
}
case CONNECTION_TCP_SOCKET:
{
TcpSocket *socket = (TcpSocket*)m_conns[i];
if(socket->host() == cfg["host"] && socket->port() == cfg["port"])
return ConnectionPointer<Connection>::fromPtr(socket);
break;
}
case CONNECTION_UDP_SOCKET:
{
UdpSocket *socket = (UdpSocket*)m_conns[i];
if (socket->name() == cfg["name"] &&
socket->host() == cfg["host"] && socket->port() == cfg["port"])
return ConnectionPointer<Connection>::fromPtr(socket);
break;
}
case CONNECTION_PROXY_TUNNEL:
{
ProxyTunnel *tunnel = (ProxyTunnel*)m_conns[i];
if(tunnel->name() == cfg["name"])
return ConnectionPointer<Connection>::fromPtr(tunnel);
break;
}
case CONNECTION_SHUPITO_TUNNEL:
{
QHash<QString, QVariant> c = cfg.value("companions").toHash();
QHash<QString, QVariant>::iterator itr = c.find(ShupitoTunnel::getCompanionName());
if(itr == c.end())
return ConnectionPointer<Connection>();
qint64 id = itr.value().toLongLong();
if(id != 0 && id == m_conns[i]->getCompanionId(ShupitoTunnel::getCompanionName()))
return ConnectionPointer<Connection>::fromPtr((ShupitoTunnel*)m_conns[i]);
break;
}
case CONNECTION_SHUPITO_SPI_TUNNEL:
{
QHash<QString, QVariant> c = cfg.value("companions").toHash();
QHash<QString, QVariant>::iterator itr = c.find(ShupitoSpiTunnelConn::getCompanionName());
if(itr == c.end())
return ConnectionPointer<Connection>();
qint64 id = itr.value().toLongLong();
if(id != 0 && id == m_conns[i]->getCompanionId(ShupitoSpiTunnelConn::getCompanionName()))
return ConnectionPointer<Connection>::fromPtr((ShupitoSpiTunnelConn*)m_conns[i]);
break;
}
#ifdef HAVE_LIBYB
case CONNECTION_USB_ACM2:
{
UsbAcmConnection2 *usb = (UsbAcmConnection2*)m_conns[i];
if (usb->vid() == cfg.value("vid", 0).toInt() &&
usb->pid() == cfg.value("pid", 0).toInt() &&
usb->serialNumber() == cfg.value("serial_number").toString() &&
usb->intfName() == cfg.value("intf_name").toString() &&
usb->baudRate() == cfg.value("baud_rate", 115200).toInt() &&
usb->stopBits() == cfg.value("stop_bits", 0).toInt() &&
usb->parity() == (UsbAcmConnection2::parity_t)cfg.value("parity", 0).toInt() &&
usb->dataBits() == cfg.value("data_bits", 8).toInt())
{
usb->applyConfig(cfg);
return ConnectionPointer<Connection>::fromPtr(usb);
}
break;
}
case CONNECTION_SHUPITO23:
{
UsbShupito23Connection *usb = (UsbShupito23Connection*)m_conns[i];
if (usb->vid() == cfg.value("vid", 0).toInt() &&
usb->pid() == cfg.value("pid", 0).toInt() &&
usb->serialNumber() == cfg.value("serial_number").toString() &&
usb->intfName() == cfg.value("intf_name").toString())
{
usb->applyConfig(cfg);
return ConnectionPointer<Connection>::fromPtr(usb);
}
break;
}
case CONNECTION_STM32:
{
STM32Connection *stm32 = (STM32Connection*)m_conns[i];
if (stm32->vid() == cfg.value("vid", 0).toInt() &&
stm32->pid() == cfg.value("pid", 0).toInt() &&
stm32->serialNumber() == cfg.value("serial_number").toString() &&
stm32->intfName() == cfg.value("intf_name").toString())
{
return ConnectionPointer<Connection>::fromPtr(stm32);
}
break;
}
#endif
default:
return ConnectionPointer<Connection>();
}
}
if(enumCon)
{
enumCon->applyConfig(cfg);
return ConnectionPointer<Connection>::fromPtr(enumCon);
}
if(type == CONNECTION_SHUPITO_TUNNEL)
{
QHash<QString, QVariant> c = cfg.value("companions").toHash();
QHash<QString, QVariant>::iterator itr = c.find(ShupitoTunnel::getCompanionName());
if(itr == c.end())
return ConnectionPointer<Connection>();
qint64 id = itr.value().toLongLong();
if(id == 0)
return ConnectionPointer<Connection>();
ConnectionPointer<Connection> tunnel(new ShupitoTunnel());
tunnel->applyConfig(cfg);
tunnel->setRemovable(false);
this->addConnection(tunnel.data());
return tunnel;
}
else if(type == CONNECTION_SHUPITO_SPI_TUNNEL)
{
QHash<QString, QVariant> c = cfg.value("companions").toHash();
QHash<QString, QVariant>::iterator itr = c.find(ShupitoSpiTunnelConn::getCompanionName());
if(itr == c.end())
return ConnectionPointer<Connection>();
qint64 id = itr.value().toLongLong();
if(id == 0)
return ConnectionPointer<Connection>();
ConnectionPointer<Connection> tunnel(new ShupitoSpiTunnelConn());
tunnel->applyConfig(cfg);
tunnel->setRemovable(false);
this->addConnection(tunnel.data());
return tunnel;
}
#ifdef HAVE_LIBYB
else if(type == CONNECTION_USB_ACM2)
{
ConnectionPointer<Connection> usb(createUsbAcmConn());
usb->applyConfig(cfg);
return usb;
}
else if(type == CONNECTION_STM32)
{
ConnectionPointer<Connection> stm32(createSTM32Conn());
stm32->applyConfig(cfg);
return stm32;
}
#endif
return ConnectionPointer<Connection>();
}
value _UdpSocket_GetTTL(value a) {
UdpSocket* s = (UdpSocket*) val_data(a);
return alloc_int(s->GetTTL());
}
