Commit 22179c59 authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

Resolve "AutoRef publishes tracker packets to wrong interface"

Closes #1582

See merge request main/Sumatra!1328

sumatra-commit: 8a6ae4c3ee6123877fc8e98c9035c845ccee4c42
parent 320bd880
Pipeline #8608 passed with stage
in 3 minutes and 1 second
......@@ -5,6 +5,7 @@
plugins {
id 'java'
id 'jacoco'
id 'idea'
id 'ca.cutterslade.analyze'
}
......
/*
* Copyright (c) 2009 - 2019, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.network;
import lombok.extern.log4j.Log4j2;
import java.io.IOException;
import java.net.DatagramPacket;
import java.net.InetAddress;
import java.net.MulticastSocket;
import java.net.NetworkInterface;
import java.net.NoRouteToHostException;
import java.net.SocketException;
import java.net.UnknownHostException;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;
/**
* This class is an {@link ITransmitter} implementation capable of sending some {@code byte[]}-data via UDP to a
* multicast-group.
*/
@Log4j2
public class MulticastUDPTransmitter implements ITransmitter<byte[]>
{
private static final Logger log = LogManager.getLogger(MulticastUDPTransmitter.class);
private final int targetPort;
private final InetAddress targetAddr;
private MulticastSocket socket = null;
private final List<MulticastSocket> sockets = new ArrayList<>();
private boolean lastSendFailed = false;
......@@ -38,13 +43,65 @@ public class MulticastUDPTransmitter implements ITransmitter<byte[]>
{
this.targetPort = targetPort;
this.targetAddr = addressByName(targetAddr);
}
private List<NetworkInterface> getNetworkInterfaces()
{
try
{
return NetworkInterface.networkInterfaces().collect(Collectors.toUnmodifiableList());
} catch (SocketException e)
{
log.error("Could not get available network interfaces", e);
}
return Collections.emptyList();
}
public void connectToAllInterfaces()
{
var networkInterfaces = getNetworkInterfaces();
for (var nif : networkInterfaces)
{
connectTo(nif);
}
}
public void connectTo(String nifName)
{
try
{
socket = new MulticastSocket();
} catch (IOException err)
var nif = NetworkInterface.getByName(nifName);
if (nif != null)
{
connectTo(nif);
} else
{
log.warn("Specified nif not found: {}", nifName);
}
} catch (SocketException e)
{
log.error("Error while creating MulticastSocket!", err);
log.error("Could not get an interface by name", e);
}
}
private void connectTo(NetworkInterface nif)
{
try
{
if (nif.supportsMulticast())
{
@SuppressWarnings("squid:S2095") // closing resources: can not close resource here
var socket = new MulticastSocket();
socket.setNetworkInterface(nif);
sockets.add(socket);
}
} catch (IOException e)
{
log.warn("Could not connect at {}", nif, e);
}
}
......@@ -65,11 +122,11 @@ public class MulticastUDPTransmitter implements ITransmitter<byte[]>
@Override
public synchronized boolean send(final byte[] data)
{
if (socket == null)
if (sockets.isEmpty())
{
if (!lastSendFailed)
{
log.error("Transmitter is not ready to send!");
log.warn("Transmitter is not ready to send!");
lastSendFailed = true;
}
return false;
......@@ -77,39 +134,37 @@ public class MulticastUDPTransmitter implements ITransmitter<byte[]>
DatagramPacket tempPacket = new DatagramPacket(data, data.length, targetAddr, targetPort);
// Receive _outside_ the synchronized state, to prevent blocking of the state
try
for (var socket : sockets)
{
socket.send(tempPacket); // DatagramPacket is sent...
lastSendFailed = false;
} catch (NoRouteToHostException nrh)
{
log.warn("No route to host: '" + targetAddr + "'. Dropping packet...", nrh);
return false;
} catch (IOException err)
{
if (!lastSendFailed)
// Receive _outside_ the synchronized state, to prevent blocking of the state
try
{
log.error("Error while sending data to: '" + targetAddr + ":" + targetPort + "'. "
+ "If you are not in any network, multicast is not supported by default. "
+ "On Linux, you can enable multicast on the loopback interface by executing following commands as root: "
+ "route add -net 224.0.0.0 netmask 240.0.0.0 dev lo && ifconfig lo multicast", err);
lastSendFailed = true;
socket.send(tempPacket); // DatagramPacket is sent...
lastSendFailed = false;
} catch (NoRouteToHostException nrh)
{
log.warn("No route to host: '" + targetAddr + "'. Dropping packet...", nrh);
} catch (IOException err)
{
if (!lastSendFailed)
{
log.warn("Error while sending data to: '" + targetAddr + ":" + targetPort + "'. "
+ "If you are not in any network, multicast is not supported by default. "
+ "On Linux, you can enable multicast on the loopback interface by executing following commands as root: "
+ "route add -net 224.0.0.0 netmask 240.0.0.0 dev lo && ifconfig lo multicast", err);
lastSendFailed = true;
}
}
return false;
}
return true;
return !lastSendFailed;
}
@Override
public synchronized void close()
{
if (socket != null)
{
socket.close();
socket = null;
}
sockets.forEach(MulticastSocket::close);
sockets.clear();
}
}
......@@ -6,7 +6,6 @@ plugins {
id 'sumatra.java-conventions'
id 'java-library'
id 'sumatra.protobuf-conventions'
id "de.undercouch.download" version "4.0.4"
}
configurations {
......
......@@ -43,7 +43,7 @@ public class CiGameControllerConnector
public synchronized void start() throws IOException
{
log.trace("Starting");
trackerPacketGenerator = new TrackerPacketGenerator();
trackerPacketGenerator = new TrackerPacketGenerator("TIGERs");
watchdog = new Watchdog(5000, this.getClass().getSimpleName(), this::onTimeout);
watchdog.start();
connect();
......
......@@ -17,6 +17,7 @@ import edu.tigers.sumatra.wp.data.ITrackedBot;
import edu.tigers.sumatra.wp.data.SimpleWorldFrame;
import edu.tigers.sumatra.wp.proto.SslVisionDetectionTracked;
import edu.tigers.sumatra.wp.proto.SslVisionWrapperTracked;
import lombok.RequiredArgsConstructor;
import java.util.Collection;
import java.util.HashSet;
......@@ -25,9 +26,10 @@ import java.util.UUID;
import java.util.stream.Collectors;
@RequiredArgsConstructor
public class TrackerPacketGenerator
{
private static final String SOURCE_NAME = "TIGERs";
private final String sourceName;
private static final Set<SslVisionDetectionTracked.Capability> CAPABILITIES = new HashSet<>();
private int frameNumber = 0;
private final String uuid = UUID.randomUUID().toString();
......@@ -53,7 +55,7 @@ public class TrackerPacketGenerator
SslVisionWrapperTracked.TrackerWrapperPacket.Builder wrapper = SslVisionWrapperTracked.TrackerWrapperPacket
.newBuilder();
wrapper.setUuid(uuid);
wrapper.setSourceName(SOURCE_NAME);
wrapper.setSourceName(sourceName);
wrapper.setTrackedFrame(frame);
return wrapper.build();
}
......
......@@ -27,14 +27,15 @@ import edu.tigers.sumatra.wp.AWorldPredictor;
import edu.tigers.sumatra.wp.IWorldFrameObserver;
import edu.tigers.sumatra.wp.data.ITrackedBot;
import edu.tigers.sumatra.wp.data.WorldFrameWrapper;
import lombok.extern.log4j.Log4j2;
/**
* Send out SSL vision frames based on WorldFrames
*/
@Log4j2
public class SSLVisionSender extends AModule implements IWorldFrameObserver
{
private static final String ADDRESS = "224.5.23.2";
private static final double GEOMETRY_BROADCAST_INTERVAL = 3;
private MulticastUDPTransmitter transmitter;
......@@ -45,8 +46,20 @@ public class SSLVisionSender extends AModule implements IWorldFrameObserver
@Override
public void startModule()
{
String address = getSubnodeConfiguration().getString("address", "224.5.23.2");
int port = getSubnodeConfiguration().getInt("port", 11006);
transmitter = new MulticastUDPTransmitter(ADDRESS, port);
transmitter = new MulticastUDPTransmitter(address, port);
String nifName = getSubnodeConfiguration().getString("interface", null);
if (nifName != null)
{
log.info("Publishing vision packets to {}:{} ({})", address, port, nifName);
transmitter.connectTo(nifName);
} else
{
log.info("Publishing vision packets to {}:{} (all interfaces)", address, port);
transmitter.connectToAllInterfaces();
}
SumatraModel.getInstance().getModule(AWorldPredictor.class).addObserver(this);
}
......
......@@ -12,16 +12,19 @@ import edu.tigers.sumatra.wp.IWorldFrameObserver;
import edu.tigers.sumatra.wp.TrackerPacketGenerator;
import edu.tigers.sumatra.wp.data.WorldFrameWrapper;
import edu.tigers.sumatra.wp.proto.SslVisionWrapperTracked;
import lombok.extern.log4j.Log4j2;
/**
* Export standardized vision tracking data.
*/
@Log4j2
public class VisionTrackerSender extends AModule implements IWorldFrameObserver
{
private MulticastUDPTransmitter transmitter;
private TrackerPacketGenerator trackerPacketGenerator;
@Override
public void startModule()
{
......@@ -29,7 +32,19 @@ public class VisionTrackerSender extends AModule implements IWorldFrameObserver
int port = getSubnodeConfiguration().getInt("port", 10010);
transmitter = new MulticastUDPTransmitter(address, port);
trackerPacketGenerator = new TrackerPacketGenerator();
String nifName = getSubnodeConfiguration().getString("interface", null);
if (nifName != null)
{
log.info("Publishing vision tracking packets to {}:{} ({})", address, port, nifName);
transmitter.connectTo(nifName);
} else
{
log.info("Publishing vision tracking packets to {}:{} (all interfaces)", address, port);
transmitter.connectToAllInterfaces();
}
String sourceName = getSubnodeConfiguration().getString("source-name", "TIGERs");
trackerPacketGenerator = new TrackerPacketGenerator(sourceName);
SumatraModel.getInstance().getModule(AWorldPredictor.class).addObserver(this);
}
......
/*
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.wp.exporter;
import edu.tigers.moduli.AModule;
import edu.tigers.sumatra.ids.ETeamColor;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.model.SumatraModel;
import edu.tigers.sumatra.network.MulticastUDPTransmitter;
import edu.tigers.sumatra.wp.AWorldPredictor;
import edu.tigers.sumatra.wp.IWorldFrameObserver;
import edu.tigers.sumatra.wp.data.ITrackedBot;
import edu.tigers.sumatra.wp.data.SimpleWorldFrame;
import edu.tigers.sumatra.wp.data.WorldFrameWrapper;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
/**
* Sends world frames in a custom protobuf format to the network.
*/
public class WorldFrameSender extends AModule implements IWorldFrameObserver
{
private static final int PORT = 42000;
private static final String ADDRESS = "224.5.23.2";
private static final boolean ADD_NOISE = false;
private static final double MIN_DT = 0.005;
private double delay;
private MulticastUDPTransmitter transmitter;
private long tLast = 0;
private final Random rnd = new Random(0);
private final List<SumatraWfExport.WorldFrame.Builder> buffer = new LinkedList<>();
@Override
public void startModule()
{
delay = getSubnodeConfiguration().getDouble("delay", 0);
transmitter = new MulticastUDPTransmitter(ADDRESS, PORT);
SumatraModel.getInstance().getModule(AWorldPredictor.class).addObserver(this);
}
@Override
public void stopModule()
{
SumatraModel.getInstance().getModule(AWorldPredictor.class).removeObserver(this);
}
@Override
public void onNewWorldFrame(final WorldFrameWrapper wFrameWrapper)
{
long tNow = wFrameWrapper.getSimpleWorldFrame().getTimestamp();
double dt = (tNow - tLast) / 1e9;
if (dt < MIN_DT)
{
return;
}
tLast = tNow;
SimpleWorldFrame swf = wFrameWrapper.getSimpleWorldFrame();
SumatraWfExport.WorldFrame.Builder wfBuilder = getWfExportBuilder(swf);
buffer.add(wfBuilder);
List<SumatraWfExport.WorldFrame.Builder> toBeRem = new ArrayList<>();
for (SumatraWfExport.WorldFrame.Builder wf : buffer)
{
if ((swf.getTimestamp() - (delay * 1E9) - wf.getTimestamp()) < 0)
{
break;
}
wf.setTimestamp(swf.getTimestamp());
toBeRem.add(wf);
transmitter.send(wf.build().toByteArray());
}
buffer.removeAll(toBeRem);
}
private SumatraWfExport.WorldFrame.Builder getWfExportBuilder(final SimpleWorldFrame swf)
{
SumatraWfExport.WorldFrame.Builder wfBuilder = SumatraWfExport.WorldFrame.newBuilder();
wfBuilder.setFrameId(swf.getFrameNumber()).setTimestamp(swf.getTimestamp());
SumatraWfExport.Ball.Builder ball = SumatraWfExport.Ball.newBuilder();
ball.setPos(convertVector(swf.getBall().getPos3().multiplyNew(1e-3)))
.setVel(convertVector(swf.getBall().getVel3()));
wfBuilder.setBall(ball);
double std = 0.1;
double aStd = 0.3;
for (ITrackedBot tBot : swf.getBots().values())
{
SumatraWfExport.Bot.Builder bot = SumatraWfExport.Bot.newBuilder();
bot.setId(tBot.getBotId().getNumber())
.setTeamColor(tBot.getBotId().getTeamColor() == ETeamColor.BLUE
? SumatraWfExport.TeamColor.BLUE
: SumatraWfExport.TeamColor.YELLOW);
bot.setPos(convertVector(tBot.getPos().multiplyNew(1e-3), tBot.getOrientation()));
IVector2 vel = tBot.getVel();
double aVel = tBot.getAngularVel();
if (ADD_NOISE)
{
vel = vel.addNew(Vector2.fromXY(rnd.nextGaussian() * std, rnd.nextGaussian() * std));
aVel += (rnd.nextGaussian() * aStd);
}
bot.setVel(convertVector(vel, aVel));
wfBuilder.addBots(bot);
}
return wfBuilder;
}
private SumatraWfExport.Vector3 convertVector(final IVector3 in)
{
SumatraWfExport.Vector3.Builder out = SumatraWfExport.Vector3.newBuilder();
return out.setX(in.x()).setY(in.y()).setZ(in.z()).build();
}
private SumatraWfExport.Vector3 convertVector(final IVector2 in, final double z)
{
SumatraWfExport.Vector3.Builder out = SumatraWfExport.Vector3.newBuilder();
return out.setX(in.x()).setY(in.y()).setZ(z).build();
}
}
syntax = "proto2";
package edu.tigers.sumatra.wp.exporter;
message WorldFrame
{
required int64 frameId = 1;
required int64 timestamp = 2;
repeated Bot bots = 3;
optional Ball ball = 4;
}
message Ball
{
required Vector3 pos = 1;
required Vector3 vel = 2;
optional Vector3 acc = 3;
}
message Bot
{
required int32 id = 1;
required TeamColor teamColor = 2;
required Vector3 pos = 3;
required Vector3 vel = 4;
optional Vector3 acc = 5;
}
message Vector3 {
required double x = 1;
required double y = 2;
required double z = 3;
}
enum TeamColor
{
YELLOW = 1;
BLUE = 2;
}
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment