Resolve "Remove vanished cameras after some timeout"

Closes #1606 See merge request main/Sumatra!1347 sumatra-commit: a86952de9cdb05ac38987bebd36377f4ac894e3e
ZJU-RoboCup · Apr 25, 2021 · b3787b4 · b3787b4
1 parent d50d1f3
commit b3787b4
Show file tree

Hide file tree

Showing 4 changed files with 151 additions and 118 deletions.
diff --git a/modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/CamFilter.java b/modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/CamFilter.java
@@ -141,6 +141,7 @@ public CamFilter(final int camId)
 	 */
 	public void update(final CamDetectionFrame frame, final FilteredVisionFrame lastFilteredFrame)
 	{
+		checkForNonConsecutiveFrames(frame);
 		CamDetectionFrame adjustedFrame = adjustTCapture(frame);
 
 		processRobots(adjustedFrame, lastFilteredFrame.getBots());
@@ -203,20 +204,19 @@ public void setBallInfo(final BallFilterOutput ballFilterOutput)
 	}
 
 
-	private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame)
+	private void reset()
 	{
-		if (frame.getCamFrameNumber() != (lastCamFrameId + 1))
-		{
-			if (lastCamFrameId != 0)
-			{
-				log.warn("Non-consecutive cam frame for cam {}: {} -> {}", frame.getCameraId(), lastCamFrameId,
-						frame.getCamFrameNumber());
-			}
-			frameIntervalFilter.reset();
-		}
+		frameIntervalFilter.reset();
+		lastKnownBallPosition = Vector2f.ZERO_VECTOR;
+		lastBallVisibleTimestamp = 0;
+		robots.clear();
+		balls.clear();
+		ballHistory.clear();
+	}
 
-		lastCamFrameId = frame.getCamFrameNumber();
 
+	private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame)
+	{
 		if ((frame.getCamFrameNumber() % FRAME_FILTER_DIVIDER) == 0)
 		{
 			frameIntervalFilter.addSample(frame.getCamFrameNumber(), frame.gettCapture());
@@ -230,6 +230,26 @@ private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame)
 	}
 
 
+	private void checkForNonConsecutiveFrames(CamDetectionFrame frame)
+	{
+		if (frame.getCamFrameNumber() != (lastCamFrameId + 1))
+		{
+			if (lastCamFrameId != 0)
+			{
+				log.warn("Non-consecutive cam frame for cam {}: {} -> {}", frame.getCameraId(), lastCamFrameId,
+						frame.getCamFrameNumber());
+			}
+			if (Math.abs(frame.getCamFrameNumber() - lastCamFrameId + 1) > 10)
+			{
+				log.info("Resetting cam filter for cam {}", camId);
+				reset();
+			}
+		}
+
+		lastCamFrameId = frame.getCamFrameNumber();
+	}
+
+
 	private List<RobotCollisionShape> getRobotCollisionShapes(final List<FilteredVisionBot> mergedRobots)
 	{
 		List<RobotCollisionShape> shapes = new ArrayList<>();