Resolve "Ignore obstacles in pathplanning that we drive away from"

Closes #1989

See merge request main/Sumatra!1883

sumatra-commit: 45132b0711fcc22f27e77083d5d9b3598faeb004

modules/common-math/src/main/java/edu/tigers/sumatra/math/AcceptablePosFinder.java

@@ -11,6 +11,7 @@
 
 import java.util.ArrayList;
 import java.util.Comparator;
+import java.util.List;
 import java.util.Optional;
 import java.util.function.Predicate;
 
@@ -22,53 +23,28 @@ public class AcceptablePosFinder
 	private final int maxIterations;
 
 
-	public Optional<IVector2> findAcceptablePos(IVector2 start, Predicate<IVector2> check)
+	public AcceptablePos findAcceptablePos(
+			IVector2 start,
+			Predicate<IVector2> check,
+			IVector2 reference
+	)
 	{
 		if (check.test(start))
 		{
-			return Optional.of(start);
+			return new AcceptablePos(Optional.of(start), List.of(), List.of());
 		}
 
 		int iteration = 0;
+		List<IVector2> possibleSolutions = new ArrayList<>();
+		List<IVector2> triedPositions = new ArrayList<>();
 		for (double radius = distanceStepSize; radius < 10_000; radius += distanceStepSize)
 		{
 			ICircle circle = Circle.createCircle(start, radius);
 			double perimeterLength = circle.getPerimeterLength();
 			for (double c = 0; c < perimeterLength; c += distanceStepSize)
 			{
 				IVector2 point = circle.stepAlongPath(c);
-				if (check.test(point))
-				{
-					return Optional.of(point);
-				}
-				iteration++;
-				if (iteration >= maxIterations)
-				{
-					return Optional.empty();
-				}
-			}
-		}
-		throw new IllegalStateException("No acceptable position found after " + iteration + " iterations.");
-	}
-
-
-	public Optional<IVector2> findAcceptablePosWithReference(IVector2 start, Predicate<IVector2> check,
-			IVector2 reference)
-	{
-		if (check.test(start))
-		{
-			return Optional.of(start);
-		}
-
-		int iteration = 0;
-		ArrayList<IVector2> possibleSolutions = new ArrayList<>();
-		for (double radius = distanceStepSize; radius < 10_000; radius += distanceStepSize)
-		{
-			ICircle circle = Circle.createCircle(start, radius);
-			double perimeterLength = circle.getPerimeterLength();
-			for (double c = 0; c < perimeterLength; c += distanceStepSize)
-			{
-				IVector2 point = circle.stepAlongPath(c);
+				triedPositions.add(point);
 				if (check.test(point))
 				{
 					possibleSolutions.add(point);
@@ -81,9 +57,22 @@ public Optional<IVector2> findAcceptablePosWithReference(IVector2 start, Predica
 			}
 			if (iteration >= maxIterations || !possibleSolutions.isEmpty())
 			{
-				return possibleSolutions.stream().min(Comparator.comparingDouble(reference::distanceToSqr));
+				return new AcceptablePos(
+						possibleSolutions.stream().min(Comparator.comparingDouble(reference::distanceToSqr)),
+						triedPositions,
+						possibleSolutions
+				);
 			}
 		}
 		throw new IllegalStateException("No acceptable position found after " + iteration + " iterations.");
 	}
+
+
+	public record AcceptablePos(
+			Optional<IVector2> pos,
+			List<IVector2> triedPositions,
+			List<IVector2> possibleSolutions
+	)
+	{
+	}
 }

modules/common/src/main/java/edu/tigers/sumatra/movingrobot/MovingRobotImpl.java → modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AMovingRobot.java

@@ -9,24 +9,23 @@
 import edu.tigers.sumatra.math.tube.ITube;
 import edu.tigers.sumatra.math.tube.Tube;
 import edu.tigers.sumatra.math.vector.IVector2;
-import lombok.AccessLevel;
 import lombok.RequiredArgsConstructor;
 
 
-@RequiredArgsConstructor(access = AccessLevel.PACKAGE)
-public class MovingRobotImpl implements IMovingRobot
+@RequiredArgsConstructor
+public abstract class AMovingRobot implements IMovingRobot
 {
 	private final IVector2 pos;
 	private final IVector2 dir;
 	private final double radius;
-	private final double speed;
-	private final MovingOffsetFunction offsetFunction;
+	protected final double speed;
+	protected final double reactionTime;
 
 
 	@Override
 	public ICircle getMovingHorizon(final double tHorizon, double tAdditionalReaction)
 	{
-		var p = offsetFunction.forwardBackwardOffset(tHorizon, tAdditionalReaction);
+		var p = forwardBackwardOffset(tHorizon, tAdditionalReaction);
 
 		double dynamicRadius = Math.abs(p.forward() - p.backward()) / 2;
 		IVector2 center = pos.addNew(dir.multiplyNew(p.backward() + dynamicRadius));
@@ -37,7 +36,7 @@ public ICircle getMovingHorizon(final double tHorizon, double tAdditionalReactio
 	@Override
 	public ITube getMovingHorizonTube(final double tHorizon, double tAdditionalReaction)
 	{
-		var p = offsetFunction.forwardBackwardOffset(tHorizon, tAdditionalReaction);
+		var p = forwardBackwardOffset(tHorizon, tAdditionalReaction);
 
 		return Tube.create(
 				dir.multiplyNew(p.backward()).add(pos),
@@ -61,4 +60,11 @@ public double getSpeed()
 	}
 
 
+	/**
+	 * @param tHorizon            time horizon in seconds
+	 * @param tAdditionalReaction additional reaction time in seconds
+	 * @return moving offsets
+	 */
+	abstract MovingOffsets forwardBackwardOffset(double tHorizon, double tAdditionalReaction);
+
 }

modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AcceleratingRobot.java

@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2009 - 2024, DHBW Mannheim - TIGERs Mannheim
+ */
+
+package edu.tigers.sumatra.movingrobot;
+
+import edu.tigers.sumatra.math.vector.IVector2;
+import edu.tigers.sumatra.trajectory.ITrajectory;
+
+
+/**
+ * A moving robot that is accelerating to the maximum velocity and keeps driving at this velocity.
+ */
+public class AcceleratingRobot extends AMovingRobot
+{
+	private final ITrajectory<Double> trajectoryForward;
+	private final ITrajectory<Double> trajectoryBackward;
+
+
+	AcceleratingRobot(
+			IVector2 pos,
+			IVector2 vel,
+			double radius,
+			double reactionTime,
+			ITrajectory<Double> trajectoryForward,
+			ITrajectory<Double> trajectoryBackward
+	)
+	{
+		super(pos, vel.normalizeNew(), radius, vel.getLength2(), reactionTime);
+		this.trajectoryForward = trajectoryForward;
+		this.trajectoryBackward = trajectoryBackward;
+	}
+
+
+	@Override
+	MovingOffsets forwardBackwardOffset(double tHorizon, double tAdditionalReaction)
+	{
+		double tReaction = Math.min(reactionTime + tAdditionalReaction, tHorizon);
+
+		double distReaction = speed * tReaction * 1000;
+		return new MovingOffsets(
+				distReaction + trajectoryForward.getPositionMM(tHorizon - tReaction),
+				distReaction + trajectoryBackward.getPositionMM(tHorizon - tReaction)
+		);
+	}
+}

modules/common/src/main/java/edu/tigers/sumatra/movingrobot/MovingRobotFactory.java

@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2009 - 2024, DHBW Mannheim - TIGERs Mannheim
+ */
+
+package edu.tigers.sumatra.movingrobot;
+
+import edu.tigers.sumatra.math.vector.IVector2;
+import edu.tigers.sumatra.trajectory.BangBangTrajectoryFactory;
+import lombok.AccessLevel;
+import lombok.NoArgsConstructor;
+
+
+/**
+ * A factory for moving robots.
+ */
+@NoArgsConstructor(access = AccessLevel.PRIVATE)
+public class MovingRobotFactory
+{
+	private static final double LARGE_DISTANCE = 20;
+	private static final BangBangTrajectoryFactory TRAJECTORY_FACTORY = new BangBangTrajectoryFactory();
+
+
+	public static IMovingRobot acceleratingRobot(
+			IVector2 pos,
+			IVector2 vel,
+			double vMax,
+			double acc,
+			double radius,
+			double reactionTime
+	)
+	{
+		double speed = vel.getLength2();
+		return new AcceleratingRobot(
+				pos,
+				vel,
+				radius,
+				reactionTime,
+				TRAJECTORY_FACTORY.single(0, LARGE_DISTANCE, speed, vMax, acc),
+				TRAJECTORY_FACTORY.single(0, -LARGE_DISTANCE, speed, vMax, acc)
+		);
+	}
+
+
+	public static IMovingRobot stoppingRobot(
+			IVector2 pos,
+			IVector2 vel,
+			double vLimit,
+			double aLimit,
+			double brkLimit,
+			double radius,
+			double reactionTime
+	)
+	{
+		return new StoppingRobot(
+				pos,
+				vel,
+				radius,
+				reactionTime,
+				vLimit,
+				aLimit,
+				brkLimit
+		);
+	}
+}