Commit 2369499a authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

Robo cup

See merge request main/Sumatra!1155

sumatra-commit: 2ce9f8dccde73e3a4d97b2da485c02ff4e17bb38
parent 6de1885d
Pipeline #8570 passed with stage
in 2 minutes and 31 seconds
......@@ -55,7 +55,7 @@ public class VisionFilterImpl extends AVisionFilter
@SuppressWarnings("unused")
private static final Logger log = Logger.getLogger(VisionFilterImpl.class.getName());
private static final long CLOCK_DT = 10_000_000;
private static final long CLOCK_DT = 12_500_000;
private final BallFilterPreprocessor ballFilterPreprocessor = new BallFilterPreprocessor();
private final BallFilter ballFilter = new BallFilter();
......
......@@ -16,7 +16,7 @@ public enum EVisionFilterShapesLayer implements IShapeLayer
QUALITY_SHAPES("Quality Inspector"),
CAM_INFO_SHAPES("Cam Info"),
VIEWPORT_SHAPES("Viewports"),
ROBOT_TRACKER_SHAPES("Robot Trackers", false, EShapeLayerPersistenceType.NEVER_PERSIST),
ROBOT_TRACKER_SHAPES("Robot Trackers", false, EShapeLayerPersistenceType.ALWAYS_PERSIST),
ROBOT_QUALITY_INSPECTOR("Robot Quality Inspector"),
BALL_TRACKER_SHAPES_IMPORTANT("Ball Trackers"),
BALL_TRACKER_SHAPES("Ball Trackers Debug", false, EShapeLayerPersistenceType.NEVER_PERSIST),
......
......@@ -20,33 +20,33 @@ public class BallLeftFieldCalculator
{
@Configurable(comment = "Time [s] to wait before using ball positions, invaliding all positions just before a chip kick", defValue = "0.3")
private static double maxTimeToDetectChipKick = 0.3;
@Configurable(comment = "Time [s] between two ball positions to pass before comparing them in order to check if the ball left the field")
@Configurable(comment = "Time [s] between two ball positions to pass before comparing them in order to check if the ball left the field", defValue = "0.05")
private static double minComparisonTimeSpan = 0.05;
static
{
ConfigRegistration.registerClass("wp", BallLeftFieldCalculator.class);
}
private final LinkedList<TimedPosition> ballPosBuffer = new LinkedList<>();
private TimedPosition lastBallLeftFieldPosition = null;
private boolean ballInsideField = true;
private long chipStartTime;
public BallLeftFieldPosition process(final SimpleWorldFrame wFrame)
{
reduceBallPosBuffer(wFrame.getTimestamp());
addToBallPosBuffer(wFrame);
removeFirstChippedBallPositions(wFrame);
updateDetection(wFrame);
return lastBallLeftFieldPosition == null ? null
: new BallLeftFieldPosition(lastBallLeftFieldPosition, currentType());
}
private BallLeftFieldPosition.EBallLeftFieldType currentType()
{
if (Math.abs(lastBallLeftFieldPosition.getPos().x()) < Geometry.getFieldLength() / 2)
......@@ -54,7 +54,7 @@ public class BallLeftFieldCalculator
return BallLeftFieldPosition.EBallLeftFieldType.TOUCH_LINE;
}
boolean overGoal = lastBallLeftFieldPosition.getPos3().z() > Geometry.getGoalHeight();
for (Goal goal : Geometry.getGoals())
{
if (goal.getLineSegment().isPointOnLine(lastBallLeftFieldPosition.getPos()))
......@@ -65,19 +65,19 @@ public class BallLeftFieldCalculator
}
return BallLeftFieldPosition.EBallLeftFieldType.GOAL_LINE;
}
private void updateDetection(final SimpleWorldFrame wFrame)
{
TimedPosition prePos = ballPosBuffer.peekLast();
TimedPosition postPos = firstValidBallPos(wFrame.getTimestamp());
if (lastBallLeftFieldPosition != null
&& Math.abs(wFrame.getTimestamp() - lastBallLeftFieldPosition.getTimestamp()) / 1e9 > 2)
{
lastBallLeftFieldPosition = null;
}
if (prePos != null && postPos != null
&& (postPos.getTimestamp() - prePos.getTimestamp()) / 1e9 >= minComparisonTimeSpan)
{
......@@ -96,8 +96,8 @@ public class BallLeftFieldCalculator
}
}
}
private TimedPosition firstValidBallPos(final long timestamp)
{
for (TimedPosition timedPosition : ballPosBuffer)
......@@ -109,8 +109,8 @@ public class BallLeftFieldCalculator
}
return null;
}
private void removeFirstChippedBallPositions(final SimpleWorldFrame wFrame)
{
if (wFrame.getBall().isChipped())
......@@ -129,15 +129,15 @@ public class BallLeftFieldCalculator
chipStartTime = 0;
}
}
private void addToBallPosBuffer(final SimpleWorldFrame frame)
{
TimedPosition pos = new TimedPosition(frame.getTimestamp(), frame.getBall().getPos3());
ballPosBuffer.offerFirst(pos);
}
private void reduceBallPosBuffer(final long currentTimestamp)
{
ballPosBuffer.removeIf(t -> t.getTimestamp() > currentTimestamp);
......@@ -153,8 +153,8 @@ public class BallLeftFieldCalculator
}
}
}
public void reset()
{
lastBallLeftFieldPosition = null;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment