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;
}
}
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");
}
