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