Resolve "Improve robot movement in external simulators"

Closes #1586

See merge request main/Sumatra!1332

sumatra-commit: 507b9394234ca785157a11b42c15c6c197360fdf

modules/common-gui/src/main/java/edu/tigers/sumatra/view/toolbar/ToolBar.java

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 package edu.tigers.sumatra.view.toolbar;
 
@@ -218,7 +218,7 @@ public void actionPerformed(final ActionEvent e)
 					observer.onToggleRecord();
 				}
 				btnRecSave.setEnabled(true);
-			});
+			}, "RecordSaveButton");
 
 			t.start();
 		}

modules/common/src/main/java/edu/tigers/sumatra/trajectory/BangBangTrajectoryFactory.java

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
 package edu.tigers.sumatra.trajectory;
@@ -14,6 +14,7 @@
 
 public final class BangBangTrajectoryFactory
 {
+	private static final double MAX_VEL_TOLERANCE = 0.2;
 	private static final float SYNC_ACCURACY = 1e-3f;
 
 
@@ -54,8 +55,14 @@ public BangBangTrajectory2D sync(
 			final double acc
 	)
 	{
-		return new BangBangTrajectory2D()
-				.generate(s0, s1, v0, (float) vmax, (float) acc, SYNC_ACCURACY, Function.identity());
+		return new BangBangTrajectory2D().generate(
+				s0,
+				s1,
+				adaptVel(v0, vmax),
+				(float) vmax,
+				(float) acc,
+				SYNC_ACCURACY,
+				Function.identity());
 	}
 
 
@@ -67,8 +74,12 @@ public BangBangTrajectory1D singleDim(
 			final double maxAcc
 	)
 	{
-		return new BangBangTrajectory1D()
-				.generate((float) initialPos, (float) finalPos, (float) initialVel, (float) maxVel, (float) maxAcc);
+		return new BangBangTrajectory1D().generate(
+				(float) initialPos,
+				(float) finalPos,
+				(float) adaptVel(initialVel, maxVel),
+				(float) maxVel,
+				(float) maxAcc);
 	}
 
 
@@ -81,6 +92,38 @@ public BangBangTrajectory1DOrient orientation(
 	)
 	{
 		var adaptedFinalPos = initialPos + AngleMath.normalizeAngle(finalPos - AngleMath.normalizeAngle(initialPos));
-		return new BangBangTrajectory1DOrient(singleDim(initialPos, adaptedFinalPos, initialVel, maxVel, maxAcc));
+		return new BangBangTrajectory1DOrient(
+				singleDim(initialPos, adaptedFinalPos, adaptVel(initialVel, maxVel), maxVel, maxAcc));
+	}
+
+
+	/**
+	 * Limit the current speed to the max speed, if it is only slightly above velMax.
+	 * Trajectories are often generated based on the last trajectory and imprecision will lead to a
+	 * propagating error that lets the robot drive significantly faster than velMax (like >+0.2m/s with 1.5m/s)
+	 *
+	 * @param v0
+	 * @param vMax
+	 * @return
+	 */
+	private static IVector2 adaptVel(IVector2 v0, double vMax)
+	{
+		var curVelAbs = v0.getLength2();
+		if (curVelAbs > vMax && curVelAbs < vMax + MAX_VEL_TOLERANCE)
+		{
+			return v0.scaleToNew(vMax);
+		}
+		return v0;
+	}
+
+
+	private static double adaptVel(double v0, double vMax)
+	{
+		var curVelAbs = v0;
+		if (curVelAbs > vMax && curVelAbs < vMax + MAX_VEL_TOLERANCE)
+		{
+			return vMax;
+		}
+		return v0;
 	}
 }

modules/moduli-geometry/src/main/java/edu/tigers/sumatra/geometry/Geometry.java

@@ -25,6 +25,7 @@
 import edu.tigers.sumatra.math.vector.Vector2;
 import edu.tigers.sumatra.math.vector.Vector2f;
 import edu.tigers.sumatra.model.SumatraModel;
+import lombok.Getter;
 import lombok.extern.log4j.Log4j2;
 
 import java.util.ArrayList;
@@ -168,6 +169,12 @@ public class Geometry
 			comment = "Delay [s] from giving a robot command to receiving the reaction on this command from vision")
 	private static double feedbackDelay = 0.06;
 
