Resolve "Sumatra simulation timing is wrong"

Closes #1584

See merge request main/Sumatra!1330

sumatra-commit: 66e44fa7f3f6431820821a6c88d03d8135c4c699

modules/common/src/main/java/edu/tigers/sumatra/clock/FpsCounter.java

@@ -24,6 +24,10 @@ public class FpsCounter
 	public boolean newFrame(final long timestamp)
 	{
 		boolean fpsChanged = false;
+		if (timestamp < lastTime)
+		{
+			reset();
+		}
 		double timeDiff = (timestamp - lastTime) / 1e9;
 		if (timeDiff > TIME_WINDOW)
 		{

modules/common/src/main/java/edu/tigers/sumatra/network/UdpTransceiver.java

@@ -78,6 +78,7 @@ private void connect()
 			} catch (IOException e)
 			{
 				log.warn("Failed to setup socket for {}", this, e);
+				socket = null;
 			}
 		}
 	}
@@ -125,7 +126,7 @@ private void receive()
 
 	public void send(byte[] bytes)
 	{
-		if (socket == null)
+		if (socket == null || address == null)
 		{
 			return;
 		}

modules/moduli-cam/src/main/java/edu/tigers/sumatra/cam/SSLVisionCam.java

@@ -18,8 +18,10 @@
 import java.io.ByteArrayInputStream;
 import java.io.IOException;
 import java.net.DatagramPacket;
+import java.net.InetAddress;
 import java.net.NetworkInterface;
 import java.nio.ByteBuffer;
+import java.util.Optional;
 
 
 /**
@@ -37,6 +39,7 @@ public class SSLVisionCam extends ACam implements Runnable, IReceiverObserver, I
 	private boolean expectIOE = false;
 	private int port;
 	private String address;
+	private InetAddress visionAddress;
 
 	private final SSLVisionCamGeometryTranslator geometryTranslator = new SSLVisionCamGeometryTranslator();
 
@@ -45,7 +48,6 @@ public class SSLVisionCam extends ACam implements Runnable, IReceiverObserver, I
 	private static String network = "";
 
 
-
 	static
 	{
 		ConfigRegistration.registerClass("user", SSLVisionCam.class);
@@ -111,6 +113,7 @@ public void run()
 				}
 				receiver.receive(packet);
 
+				visionAddress = packet.getAddress();
 				final ByteArrayInputStream packetIn = new ByteArrayInputStream(packet.getData(), 0, packet.getLength());
 
 				// Translate
@@ -208,4 +211,10 @@ public final String getAddress()
 	{
 		return address;
 	}
+
+
+	public Optional<InetAddress> getVisionAddress()
+	{
+		return Optional.ofNullable(visionAddress);
+	}
 }

modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/CamFilter.java

@@ -35,16 +35,20 @@
 import edu.tigers.sumatra.vision.data.RobotCollisionShape;
 import edu.tigers.sumatra.vision.tracker.BallTracker;
 import edu.tigers.sumatra.vision.tracker.RobotTracker;
+import lombok.Getter;
+import org.apache.commons.collections4.QueueUtils;
 import org.apache.commons.collections4.queue.CircularFifoQueue;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
 import java.awt.Color;
 import java.util.ArrayList;
-import java.util.HashMap;
+import java.util.Collections;
 import java.util.List;
 import java.util.Map;
 import java.util.Optional;
+import java.util.Queue;
+import java.util.concurrent.ConcurrentHashMap;
 
 
 /**
@@ -62,6 +66,8 @@ public class CamFilter
 	private final FirstOrderMultiSampleEstimator frameIntervalFilter = new FirstOrderMultiSampleEstimator(
 			FRAME_FILTER_NUM_SAMPLES);
 	private long lastCamFrameId;
+	@Getter
+	private long timestamp;
 
 	private Optional<CamCalibration> calibration = Optional.empty();
 	private Optional<IRectangle> fieldRectWithBoundary = Optional.empty();
@@ -71,13 +77,13 @@ public class CamFilter
 	private IVector2 lastKnownBallPosition = Vector2f.ZERO_VECTOR;
 	private long lastBallVisibleTimestamp = 0;
 
-	private final Map<BotID, RobotTracker> robots = new HashMap<>();
+	private final Map<BotID, RobotTracker> robots = new ConcurrentHashMap<>();
 
-	private final List<BallTracker> balls = new ArrayList<>();
+	private final List<BallTracker> balls = Collections.synchronizedList(new ArrayList<>());
 
-	private Map<BotID, RobotInfo> robotInfoMap = new HashMap<>();
+	private Map<BotID, RobotInfo> robotInfoMap = new ConcurrentHashMap<>();
 
-	private CircularFifoQueue<CamBall> ballHistory = new CircularFifoQueue<>(100);
+	private Queue<CamBall> ballHistory = QueueUtils.synchronizedQueue(new CircularFifoQueue<>(100));
 
 	@Configurable(defValue = "1.0", comment = "Time in [s] after an invisible ball is removed")
 	private static double invisibleLifetimeBall = 1.0;
@@ -132,16 +138,15 @@ public CamFilter(final int camId)
 	 *
 	 * @param frame
 	 * @param lastFilteredFrame
-	 * @return adjusted tCapture
 	 */
-	public long update(final CamDetectionFrame frame, final FilteredVisionFrame lastFilteredFrame)
+	public void update(final CamDetectionFrame frame, final FilteredVisionFrame lastFilteredFrame)
 	{
 		CamDetectionFrame adjustedFrame = adjustTCapture(frame);
 
 		processRobots(adjustedFrame, lastFilteredFrame.getBots());
 		processBalls(adjustedFrame, lastFilteredFrame.getBall(), lastFilteredFrame.getBots());
 
-		return adjustedFrame.gettCapture();
+		timestamp = adjustedFrame.gettCapture();
 	}
 
 
@@ -204,7 +209,8 @@ private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame)
 		{
 			if (lastCamFrameId != 0)
 			{
-				log.warn("Non-consecutive cam frame: " + lastCamFrameId + " -> " + frame.getCamFrameNumber());
+				log.warn("Non-consecutive cam frame for cam {}: {} -> {}", frame.getCameraId(), lastCamFrameId,
+						frame.getCamFrameNumber());
 			}
 			frameIntervalFilter.reset();
 		}

modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/RobotQualityInspector.java

@@ -1,16 +1,19 @@
-package edu.tigers.sumatra.vision;
+/*
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
+ */
 
-import java.util.ArrayList;
-import java.util.IdentityHashMap;
-import java.util.List;
-import java.util.Map;
+package edu.tigers.sumatra.vision;
 
 import com.github.g3force.configurable.ConfigRegistration;
 import com.github.g3force.configurable.Configurable;
-
 import edu.tigers.sumatra.cam.data.CamRobot;
 import edu.tigers.sumatra.ids.BotID;
 
+import java.util.ArrayList;
+import java.util.IdentityHashMap;
+import java.util.List;
+import java.util.Map;
+
 
 /**
  * Track the overall quality/visibility of each robot by counting the number of detection over a fixed time horizon.
@@ -42,13 +45,13 @@ public RobotQualityInspector()
 	}
 
 
-	public void addDetection(CamRobot camRobot)
+	public synchronized void addDetection(CamRobot camRobot)
 	{
 		measurements.get(camRobot.getBotId()).add(camRobot.getTimestamp());
 	}
 
 
-	public void prune(long currentTimestamp)
+	public synchronized void prune(long currentTimestamp)
 	{
 		long timestamp = currentTimestamp - (long) (trackingTimeHorizon * 1e9);
 		for (List<Long> timestamps : measurements.values())
@@ -67,7 +70,7 @@ public void prune(long currentTimestamp)
 	}
 
 
-	public void updateAverageDt(double averageDt)
+	public synchronized void updateAverageDt(double averageDt)
 	{
 		maxPossibleDetectionsPerCam = trackingTimeHorizon / averageDt;
 	}
@@ -79,19 +82,19 @@ private double getQuality(final BotID botID)
 	}
 
 
-	public long getNumDetections(final BotID botID)
+	public synchronized long getNumDetections(final BotID botID)
 	{
 		return measurements.get(botID).size();
 	}
 
 
-	public double getPossibleDetections()
+	public synchronized double getPossibleDetections()
 	{
 		return maxPossibleDetectionsPerCam;
 	}
 
 
-	public boolean passesQualityInspection(final BotID botID)
+	public synchronized boolean passesQualityInspection(final BotID botID)
 	{
 		return getQuality(botID) > robotQualityThreshold;
 	}

modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/ViewportArchitect.java

@@ -1,21 +1,10 @@
 /*
- * Copyright (c) 2009 - 2017, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 package edu.tigers.sumatra.vision;
 
-import java.awt.Color;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-import java.util.Map.Entry;
-import java.util.Optional;
-import java.util.concurrent.CopyOnWriteArrayList;
-import java.util.stream.Collectors;
-
 import com.github.g3force.configurable.ConfigRegistration;
 import com.github.g3force.configurable.Configurable;
-
 import edu.tigers.sumatra.cam.data.CamCalibration;
 import edu.tigers.sumatra.cam.data.CamDetectionFrame;
 import edu.tigers.sumatra.cam.data.CamGeometry;
@@ -30,22 +19,32 @@
 import edu.tigers.sumatra.math.vector.Vector2;
 import edu.tigers.sumatra.math.vector.Vector2f;
 
+import java.awt.Color;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.Map.Entry;
+import java.util.Optional;
+import java.util.concurrent.ConcurrentSkipListMap;
+import java.util.concurrent.CopyOnWriteArrayList;
+import java.util.stream.Collectors;
+
 
 /**
  * The viewport architect inspects all camera geometries and aligns their viewports to a predefined overlap.
- * 
+ *
  * @author AndreR <[email protected]>
  */
 public class ViewportArchitect
 {
-	private Map<Integer, Viewport> viewports = new HashMap<>();
+	private Map<Integer, Viewport> viewports = new ConcurrentSkipListMap<>();
 	private final List<IViewportArchitect> observers = new CopyOnWriteArrayList<>();
 	private Viewport field;
-	
-	
+
+
 	@Configurable(defValue = "400.0", comment = "Maximum camera overlap. [mm]")
 	private static double maxViewportOverlap = 400.0;
-	
+
 	@Configurable(defValue = "DYNAMICALLY", comment = "Method to be used to construct viewports.")
 	private static EViewportConstruction viewportConstruction = EViewportConstruction.DYNAMICALLY;