int main( int argc, char* argv[])
{
if ( argc != 3 )
{
cout << "usage: " << argv[0] << " [localport] [remoteport]" <<
endl;
return 1;
}
int localport = atoi( argv[1] );
int remoteport = atoi( argv[2] );
UdpSocket mySock;
mySock.initialize( localport );
string msg = "herro!";
char msgrx[256];
memset( msgrx, 0, sizeof(msgrx) );
cout << "send returned " << mySock.send( "localhost", remoteport, (unsigned char*)msg.c_str(), msg.length() ) << endl;
cout << "receive returned " << mySock.receive( (unsigned char*)msgrx, sizeof(msgrx) ) << endl;
cout << "received ";
cout << msgrx << endl;
cout << endl;
return 0;
}
void sender(std::uint16_t port, const std::string &message, bool enableLogging) {
std::vector<std::string> addresses;
getIPV4BroadcastAddresses(addresses);
if(enableLogging) {
for(std::string a : addresses) {
std::cout << a << std::endl;
}
}
UdpSocket socket;
socket.enableBroadcast();
socket.enableReuseAddr();
socket.bind();
Datagram datagram;
datagram.port = port;
std::strcpy(datagram.buffer, message.c_str());
datagram.bufferUsed = message.size() + 1;
while(true) {
for(const std::string address : addresses) {
datagram.address = address;
socket.send(datagram);
}
usleep(1000*1000);
}
}
void run()
{
UdpSocket socket;
char_t buffer[1450];
char_t* pos = buffer;
int32_t networkInt = htonl(mIntValue);
int32_t networkFloat = htonl(*reinterpret_cast<int32_t*>(&mFloatValue));
int32_t strLen = static_cast<int32_t>(mStringValue.getLength());
int32_t netowrkStrlen = htonl(strLen);
Memory::Copy(pos, reinterpret_cast<char_t*>(&networkInt), sizeof(int32_t));
pos += sizeof(int32_t);
Memory::Copy(pos, reinterpret_cast<char_t*>(&netowrkStrlen), sizeof(int32_t));
pos += sizeof(int32_t);
Memory::Copy(pos, mStringValue.c_str(), strLen);
pos += strLen;
Memory::Copy(pos, reinterpret_cast<char_t*>(&networkFloat), sizeof(int32_t));
pos += sizeof(int32_t);
Memory::Copy(pos, reinterpret_cast<char_t*>(&mBoolValue), sizeof(bool_t));
pos += sizeof(bool_t);
socket.send(buffer, static_cast<int32_t>(pos - buffer), "127.0.0.1", mPort);
}
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;
}
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;
}
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
}
void* client_handler(void* _sockfd)
{
char* buf = 0;
int len;
uint16_t tcplen;
TcpMessage msg(mode);
// setup udp socket
UdpMessage udpMsg;
debug("Get connection from client, start authentication.");
int sockfd = reinterpret_cast<long>(_sockfd);
Connection conn(sockfd);
// Get A0 command
{
debug("Get A0 command from client");
tcplen = conn.recv_uint16();
buf = (char*) malloc(tcplen);
msg.len = tcplen;
len = conn.recv(buf + 2, tcplen - 2);
msg.deserialize(buf, tcplen);
free(buf);
// the user is not in the user database, close the TCP connection
if (users.find(msg.cmd.user) == users.end())
{
info(
"The user <%s> is not found in user database, close the connection with client",
msg.cmd.user.c_str());
// the de-constructor of conn will close the socket
return 0;
}
}
{
// A1 command: A1 32bit-random-number
debug("Send A1 command to client");
msg.cmd.step = 1;
buf = (char*) malloc(msg.length());
len = msg.serialize(buf);
conn.send(buf, len);
free(buf);
// A1 command -- end
}
{
debug("Get A2 command from client");
tcplen = conn.recv_uint16();
msg.len = tcplen;
buf = (char*) malloc(tcplen);
len = conn.recv(buf + 2, tcplen - 2);
msg.deserialize(buf, len);
free(buf);
}
UdpSocket udpSkt;
{
debug("Send A3 command to client");
msg.cmd.step = 3;
msg.cmd.status = 0;
buf = (char*) malloc(msg.length());
if (mode == '1')
{
std::string passwd = users[msg.cmd.user];
char sha[128];
get_sha256(sha, passwd, msg.cmd.rn);
if (strncmp(sha, msg.cmd.sha, 64) == 0)
{
msg.cmd.status = 0;
}
else
{
msg.cmd.status = 1;
}
}
// if auth successfully, setup UDP for query
if (msg.cmd.status == 0)
{
msg.cmd.sid = udpSkt.get_port(); // should be udp port
info("User Authentication done for <%s>.", msg.cmd.user.c_str());
}
len = msg.serialize(buf);
conn.send(buf, len);
free(buf);
}
{
debug("Get UDP query from client");
char udpbuf[PRO_UDP_MAX_LEN] = { 0 };
len = udpSkt.recv(udpbuf, PRO_UDP_MAX_LEN);
udpMsg.deserialize(udpbuf, len);
}
{
debug("Send UDP response to client");
udpMsg.dir = 'C';
udpMsg.timestamp = 1;
udpMsg.status = 1;
char udpbuf[PRO_UDP_MAX_LEN] = { 0 };
len = udpMsg.serialize(udpbuf);
errno = 0;
udpSkt.send(udpbuf, len);
}
printf("\n");
}