+	@Getter
+	@Configurable(spezis = { "NICOLAI", "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "SIMULATOR" },
+			defValueSpezis = { "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "0.0125" },
+			comment = "Delay [s] between path planning trajectory state and robot feedback state")
+	private static double robotFeedbackDelay = 0.0;
+
 
 	private final IRectangle field;
 	private final IRectangle fieldWBorders;

modules/moduli-record/src/main/java/edu/tigers/sumatra/persistence/BerkeleyAsyncRecorder.java

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2009 - 2019, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
 package edu.tigers.sumatra.persistence;
@@ -124,8 +124,6 @@ private class RecordSaver implements Runnable
 		private final ScheduledExecutorService execService;
 
 
-		/**
-		 */
 		RecordSaver()
 		{
 			execService = Executors.newSingleThreadScheduledExecutor(new NamedThreadFactory("RecordSaver"));
@@ -160,6 +158,17 @@ private void close()
 			execService.execute(this::printPeriod);
 			execService.execute(db::close);
 			execService.shutdown();
+			try
+			{
+				boolean terminated = execService.awaitTermination(10, TimeUnit.SECONDS);
+				if (!terminated)
+				{
+					log.warn("Could not terminate record saver within 10s");
+				}
+			} catch (InterruptedException e)
+			{
+				Thread.currentThread().interrupt();
+			}
 		}
 	}
 }

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

@@ -378,13 +378,12 @@ private void createNewRobotTracker(final CamRobot robot, final List<FilteredVisi
 		{
 			// but it is on a different camera already, copy its state from there
 			tracker = new RobotTracker(robot, filteredBot.get());
-			robots.put(robot.getBotId(), tracker);
 		} else
 		{
 			// completely new robot on the field
 			tracker = new RobotTracker(robot);
-			robots.put(robot.getBotId(), tracker);
 		}
+		robots.put(robot.getBotId(), tracker);
 	}
 
 
@@ -446,7 +445,7 @@ private void createNewBallTracker(final CamBall cam, final FilteredVisionBall fi
 		// if this ball is not used by any other tracker we may do:
 		// - if we know the field size => only accept new balls on the field (not in boundary area)
 		// - if we don't know the field size => simply accept the ball
-		if (!fieldRectWithBoundary.isPresent() || fieldRectWithBoundary.get().isPointInShape(cam.getPos().getXYVector()))
+		if (fieldRectWithBoundary.isEmpty() || fieldRectWithBoundary.get().isPointInShape(cam.getPos().getXYVector()))
 		{
 			// if nobody else wanted this ball we create a new tracker, very gentle :)
 			BallTracker tracker;
@@ -503,7 +502,7 @@ public List<IDrawableShape> getInfoShapes()
 	{
 		List<IDrawableShape> shapes = new ArrayList<>();
 
-		if (!calibration.isPresent())
+		if (calibration.isEmpty())
 		{
 			return shapes;
 		}
@@ -604,7 +603,8 @@ public List<IDrawableShape> getBallInfoShapes()
 	{
 		List<IDrawableShape> shapes = new ArrayList<>();
 
-		for (CamBall b : ballHistory)
+		var ballHistorySnapshot = new ArrayList<>(ballHistory);
+		for (CamBall b : ballHistorySnapshot)
 		{
 			DrawableCircle pos = new DrawableCircle(b.getFlatPos(), 15, Color.BLACK);
 			pos.setFill(false);

modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/WorldInfoCollector.java

@@ -187,7 +187,9 @@ private TrackedBot createTrackedBot(final RobotInfo robotInfo,
 			robotInfo.getTrajectory().ifPresentOrElse(t -> sync.add(t, lastWFTimestamp), sync::reset);
 		}
 
-		var feedbackDelay = useInternalState ? 0.0 : Geometry.getFeedbackDelay();
+		var feedbackDelay = useInternalState
+				? Geometry.getRobotFeedbackDelay()
+				: Geometry.getFeedbackDelay();
 		var trackedState = sync.updateState(lastWFTimestamp, feedbackDelay, currentBotState);
 
 		Optional<State> bufferedState = sync.getLatestState();

modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/util/BallContactCalculator.java

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
 package edu.tigers.sumatra.wp.util;
@@ -10,8 +10,6 @@
 import edu.tigers.sumatra.geometry.Geometry;
 import edu.tigers.sumatra.ids.BotID;
 import edu.tigers.sumatra.math.botshape.BotShape;
-import edu.tigers.sumatra.math.circle.Circle;
-import edu.tigers.sumatra.math.circle.ICircle;
 import edu.tigers.sumatra.math.pose.Pose;
 import edu.tigers.sumatra.math.vector.IVector2;
 import edu.tigers.sumatra.wp.data.BallContact;
@@ -82,9 +80,7 @@ private boolean hasBallContactFromVision(final RobotInfo robotInfo, final Pose p
 
 		final IVector2 optimalBallPossPos = BotShape.getKickerCenterPos(pose.getPos(), pose.getOrientation(),
 				center2Dribbler + Geometry.getBallRadius());
-		ICircle circle = Circle.createCircle(optimalBallPossPos, ballPossTolerance);
-
-		return circle.isPointInShape(ballPos);
+		return optimalBallPossPos.distanceTo(ballPos) < ballPossTolerance;
 	}