Resolve "Print current ball model parameters somewhere"

Closes #1992 See merge request main/Sumatra!1893 sumatra-commit: 6b285e45bdc721c796934119b7a3d24ad1385088
TIGERs-Mannheim · Jul 16, 2024 · 9482c7c · 9482c7c
1 parent 195cc2c
commit 9482c7c
Show file tree

Hide file tree

Showing 4 changed files with 81 additions and 28 deletions.
diff --git a/modules/moduli-cam/src/main/java/edu/tigers/sumatra/cam/data/CamGeometry.java b/modules/moduli-cam/src/main/java/edu/tigers/sumatra/cam/data/CamGeometry.java
@@ -39,4 +39,16 @@ public CamGeometry merge(CamGeometry update)
 				.ballModels(update.getBallModels())
 				.build();
 	}
+
+
+	/**
+	 * Check if ballModels have changed.
+	 *
+	 * @param compare
+	 * @return True if the models are equal
+	 */
+	public boolean equalBallModels(CamGeometry compare)
+	{
+		return ballModels.equals(compare.getBallModels());
+	}
 }
diff --git a/modules/moduli-geometry/src/main/java/edu/tigers/sumatra/geometry/Geometry.java b/modules/moduli-geometry/src/main/java/edu/tigers/sumatra/geometry/Geometry.java
@@ -41,15 +41,14 @@ public class Geometry
 
 	@Configurable(defValue = "85.0")
 	private static double opponentCenter2DribblerDist = 85;
+	private static Geometry instance = defaultInstance();
+	private static ETeamColor negativeHalfTeam = ETeamColor.BLUE;
 
 	static
 	{
 		ConfigRegistration.registerClass("geom", Geometry.class);
 	}
 
-	private static Geometry instance = defaultInstance();
-	private static ETeamColor negativeHalfTeam = ETeamColor.BLUE;
-
 	private final IRectangle field;
 	private final IRectangle fieldWBorders;
 	private final Goal goalOur;
@@ -144,31 +143,6 @@ private static Geometry defaultInstance()
 	}
 
 
-	private BallParameters createBallParameters(CamGeometry geometry)
-	{
-		BallParameters newBallParameters = new BallParameters();
-
-		String env = SumatraModel.getInstance().getEnvironment();
-		ConfigRegistration.applySpezis(newBallParameters, "geom", "");
-		ConfigRegistration.applySpezis(newBallParameters, "geom", env);
-
-		var models = geometry.getBallModels();
-		if (models.hasStraightTwoPhase())
-		{
-			newBallParameters.setAccSlide(models.getStraightTwoPhase().getAccSlide() * 1000);
-			newBallParameters.setAccRoll(models.getStraightTwoPhase().getAccRoll() * 1000);
-			newBallParameters.setKSwitch(models.getStraightTwoPhase().getKSwitch());
-		}
-		if (models.hasChipFixedLoss())
-		{
-			newBallParameters.setChipDampingXYFirstHop(models.getChipFixedLoss().getDampingXyFirstHop());
-			newBallParameters.setChipDampingXYOtherHops(models.getChipFixedLoss().getDampingXyOtherHops());
-			newBallParameters.setChipDampingZ(models.getChipFixedLoss().getDampingZ());
-		}
-		return newBallParameters;
-	}
-
-
 	/**
 	 * Update geometry with the given data.
 	 * This will merge the geometry with the existing one.
@@ -177,6 +151,10 @@ private BallParameters createBallParameters(CamGeometry geometry)
 	 */
 	public static synchronized void update(final CamGeometry geometry)
 	{
+		if (!instance.lastCamGeometry.equalBallModels(geometry))
+		{
+			log.info("New Ball Models received from SSL vision!");
+		}
 		instance = new Geometry(instance.lastCamGeometry.merge(geometry));
 	}
 
@@ -484,4 +462,29 @@ public static void setNegativeHalfTeam(ETeamColor negativeHalfTeam)
 	{
 		Geometry.negativeHalfTeam = negativeHalfTeam;
 	}
