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 ...@@ -55,7 +55,7 @@ public class VisionFilterImpl extends AVisionFilter
@SuppressWarnings("unused") @SuppressWarnings("unused")
private static final Logger log = Logger.getLogger(VisionFilterImpl.class.getName()); 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 BallFilterPreprocessor ballFilterPreprocessor = new BallFilterPreprocessor();
private final BallFilter ballFilter = new BallFilter(); private final BallFilter ballFilter = new BallFilter();
......
...@@ -16,7 +16,7 @@ public enum EVisionFilterShapesLayer implements IShapeLayer ...@@ -16,7 +16,7 @@ public enum EVisionFilterShapesLayer implements IShapeLayer
QUALITY_SHAPES("Quality Inspector"), QUALITY_SHAPES("Quality Inspector"),
CAM_INFO_SHAPES("Cam Info"), CAM_INFO_SHAPES("Cam Info"),
VIEWPORT_SHAPES("Viewports"), 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"), ROBOT_QUALITY_INSPECTOR("Robot Quality Inspector"),
BALL_TRACKER_SHAPES_IMPORTANT("Ball Trackers"), BALL_TRACKER_SHAPES_IMPORTANT("Ball Trackers"),
BALL_TRACKER_SHAPES("Ball Trackers Debug", false, EShapeLayerPersistenceType.NEVER_PERSIST), BALL_TRACKER_SHAPES("Ball Trackers Debug", false, EShapeLayerPersistenceType.NEVER_PERSIST),
......
...@@ -20,33 +20,33 @@ public class BallLeftFieldCalculator ...@@ -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") @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; 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; private static double minComparisonTimeSpan = 0.05;
static static
{ {
ConfigRegistration.registerClass("wp", BallLeftFieldCalculator.class); ConfigRegistration.registerClass("wp", BallLeftFieldCalculator.class);
} }
private final LinkedList<TimedPosition> ballPosBuffer = new LinkedList<>(); private final LinkedList<TimedPosition> ballPosBuffer = new LinkedList<>();
private TimedPosition lastBallLeftFieldPosition = null; private TimedPosition lastBallLeftFieldPosition = null;
private boolean ballInsideField = true; private boolean ballInsideField = true;
private long chipStartTime; private long chipStartTime;
public BallLeftFieldPosition process(final SimpleWorldFrame wFrame) public BallLeftFieldPosition process(final SimpleWorldFrame wFrame)
{ {
reduceBallPosBuffer(wFrame.getTimestamp()); reduceBallPosBuffer(wFrame.getTimestamp());
addToBallPosBuffer(wFrame); addToBallPosBuffer(wFrame);
removeFirstChippedBallPositions(wFrame); removeFirstChippedBallPositions(wFrame);
updateDetection(wFrame); updateDetection(wFrame);
return lastBallLeftFieldPosition == null ? null return lastBallLeftFieldPosition == null ? null
: new BallLeftFieldPosition(lastBallLeftFieldPosition, currentType()); : new BallLeftFieldPosition(lastBallLeftFieldPosition, currentType());
} }
private BallLeftFieldPosition.EBallLeftFieldType currentType() private BallLeftFieldPosition.EBallLeftFieldType currentType()
{ {
if (Math.abs(lastBallLeftFieldPosition.getPos().x()) < Geometry.getFieldLength() / 2) if (Math.abs(lastBallLeftFieldPosition.getPos().x()) < Geometry.getFieldLength() / 2)
...@@ -54,7 +54,7 @@ public class BallLeftFieldCalculator ...@@ -54,7 +54,7 @@ public class BallLeftFieldCalculator
return BallLeftFieldPosition.EBallLeftFieldType.TOUCH_LINE; return BallLeftFieldPosition.EBallLeftFieldType.TOUCH_LINE;
} }
boolean overGoal = lastBallLeftFieldPosition.getPos3().z() > Geometry.getGoalHeight(); boolean overGoal = lastBallLeftFieldPosition.getPos3().z() > Geometry.getGoalHeight();
for (Goal goal : Geometry.getGoals()) for (Goal goal : Geometry.getGoals())
{ {
if (goal.getLineSegment().isPointOnLine(lastBallLeftFieldPosition.getPos())) if (goal.getLineSegment().isPointOnLine(lastBallLeftFieldPosition.getPos()))
...@@ -65,19 +65,19 @@ public class BallLeftFieldCalculator ...@@ -65,19 +65,19 @@ public class BallLeftFieldCalculator
} }
return BallLeftFieldPosition.EBallLeftFieldType.GOAL_LINE; return BallLeftFieldPosition.EBallLeftFieldType.GOAL_LINE;
} }
private void updateDetection(final SimpleWorldFrame wFrame) private void updateDetection(final SimpleWorldFrame wFrame)
{ {
TimedPosition prePos = ballPosBuffer.peekLast(); TimedPosition prePos = ballPosBuffer.peekLast();
TimedPosition postPos = firstValidBallPos(wFrame.getTimestamp()); TimedPosition postPos = firstValidBallPos(wFrame.getTimestamp());
if (lastBallLeftFieldPosition != null if (lastBallLeftFieldPosition != null
&& Math.abs(wFrame.getTimestamp() - lastBallLeftFieldPosition.getTimestamp()) / 1e9 > 2) && Math.abs(wFrame.getTimestamp() - lastBallLeftFieldPosition.getTimestamp()) / 1e9 > 2)
{ {
lastBallLeftFieldPosition = null; lastBallLeftFieldPosition = null;
} }
if (prePos != null && postPos != null if (prePos != null && postPos != null
&& (postPos.getTimestamp() - prePos.getTimestamp()) / 1e9 >= minComparisonTimeSpan) && (postPos.getTimestamp() - prePos.getTimestamp()) / 1e9 >= minComparisonTimeSpan)
{ {
...@@ -96,8 +96,8 @@ public class BallLeftFieldCalculator ...@@ -96,8 +96,8 @@ public class BallLeftFieldCalculator
} }
} }
} }
private TimedPosition firstValidBallPos(final long timestamp) private TimedPosition firstValidBallPos(final long timestamp)
{ {
for (TimedPosition timedPosition : ballPosBuffer) for (TimedPosition timedPosition : ballPosBuffer)
...@@ -109,8 +109,8 @@ public class BallLeftFieldCalculator ...@@ -109,8 +109,8 @@ public class BallLeftFieldCalculator
} }
return null; return null;
} }
private void removeFirstChippedBallPositions(final SimpleWorldFrame wFrame) private void removeFirstChippedBallPositions(final SimpleWorldFrame wFrame)
{ {
if (wFrame.getBall().isChipped()) if (wFrame.getBall().isChipped())
...@@ -129,15 +129,15 @@ public class BallLeftFieldCalculator ...@@ -129,15 +129,15 @@ public class BallLeftFieldCalculator
chipStartTime = 0; chipStartTime = 0;
} }
} }
private void addToBallPosBuffer(final SimpleWorldFrame frame) private void addToBallPosBuffer(final SimpleWorldFrame frame)
{ {
TimedPosition pos = new TimedPosition(frame.getTimestamp(), frame.getBall().getPos3()); TimedPosition pos = new TimedPosition(frame.getTimestamp(), frame.getBall().getPos3());
ballPosBuffer.offerFirst(pos); ballPosBuffer.offerFirst(pos);
} }
private void reduceBallPosBuffer(final long currentTimestamp) private void reduceBallPosBuffer(final long currentTimestamp)
{ {
ballPosBuffer.removeIf(t -> t.getTimestamp() > currentTimestamp); ballPosBuffer.removeIf(t -> t.getTimestamp() > currentTimestamp);
...@@ -153,8 +153,8 @@ public class BallLeftFieldCalculator ...@@ -153,8 +153,8 @@ public class BallLeftFieldCalculator
} }
} }
} }
public void reset() public void reset()
{ {
lastBallLeftFieldPosition = null; 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