Commit cf71e2cd authored by MarkG's avatar MarkG Committed by TIGERs GitLab
Browse files

Resolve "Respect the calculation time of path planning when applying resulting trajectory"

Closes #1615

See merge request main/Sumatra!1355

sumatra-commit: 279d1a165b94392e5c49f0d0e5c97343a5f8485b
parent caa68ce6
Pipeline #8913 passed with stage
in 4 minutes and 37 seconds
......@@ -44,6 +44,7 @@ game:
ball-placement-min-robot-distance: 0.05
distance-to-ball-in-stop: 0.5
auto-approve-goals: true
continue-from-halt: false
challenge-flags: 3
emergency-stop-grace-period: 10s
normal:
......
......@@ -17,14 +17,14 @@ public class BotState extends State
@SuppressWarnings("unused") // berkeley
private BotState()
protected BotState()
{
super();
botID = BotID.noBot();
}
private BotState(final BotID botID, final State state)
protected BotState(final BotID botID, final State state)
{
super(state.getPose(), state.getVel3());
this.botID = botID;
......@@ -37,6 +37,12 @@ public class BotState extends State
}
public static BotState zero()
{
return BotState.of(BotID.noBot(), State.of(Pose.zero(), Vector3f.zero()));
}
public static BotState nan()
{
return BotState.of(BotID.noBot(), State.of(Pose.nan(), Vector3f.nan()));
......
/*
* Copyright (c) 2009 - 2018, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.math.line.v2;
import java.util.List;
import edu.tigers.sumatra.math.vector.IVector2;
import java.util.List;
/**
* This interface represents a bounded line segment. It extends from a starting point A to an ending point B. A line
* segment has a well-defined length.
*
*
* @author Lukas Magel
*/
public interface ILineSegment extends ILineBase
{
@Override
ILineSegment copy();
/**
* Returns the starting point A of the line.
*
* @return
* The vector from which the segment extends
*
* @return The vector from which the segment extends
*/
IVector2 getStart();
/**
* Returns the ending point B of the line.
*
* @return
* The vector to which the line extends
*
* @return The vector to which the line extends
*/
IVector2 getEnd();
/**
* Returns the point located in the middle between start and end
*
*
* @return the center position of this line
*/
IVector2 getCenter();
/**
* Returns the absolute length of this line segment between A and B. The length of the line segment can be zero if
* {@code start} and {@code end} are identical.
*
* @return
* The absolute length of the line segment
* @return The absolute length of the line segment
*/
double getLength();
/**
* Converts this instance into a {@link IHalfLine}. The resulting half-line extends from the start (i.e.
* {@link #getStart()} in the direction of the {@link #directionVector()}. Please note that the resulting
* {@link ILine} instance is only valid if this instance is also valid.
*
* @return
* A half-line instance which is centered at {@link #getStart()} and points in the same direction as this
* line segment
*
* @return A half-line instance which is centered at {@link #getStart()} and points in the same direction as this
* line segment
*/
IHalfLine toHalfLine();
/**
* Step the requested absolute distance along this line segment.
* If this segment has a length {@code l} and the parameter was set to an absolute value which equals {@code l / 2}
* then this method will return the point which is located exactly in between the two support points of this line
* segment.
*
* @param stepSize
* The absolute length of the step to make along this line
* @return
* The resulting vector if this segment is valid or one of the two support points if it is not valid.
*
* @param stepSize The absolute length of the step to make along this line
* @return The resulting vector if this segment is valid or one of the two support points if it is not valid.
*/
IVector2 stepAlongLine(double stepSize);
@Override
default IVector2 supportVector()
{
return getStart();
}
/**
* Create a new line segment with a positive or negative margin.
* If the margin is negative and the line segment length is less than |margin*2|, the behavior is undefined.
*
*
* @param margin the margin to apply to both ends
* @return a new line segment
*/
ILineSegment withMargin(double margin);
/**
* Returns a list of points on a line. All points are separated by
* stepSize. Start and end of the line are always in the list.
......@@ -107,4 +101,13 @@ public interface ILineSegment extends ILineBase
* @return a list of positions on the line, all separated by stepSize
*/
List<IVector2> getSteps(double stepSize);
/**
* Smallest distance between this line segment and the given.
*
* @param line
* @return
*/
double distanceTo(final ILineSegment line);
}
/*
* Copyright (c) 2009 - 2018, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.math.line.v2;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import com.sleepycat.persist.model.Persistent;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.Vector2f;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
/**
* Implementation of the {@link ILineSegment} interface. It provides a factory method to create new instances.
*
*
* @author Lukas Magel
*/
@Persistent
......@@ -24,38 +23,39 @@ final class LineSegment extends ALine implements ILineSegment
{
private final Vector2f start;
private final Vector2f end;
/** this field is calculated on demand */
/**
* this field is calculated on demand
*/
private transient IVector2 directionVector;
/** Used by berkely */
/**
* Used by berkely
*/
@SuppressWarnings("unused")
private LineSegment()
{
start = Vector2f.ZERO_VECTOR;
end = Vector2f.ZERO_VECTOR;
}
private LineSegment(final IVector2 start, final IVector2 end)
{
this.start = Vector2f.copy(start);
this.end = Vector2f.copy(end);
}
/**
* Create a new line instance which is defined by a single {@code supportVector}, i.e. the starting point, and the
* offset from start to end. This means that the end point is calculated as follows:
* {@code supportVector + offset}.
*
* @param supportVector
* The origin of this line segment
* @param offset
* The offset between start and end of this line {@code end - start}
* @return
* A new line instance
*
* @param supportVector The origin of this line segment
* @param offset The offset between start and end of this line {@code end - start}
* @return A new line instance
* @throws IllegalArgumentException If the {@code offset} has a length of zero
*/
public static ILineSegment fromOffset(final IVector2 supportVector, final IVector2 offset)
......@@ -63,33 +63,30 @@ final class LineSegment extends ALine implements ILineSegment
IVector2 end = supportVector.addNew(offset);
return fromPoints(supportVector, end);
}
/**
* Create a new line segment which spans from the specified point {@code start} to the second point {@code end}.
*
* @param start
* The point from which the line segment extends
* @param end
* The point which the line segment extends to
* @return
* A new line segment instance
*
* @param start The point from which the line segment extends
* @param end The point which the line segment extends to
* @return A new line segment instance
* @throws IllegalArgumentException If the points {@code start} and {@code end} are identical. Please perform a check
* in your code before you call this method!
* in your code before you call this method!
*/
public static ILineSegment fromPoints(final IVector2 start, final IVector2 end)
{
return new LineSegment(start, end);
}
@Override
public boolean isValid()
{
return !start.equals(end);
}
@Override
public final boolean equals(final Object other)
{
......@@ -101,13 +98,13 @@ final class LineSegment extends ALine implements ILineSegment
{
return false;
}
final ILineSegment that = (ILineSegment) other;
return getStart().equals(that.getStart()) && getEnd().equals(that.getEnd());
}
@Override
public final int hashCode()
{
......@@ -115,36 +112,36 @@ final class LineSegment extends ALine implements ILineSegment
result = (31 * result) + getEnd().hashCode();
return result;
}
@Override
public IVector2 getStart()
{
return start;
}
@Override
public IVector2 getEnd()
{
return end;
}
@Override
public IVector2 getCenter()
{
return start.addNew(directionVector().multiplyNew(0.5));
}
@Override
public double getLength()
{
return directionVector().getLength();
}
@Override
public IVector2 directionVector()
{
......@@ -154,50 +151,50 @@ final class LineSegment extends ALine implements ILineSegment
}
return directionVector;
}
@Override
public IVector2 closestPointOnLine(final IVector2 point)
{
return LineMath.closestPointOnLineSegment(this, point);
}
@Override
public ILine toLine()
{
return Line.createNewWithoutCopy(getStart(), directionVector());
}
@Override
public IHalfLine toHalfLine()
{
return HalfLine.createNewWithoutCopy(getStart(), directionVector());
}
@Override
public ILineSegment copy()
{
return fromPoints(getStart(), getEnd());
}
@Override
public Optional<IVector2> intersectLine(final ILine line)
{
return line.intersectSegment(this);
}
@Override
public Optional<IVector2> intersectHalfLine(final IHalfLine halfLine)
{
return halfLine.intersectSegment(this);
}
@Override
public Optional<IVector2> intersectSegment(final ILineSegment other)
{
......@@ -207,8 +204,8 @@ final class LineSegment extends ALine implements ILineSegment
}
return Optional.empty();
}
@Override
public IVector2 stepAlongLine(final double stepSize)
{
......@@ -218,8 +215,8 @@ final class LineSegment extends ALine implements ILineSegment
}
return LineMath.stepAlongLine(getStart(), getEnd(), stepSize);
}
@Override
public ILineSegment withMargin(final double margin)
{
......@@ -227,15 +224,15 @@ final class LineSegment extends ALine implements ILineSegment
IVector2 newEnd = LineMath.stepAlongLine(getEnd(), getStart(), -margin);
return Lines.segmentFromPoints(newStart, newEnd);
}
@Override
public String toString()
{
return "LineSegment(" + start + " -> " + end + ")";
}
@Override
public List<IVector2> getSteps(final double stepSize)
{
......@@ -248,4 +245,20 @@ final class LineSegment extends ALine implements ILineSegment
steps.add(stepAlongLine(len));
return steps;
}
@Override
public double distanceTo(final ILineSegment line)
{
if (intersectSegment(line).isPresent())
{
return 0;
}
return Math.sqrt(List.of(
line.distanceToSqr(this.getStart()),
line.distanceToSqr(this.getEnd()),
this.distanceToSqr(line.getStart()),
this.distanceToSqr(line.getEnd())
).stream().mapToDouble(d -> d).min().orElseThrow());
}
}
......@@ -6,7 +6,6 @@ package edu.tigers.sumatra.math.line.v2;
import edu.tigers.sumatra.math.SumatraMath;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.math.vector.Vector2f;
import nl.jqno.equalsverifier.EqualsVerifier;
import nl.jqno.equalsverifier.Warning;
......@@ -14,6 +13,8 @@ import org.junit.Test;
import java.util.Optional;
import static edu.tigers.sumatra.math.line.v2.Lines.segmentFromPoints;
import static edu.tigers.sumatra.math.vector.Vector2.fromXY;
import static org.assertj.core.api.Assertions.assertThat;
import static org.assertj.core.api.Assertions.within;
......@@ -30,8 +31,8 @@ public class LineSegmentTest extends AbstractLineTest
@Test
public void testFromPoints()
{
IVector2 start = Vector2.fromXY(1, 2);
IVector2 end = Vector2.fromXY(21, 42);
IVector2 start = fromXY(1, 2);
IVector2 end = fromXY(21, 42);
IVector2 dV = end.subtractNew(start);
ILineSegment segment = LineSegment.fromPoints(start, end);
......@@ -47,7 +48,7 @@ public class LineSegmentTest extends AbstractLineTest
@Test
public void testFromIdenticalPoints()
{
IVector2 point = Vector2.fromXY(21, 42);
IVector2 point = fromXY(21, 42);
ILineSegment segment = LineSegment.fromPoints(point, point);
assertThat(segment.getStart()).isEqualTo(point);
......@@ -59,8 +60,8 @@ public class LineSegmentTest extends AbstractLineTest
@Test
public void testFromDisplacement()
{
IVector2 start = Vector2.fromXY(-10, 20);
IVector2 displacement = Vector2.fromXY(1, 5);
IVector2 start = fromXY(-10, 20);
IVector2 displacement = fromXY(1, 5);
ILineSegment segment = LineSegment.fromOffset(start, displacement);
......@@ -74,7 +75,7 @@ public class LineSegmentTest extends AbstractLineTest
@Test
public void testFromZeroDisplacement()
{
IVector2 start = Vector2.fromXY(4, 5);
IVector2 start = fromXY(4, 5);
IVector2 displacement = Vector2f.ZERO_VECTOR;
ILineSegment segment = LineSegment.fromOffset(start, displacement);
......@@ -88,7 +89,7 @@ public class LineSegmentTest extends AbstractLineTest
@Test
public void testIsValid()
{
IVector2 start = Vector2.fromXY(10, -20);
IVector2 start = fromXY(10, -20);
IVector2 end = Vector2f.ZERO_VECTOR;
ILineSegment properLine = LineSegment.fromPoints(start, end);
......@@ -114,8 +115,8 @@ public class LineSegmentTest extends AbstractLineTest
{
double radAngle = Math.toRadians(degAngle);
IVector2 start = Vector2.fromXY(0, 1);
IVector2 end = Vector2f.X_AXIS.turnToNew(radAngle).add(Vector2.fromXY(0, 1));
IVector2 start = fromXY(0, 1);
IVector2 end = Vector2f.X_AXIS.turnToNew(radAngle).add(fromXY(0, 1));
ILineSegment segment = LineSegment.fromPoints(start, end);
ILineSegment invalidSegment = LineSegment.fromPoints(start, start);
......@@ -143,6 +144,38 @@ public class LineSegmentTest extends AbstractLineTest
}
@Test
public void testDistanceToLineSegment()
{
// two zero-lines
assertThat(segmentFromPoints(fromXY(0, 0), fromXY(0, 0)).distanceTo(
segmentFromPoints(fromXY(0, 0), fromXY(0, 0))))
.isCloseTo(0, within(1e-10));