+
+
+	private BallParameters createBallParameters(CamGeometry geometry)
+	{
+		BallParameters newBallParameters = new BallParameters();
+
+		String env = SumatraModel.getInstance().getEnvironment();
+		ConfigRegistration.applySpezis(newBallParameters, "geom", "");
+		ConfigRegistration.applySpezis(newBallParameters, "geom", env);
+
+		var models = geometry.getBallModels();
+		if (models.hasStraightTwoPhase())
+		{
+			newBallParameters.setAccSlide(models.getStraightTwoPhase().getAccSlide() * 1000);
+			newBallParameters.setAccRoll(models.getStraightTwoPhase().getAccRoll() * 1000);
+			newBallParameters.setKSwitch(models.getStraightTwoPhase().getKSwitch());
+		}
+		if (models.hasChipFixedLoss())
+		{
+			newBallParameters.setChipDampingXYFirstHop(models.getChipFixedLoss().getDampingXyFirstHop());
+			newBallParameters.setChipDampingXYOtherHops(models.getChipFixedLoss().getDampingXyOtherHops());
+			newBallParameters.setChipDampingZ(models.getChipFixedLoss().getDampingZ());
+		}
+		return newBallParameters;
+	}
 }
diff --git a/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/BallVisCalc.java b/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/BallVisCalc.java
@@ -6,12 +6,14 @@
 
 import edu.tigers.sumatra.drawable.DrawableAnnotation;
 import edu.tigers.sumatra.drawable.DrawableArrow;
+import edu.tigers.sumatra.drawable.DrawableBorderText;
 import edu.tigers.sumatra.drawable.DrawableCircle;
 import edu.tigers.sumatra.drawable.DrawableLine;
 import edu.tigers.sumatra.drawable.DrawablePlanarCurve;
 import edu.tigers.sumatra.drawable.IDrawableShape;
 import edu.tigers.sumatra.drawable.ShapeMap;
 import edu.tigers.sumatra.drawable.animated.AnimatedCrosshair;
+import edu.tigers.sumatra.geometry.BallParameters;
 import edu.tigers.sumatra.geometry.Geometry;
 import edu.tigers.sumatra.geometry.RuleConstraints;
 import edu.tigers.sumatra.math.circle.Circle;
@@ -23,6 +25,8 @@
 import edu.tigers.sumatra.wp.data.WorldFrameWrapper;
 
 import java.awt.Color;
+import java.util.ArrayList;
+import java.util.List;
 
 
 /**
@@ -95,5 +99,37 @@ public void process(final WorldFrameWrapper wfw, final ShapeMap shapeMap)
 		wfw.getSimpleWorldFrame().getKickedBall().ifPresent(kickedBall -> shapeMap.get(EWpShapesLayer.BALL_PREDICTION)
 				.add(new DrawableArrow(kickedBall.getKickPos(), kickedBall.getKickVel().getXYVector().multiplyNew(100))
 						.setColor(Color.magenta)));
+
+		createBallParameterShapes(shapeMap);
+	}
+
+
+	private void createBallParameterShapes(ShapeMap shapeMap)
+	{
+		BallParameters params = Geometry.getBallParameters();
+		List<String> textLinesStraight = new ArrayList<>();
+		textLinesStraight.add("Straight");
+		textLinesStraight.add("AccSlide: " + params.getAccSlide());
+		textLinesStraight.add("AccRoll: " + params.getAccRoll());
+		textLinesStraight.add("KSwitch: " + params.getKSwitch());
+		List<String> textLinesChip = new ArrayList<>();
+		textLinesChip.add("Chip");
+		textLinesChip.add("DampingXY 1. Hop: " + params.getChipDampingXYFirstHop());
+		textLinesChip.add("DampingXy n. Hops: " + params.getChipDampingXYOtherHops());
+		textLinesChip.add("DampingZ: " + params.getChipDampingZ());
+		double posXStraight = 1.0;
+		double posXChip = 10.0;
+		double posY = 50.0;
+		for (int i = 0; i < textLinesStraight.size(); i++)
+		{
+			DrawableBorderText sText = new DrawableBorderText(Vector2.fromXY(posXStraight, posY),
+					textLinesStraight.get(i));
+			shapeMap.get(EWpShapesLayer.BALL_MODELS).add(sText.setColor(Color.CYAN));
+			DrawableBorderText cText = new DrawableBorderText(Vector2.fromXY(posXChip, posY), textLinesChip.get(i));
+			shapeMap.get(EWpShapesLayer.BALL_MODELS).add(cText.setColor(Color.CYAN));
+			posY = posY + 1.2;
+		}
+
+
 	}
 }
diff --git a/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/EWpShapesLayer.java b/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/EWpShapesLayer.java
@@ -63,4 +63,6 @@ public final class EWpShapesLayer
 			F.category(FIELD).category(CAT_BALL).layerName("Highlighter").visibleByDefault(true));
 	public static final IShapeLayerIdentifier BALL_PREDICTION = F.create(
 			F.category(FIELD).category(CAT_BALL).layerName("Prediction"));
+	public static final IShapeLayerIdentifier BALL_MODELS = F.create(
+			F.category(FIELD).category(CAT_BALL).layerName("Models"));
 }