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

Resolve "Improve interaction with external simulator"

Closes #1613

See merge request main/Sumatra!1354

sumatra-commit: 6b15366c73e8263ef69d8cae026289abcf18118e
parent ce70b7da
Pipeline #8806 passed with stage
in 4 minutes and 25 seconds
/*
* Copyright (c) 2009 - 2017, DHBW Mannheim - Tigers Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.bot.params;
......
......@@ -4,20 +4,17 @@
package edu.tigers.autoreferee.engine.detector;
import com.github.g3force.configurable.Configurable;
import com.google.common.collect.Sets;
import edu.tigers.sumatra.geometry.RuleConstraints;
import edu.tigers.sumatra.ids.BotID;
import edu.tigers.sumatra.ids.ETeamColor;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.referee.data.EGameState;
import edu.tigers.sumatra.referee.gameevent.BotTooFastInStop;
import edu.tigers.sumatra.referee.gameevent.IGameEvent;
import edu.tigers.sumatra.wp.data.ITrackedBot;
import lombok.RequiredArgsConstructor;
import lombok.extern.log4j.Log4j2;
import java.util.Collection;
import java.util.EnumMap;
import java.util.HashMap;
import java.util.HashSet;
import java.util.IdentityHashMap;
import java.util.Map;
import java.util.Optional;
import java.util.Set;
......@@ -32,14 +29,16 @@ public class BotStopSpeedDetector extends AGameEventDetector
{
@Configurable(comment = "[s] Grace period before reporting any events", defValue = "2.0")
private static double gracePeriod = 2.0;
@Configurable(comment = "[s] The number of milliseconds that a bot needs violate the stop speed limit to be reported", defValue = "0.0")
private static double minViolationDuration = 0.0;
@Configurable(comment = "[s] The number of milliseconds that a bot needs violate the stop speed limit to be reported (to compensate known bad vision filter detections)", defValue = "0.3")
private static double minViolationDuration = 0.3;
private final Map<BotID, Long> currentViolators = new HashMap<>();
private final Set<BotID> lastViolators = new HashSet<>();
private final Map<ETeamColor, Boolean> infringementRecordedThisStopPhase = new EnumMap<>(ETeamColor.class);
private long entryTime = 0;
private final Map<BotID, Violator> violatorMap = new IdentityHashMap<>();
/**
* Rules state: A violation of this rule is only counted once per robot and stoppage.
*/
private final Map<BotID, Boolean> infringementRecordedThisStopPhase = new IdentityHashMap<>();
private long entryTime;
private long lastGameEventRaised;
public BotStopSpeedDetector()
......@@ -51,113 +50,73 @@ public class BotStopSpeedDetector extends AGameEventDetector
@Override
protected void doPrepare()
{
lastGameEventRaised = 0;
entryTime = frame.getTimestamp();
infringementRecordedThisStopPhase.put(ETeamColor.YELLOW, false);
infringementRecordedThisStopPhase.put(ETeamColor.BLUE, false);
infringementRecordedThisStopPhase.clear();
}
@Override
public Optional<IGameEvent> doUpdate()
{
/*
* We wait an initial time before reporting any events to give robots time to slow down after the STOP command
*/
if ((frame.getTimestamp() - entryTime) / 1e9 < gracePeriod)
{
// Rules state: There is a grace period of 2 seconds for the robots to slow down.
return Optional.empty();
}
Map<BotID, ITrackedBot> bots = frame.getWorldFrame().getBots();
long delta = frame.getTimestamp() - frame.getPreviousFrame().getTimestamp();
Set<BotID> frameViolators = getViolators(bots.values());
Set<BotID> frameNonViolators = Sets.difference(bots.keySet(), frameViolators);
updateCurrentViolators(frameViolators, frameNonViolators, delta);
Set<BotID> frameCountViolators = currentViolators.entrySet().stream()
.filter(entry ->
(entry.getValue() / 1e9 >= minViolationDuration)
|| (lastViolators.contains(entry.getKey()) && (entry.getValue() > 0)))
.map(Map.Entry::getKey)
.collect(Collectors.toSet());
Set<BotID> oldViolators = Sets.difference(lastViolators, frameCountViolators).immutableCopy();
lastViolators.removeAll(oldViolators);
Set<BotID> frameViolators = getViolators();
// remove non-violators
violatorMap.keySet().removeIf(botId -> !frameViolators.contains(botId));
// add new violators
frameViolators.forEach(botId -> violatorMap.computeIfAbsent(botId,
id -> new Violator(
id,
frame.getTimestamp(),
frame.getWorldFrame().getBot(id).getPos()
)
));
// update max velocity
violatorMap.forEach((id, v) -> v.vMax = Math.max(v.vMax, getBotVel(frame.getWorldFrame().getBot(id))));
Optional<BotID> optViolator = Sets.difference(frameCountViolators, lastViolators).stream().findFirst();
if (optViolator.isPresent())
if ((frame.getTimestamp() - lastGameEventRaised) < gracePeriod)
{
BotID violator = optViolator.get();
ITrackedBot bot = bots.get(violator);
if (bot == null)
{
log.debug("Bot Stop Speed violator disappeared from the field: " + violator);
return Optional.empty();
}
lastViolators.add(violator);
if (!infringementRecordedThisStopPhase.getOrDefault(violator.getTeamColor(), true))
{
infringementRecordedThisStopPhase.put(violator.getTeamColor(), true);
}
var violator = violatorMap.values().stream()
.filter(v -> (frame.getTimestamp() - v.tStart) / 1e9 > minViolationDuration)
.findFirst();
return Optional.of(new BotTooFastInStop(violator, bot.getPos(), bot.getVel().getLength()));
}
violator.ifPresent(v -> lastGameEventRaised = frame.getTimestamp());
violator.ifPresent(v -> infringementRecordedThisStopPhase.put(v.botID, true));
return Optional.empty();
return violator.map(v -> new BotTooFastInStop(v.botID, v.location, v.vMax));
}
private Set<BotID> getViolators(final Collection<ITrackedBot> bots)
private Set<BotID> getViolators()
{
return bots.stream()
.filter(bot -> bot.getFilteredState().orElse(bot.getBotState()).getVel2().getLength() > RuleConstraints
.getStopSpeed())
return frame.getWorldFrame().getBots().values().stream()
.filter(bot -> !infringementRecordedThisStopPhase.containsKey(bot.getBotId()))
.filter(bot -> getBotVel(bot) > RuleConstraints.getStopSpeed())
.map(ITrackedBot::getBotId)
.collect(Collectors.toSet());
}
private void updateCurrentViolators(final Set<BotID> violators, final Set<BotID> nonViolators, final long timeDelta)
{
for (BotID violator : violators)
{
long value = 0;
if (currentViolators.containsKey(violator))
private double getBotVel(ITrackedBot bot)
{
value = currentViolators.get(violator);
}
if (value / 1e9 < minViolationDuration)
{
value += timeDelta;
}
currentViolators.put(violator, value);
}
for (BotID nonViolator : nonViolators)
{
if (currentViolators.containsKey(nonViolator))
{
long value = currentViolators.get(nonViolator);
value -= timeDelta;
if (value <= 0)
{
currentViolators.remove(nonViolator);
} else
{
currentViolators.put(nonViolator, value);
}
}
}
return bot.getFilteredState().orElse(bot.getBotState()).getVel2().getLength();
}
@Override
public void doReset()
@RequiredArgsConstructor
private static class Violator
{
entryTime = 0;
lastViolators.clear();
currentViolators.clear();
final BotID botID;
final long tStart;
final IVector2 location;
double vMax;
}
}
......@@ -32,9 +32,6 @@ public class RuleConstraints
@Configurable(comment = "Bot speed in stop phases", defValue = "1.5")
private static double stopSpeed = 1.5;
@Getter
@Configurable(comment = "This tolerance is subtracted from the default bot speed that is required on STOP", defValue = "0.2")
private static double stopSpeedTolerance = 0.2;
@Getter
@Configurable(comment = "Distance between bots and penalty area in standard situations", defValue = "200.0")
private static double botToPenaltyAreaMarginStandard = 200;
@Getter
......@@ -56,13 +53,4 @@ public class RuleConstraints
private RuleConstraints()
{
}
/**
* @return The bot speed for our bots during stop including a tolerance
*/
public static double getStopTargetSpeed()
{
return stopSpeed - stopSpeedTolerance;
}
}
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