Commit a65ffff3 authored by AndreR's avatar AndreR Committed by TIGERs GitLab
Browse files

Resolve "Fix autoRef integration test failures"

Closes #1637

See merge request main/Sumatra!1380

sumatra-commit: 27841becc6340e0169551287410d51a8865d86cc
parent 22c8f424
Pipeline #16670 passed with stage
in 4 minutes and 9 seconds
......@@ -36,8 +36,7 @@ public class BallSpeedModel
lastBallSpeed = wFrameWrapper.getSimpleWorldFrame().getBall().getVel3().getLength();
lastEstimatedBallSpeed = wFrameWrapper.getSimpleWorldFrame().getKickFitState()
.map(BallKickFitState::getKickVel)
.map(IVector3::getLength).orElse(0.0)
/ 1000.0;
.map(IVector3::getLength).orElse(0.0);
lastState = curState;
......
......@@ -40,7 +40,8 @@ public class BotBallContactAutoRefCalc implements IAutoRefereeCalc
if (currentlyTouchingBots.isEmpty())
{
var newKickEvent = frame.getWorldFrame().getKickEvent().filter(k -> !k.equals(lastKickEvent));
var newKickEvent = frame.getWorldFrame().getKickEvent()
.filter(k -> lastKickEvent == null || k.getTimestamp() != lastKickEvent.getTimestamp());
lastBotTouchedBall = newKickEvent
.map(k -> new BotPosition(k.getTimestamp(), k.getPosition(), k.getKickingBot()))
.map(List::of)
......
......@@ -57,6 +57,7 @@ public class PassDetectionAutoRefCalc implements IAutoRefereeCalc
private Pass lastPass;
private IKickEvent lastConsumedKickEvent;
@Override
public void process(AutoRefFrame frame)
{
......@@ -143,7 +144,7 @@ public class PassDetectionAutoRefCalc implements IAutoRefereeCalc
var direction = target.subtractNew(source).getAngle();
var directionChange = getDirectionChange(direction);
var initialBallSpeed = lastKickFitState.getAbsoluteKickSpeed() / 1000;
var initialBallSpeed = lastKickFitState.getAbsoluteKickSpeed();
var valid = (directionChange == null || directionChange > minDirectionChange)
&& distance > minDistance
&& initialBallSpeed <= RuleConstraints.getMaxBallSpeed();
......@@ -176,7 +177,8 @@ public class PassDetectionAutoRefCalc implements IAutoRefereeCalc
private boolean passFinished(AutoRefFrame frame)
{
if (lastKickEvent == null || lastKickEvent.equals(lastConsumedKickEvent))
if (lastKickEvent == null ||
(lastConsumedKickEvent != null && lastKickEvent.getTimestamp() == lastConsumedKickEvent.getTimestamp()))
{
return false;
}
......@@ -193,7 +195,7 @@ public class PassDetectionAutoRefCalc implements IAutoRefereeCalc
// kick event reset - ball might be received and stopped by receiver
return true;
}
if (!newKickEvent.equals(lastKickEvent))
if (newKickEvent.getTimestamp() != lastKickEvent.getTimestamp())
{
// kick event changed
return true;
......
......@@ -26,8 +26,8 @@ public class BallSpeedingDetector extends AGameEventDetector
@Configurable(comment = "Max waiting time [s]", defValue = "0.8")
private static double maxWaitingTime = 0.8;
@Configurable(comment = "Min waiting time [s]", defValue = "0.2")
private static double minWaitingTime = 0.2;
@Configurable(comment = "Min waiting time [s]", defValue = "0.1")
private static double minWaitingTime = 0.1;
private IKickEvent lastReportedKickEvent;
......@@ -42,46 +42,37 @@ public class BallSpeedingDetector extends AGameEventDetector
@Override
public Optional<IGameEvent> doUpdate()
{
if (frame.getPreviousFrame().getWorldFrame().getKickFitState().isEmpty()
|| frame.getWorldFrame().getKickEvent().isEmpty())
Optional<BallKickFitState> lastKickFitState = frame.getPreviousFrame().getWorldFrame().getKickFitState();
Optional<IKickEvent> lastKickEvent = frame.getPreviousFrame().getWorldFrame().getKickEvent();
if (lastKickFitState.isEmpty() || lastKickEvent.isEmpty())
{
return Optional.empty();
}
IKickEvent currentKickEvent = frame.getWorldFrame().getKickEvent().get();
if (kickEventHasBeenReported(currentKickEvent))
Optional<IKickEvent> currentKickEvent = frame.getWorldFrame().getKickEvent();
if (kickEventHasBeenReported(currentKickEvent.orElse(lastKickEvent.get())))
{
return Optional.empty();
}
// take the last kickFitState, because if the ball hits another robot, we still want to have the original ball
// velocity
BallKickFitState lastKickFitState = frame.getPreviousFrame().getWorldFrame().getKickFitState().get();
double kickSpeed = lastKickFitState.getKickVel().getLength() / 1000.;
if (isKickTooFast(kickSpeed)
&& kickEstimateIsReady(currentKickEvent, lastKickFitState))
double kickSpeed = lastKickFitState.get().getKickVel().getLength();
if (isKickTooFast(kickSpeed) && kickEstimateIsReady(lastKickFitState.get()))
{
lastReportedKickEvent = currentKickEvent;
IGameEvent violation;
if (lastKickFitState.getKickPos().distanceTo(currentKickEvent.getPosition()) < 1000)
{
violation = createViolation(currentKickEvent.getKickingBot(), kickSpeed);
} else if (frame.getBotsLastTouchedBall().size() == 1)
{
violation = createViolation(frame.getBotsLastTouchedBall().get(0).getBotID(), kickSpeed);
} else
lastReportedKickEvent = lastKickEvent.get();
if (lastReportedKickEvent.getKickingBot().isBot())
{
// could not determine the violator
return Optional.empty();
return Optional.of(createViolation(lastReportedKickEvent.getKickingBot(), kickSpeed));
}
return Optional.of(violation);
// could not determine the violator
return Optional.empty();
}
if (ballTouchedAnotherRobot(currentKickEvent))
if (kickFinished())
{
// reset detection for this kick event
lastReportedKickEvent = currentKickEvent;
lastReportedKickEvent = lastKickEvent.get();
}
return Optional.empty();
......@@ -94,17 +85,21 @@ public class BallSpeedingDetector extends AGameEventDetector
}
private boolean kickEstimateIsReady(final IKickEvent currentKickEvent, BallKickFitState lastKickFitState)
private boolean kickEstimateIsReady(BallKickFitState lastKickFitState)
{
double kickEstimateAge = (frame.getTimestamp() - currentKickEvent.getTimestamp()) / 1e9;
double kickFitEstimateAge = (frame.getTimestamp() - lastKickFitState.getKickTimestamp()) / 1e9;
boolean kickEstimateIsReady = kickEstimateAge > maxWaitingTime;
boolean kickEstimateAged = kickFitEstimateAge > minWaitingTime;
boolean kickEstimateIsReady = kickFitEstimateAge > maxWaitingTime;
// either time is up, or ball has left the field, or ball touched another bot
return kickEstimateAged &&
(kickEstimateIsReady || ballIsNotInsideField() || ballTouchedAnotherRobot(currentKickEvent));
return kickEstimateAged && (kickEstimateIsReady || kickFinished());
}
private boolean kickFinished()
{
var kickEvent = frame.getWorldFrame().getKickEvent();
return kickEvent.isEmpty() || ballIsNotInsideField() || ballTouchedAnotherRobot(kickEvent.get());
}
......
......@@ -70,7 +70,7 @@ public class TrackerPacketGenerator
final SslVisionDetectionTracked.KickedBall.Builder kickedBall = SslVisionDetectionTracked.KickedBall
.newBuilder()
.setPos(buildVector2(ballKickFitState.getKickPos().multiplyNew(1e-3)))
.setVel(buildVector3(ballKickFitState.getKickVel().multiplyNew(1e-3)))
.setVel(buildVector3(ballKickFitState.getKickVel()))
.setStartTimestamp(buildTimestamp(ballKickFitState.getKickTimestamp()))
.setStopTimestamp(buildTimestamp(stopTimestamp))
.setStopPos(buildVector2(stopPos.multiplyNew(1e-3)));
......
......@@ -27,7 +27,7 @@ public class BallKickFitState implements IMirrorable<BallKickFitState>
IVector2 kickPos;
/**
* Kick velocity [mm/s]
* Kick velocity [m/s]
*/
IVector3 kickVel;
......
......@@ -71,7 +71,7 @@ public class RefereeVisCalc implements IWpCalc
double ballSpeed = wfw.getSimpleWorldFrame().getBall().getVel3().getLength();
double initBallSpeed = wfw.getSimpleWorldFrame().getKickFitState()
.map(BallKickFitState::getAbsoluteKickSpeed).orElse(0.0) / 1000.0;
.map(BallKickFitState::getAbsoluteKickSpeed).orElse(0.0);
double ballHeight = wfw.getSimpleWorldFrame().getBall().getPos3().z();
String ballVelStr = "Ball vel: " + dfBallVel.format(ballSpeed) + "| "
+ dfBallVel.format(initBallSpeed) + "; height: " + dfBallVel.format(ballHeight);
......
Markdown is supported
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