TEST(UdpSocket, bind) { // tests binding a socket to a port
UdpSocket server;
UdpSocket server2;
EXPECT_EQ(ClockError::SUCCESS, server.bind(12345));
EXPECT_EQ(ClockError::INVALID_USAGE, server.bind(12345));
EXPECT_EQ(ClockError::ADDRESS_INUSE, server2.bind(12345));
std::this_thread::sleep_for(std::chrono::milliseconds(20));
}
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);
}
}
int main(int argc, char** argv)
{
UdpSocket client;
client.bind(8002);
const char* message = "ahoj\n";
//client.write("224.3.29.71", 8001, message); multicast
/*int broadcastEnable=1;
int ret = setsockopt(client.get_handle(), SOL_SOCKET, SO_BROADCAST, &broadcastEnable, sizeof(broadcastEnable));
client.write("255.255.255.255", 8001, message); // broadcast
*/
printf("Sending to server: %s", message);
client.write("localhost", 8001, message);
sockaddr_in from_addr;
unsigned int from_len;
char buf[1024];
int size = client.readline(buf, sizeof(buf), (sockaddr*) &from_addr);
buf[size] = '\0';
printf("Received from server: %s", buf);
client.close();
return 0;
}
int my_receive()
{
printf("==== Receive\n");
UdpSocket receiver;
DataBuffer buffer(4096);
buffer.set_data_size(4096);
YETI_Result result = receiver.bind(SocketAddress(IpAddress::Any, 9123));
if (YETI_FAILED(result)) {
fprintf(stderr, "bind() failed (%d)\n", result);
return result;
}
SocketAddress address;
result = receiver.receive(buffer, &address);
if (YETI_FAILED(result)) {
fprintf(stderr, "receive() failed (%d)\n", result);
return result;
}
String addr_string = address.get_ipaddress().to_string();
printf("received packet, size=%d, from %s:%d\n",
(int)buffer.get_data_size(),
(const char*)addr_string,
(int)address.get_port());
return 0;
}
Program()
{
int r;
r = socket1.init();
socket1.setPort(10000);
socket1.bind();
socket1.setListener(this);
}
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;
}
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;
}
}
