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; package edu.tigers.sumatra.bot.params;
......
...@@ -4,20 +4,17 @@ ...@@ -4,20 +4,17 @@
package edu.tigers.autoreferee.engine.detector; package edu.tigers.autoreferee.engine.detector;
import com.github.g3force.configurable.Configurable; import com.github.g3force.configurable.Configurable;
import com.google.common.collect.Sets;
import edu.tigers.sumatra.geometry.RuleConstraints; import edu.tigers.sumatra.geometry.RuleConstraints;
import edu.tigers.sumatra.ids.BotID; 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.data.EGameState;
import edu.tigers.sumatra.referee.gameevent.BotTooFastInStop; import edu.tigers.sumatra.referee.gameevent.BotTooFastInStop;
import edu.tigers.sumatra.referee.gameevent.IGameEvent; import edu.tigers.sumatra.referee.gameevent.IGameEvent;
import edu.tigers.sumatra.wp.data.ITrackedBot; import edu.tigers.sumatra.wp.data.ITrackedBot;
import lombok.RequiredArgsConstructor;
import lombok.extern.log4j.Log4j2; import lombok.extern.log4j.Log4j2;
import java.util.Collection; import java.util.IdentityHashMap;
import java.util.EnumMap;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map; import java.util.Map;
import java.util.Optional; import java.util.Optional;
import java.util.Set; import java.util.Set;
...@@ -32,14 +29,16 @@ public class BotStopSpeedDetector extends AGameEventDetector ...@@ -32,14 +29,16 @@ public class BotStopSpeedDetector extends AGameEventDetector
{ {
@Configurable(comment = "[s] Grace period before reporting any events", defValue = "2.0") @Configurable(comment = "[s] Grace period before reporting any events", defValue = "2.0")
private static double gracePeriod = 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") @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.0; private static double minViolationDuration = 0.3;
private final Map<BotID, Violator> violatorMap = new IdentityHashMap<>();
private final Map<BotID, Long> currentViolators = new HashMap<>(); /**
private final Set<BotID> lastViolators = new HashSet<>(); * Rules state: A violation of this rule is only counted once per robot and stoppage.
private final Map<ETeamColor, Boolean> infringementRecordedThisStopPhase = new EnumMap<>(ETeamColor.class); */
private long entryTime = 0; private final Map<BotID, Boolean> infringementRecordedThisStopPhase = new IdentityHashMap<>();
private long entryTime;
private long lastGameEventRaised;
public BotStopSpeedDetector() public BotStopSpeedDetector()
...@@ -51,113 +50,73 @@ public class BotStopSpeedDetector extends AGameEventDetector ...@@ -51,113 +50,73 @@ public class BotStopSpeedDetector extends AGameEventDetector
@Override @Override
protected void doPrepare() protected void doPrepare()
{ {
lastGameEventRaised = 0;
entryTime = frame.getTimestamp(); entryTime = frame.getTimestamp();
infringementRecordedThisStopPhase.put(ETeamColor.YELLOW, false); infringementRecordedThisStopPhase.clear();
infringementRecordedThisStopPhase.put(ETeamColor.BLUE, false);
} }
@Override @Override
public Optional<IGameEvent> doUpdate() 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) 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(); return Optional.empty();
} }
Map<BotID, ITrackedBot> bots = frame.getWorldFrame().getBots(); Set<BotID> frameViolators = getViolators();
// remove non-violators
long delta = frame.getTimestamp() - frame.getPreviousFrame().getTimestamp(); violatorMap.keySet().removeIf(botId -> !frameViolators.contains(botId));
Set<BotID> frameViolators = getViolators(bots.values()); // add new violators
Set<BotID> frameNonViolators = Sets.difference(bots.keySet(), frameViolators); frameViolators.forEach(botId -> violatorMap.computeIfAbsent(botId,
updateCurrentViolators(frameViolators, frameNonViolators, delta); id -> new Violator(
id,
Set<BotID> frameCountViolators = currentViolators.entrySet().stream() frame.getTimestamp(),
.filter(entry -> frame.getWorldFrame().getBot(id).getPos()
(entry.getValue() / 1e9 >= minViolationDuration) )
|| (lastViolators.contains(entry.getKey()) && (entry.getValue() > 0))) ));
.map(Map.Entry::getKey) // update max velocity
.collect(Collectors.toSet()); violatorMap.forEach((id, v) -> v.vMax = Math.max(v.vMax, getBotVel(frame.getWorldFrame().getBot(id))));
Set<BotID> oldViolators = Sets.difference(lastViolators, frameCountViolators).immutableCopy(); if ((frame.getTimestamp() - lastGameEventRaised) < gracePeriod)
lastViolators.removeAll(oldViolators);
Optional<BotID> optViolator = Sets.difference(frameCountViolators, lastViolators).stream().findFirst();
if (optViolator.isPresent())
{ {
BotID violator = optViolator.get(); return Optional.empty();
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);
}
return Optional.of(new BotTooFastInStop(violator, bot.getPos(), bot.getVel().getLength()));
} }
return Optional.empty(); var violator = violatorMap.values().stream()
.filter(v -> (frame.getTimestamp() - v.tStart) / 1e9 > minViolationDuration)
.findFirst();
violator.ifPresent(v -> lastGameEventRaised = frame.getTimestamp());
violator.ifPresent(v -> infringementRecordedThisStopPhase.put(v.botID, true));
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() return frame.getWorldFrame().getBots().values().stream()
.filter(bot -> bot.getFilteredState().orElse(bot.getBotState()).getVel2().getLength() > RuleConstraints .filter(bot -> !infringementRecordedThisStopPhase.containsKey(bot.getBotId()))
.getStopSpeed()) .filter(bot -> getBotVel(bot) > RuleConstraints.getStopSpeed())
.map(ITrackedBot::getBotId) .map(ITrackedBot::getBotId)
.collect(Collectors.toSet()); .collect(Collectors.toSet());
} }
private void updateCurrentViolators(final Set<BotID> violators, final Set<BotID> nonViolators, final long timeDelta) private double getBotVel(ITrackedBot bot)
{ {
for (BotID violator : violators) return bot.getFilteredState().orElse(bot.getBotState()).getVel2().getLength();
{
long value = 0;
if (currentViolators.containsKey(violator))
{
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);
}
}
}
} }
@Override @RequiredArgsConstructor
public void doReset() private static class Violator
{ {
entryTime = 0; final BotID botID;
lastViolators.clear(); final long tStart;
currentViolators.clear(); final IVector2 location;
double vMax;
} }
} }
...@@ -32,9 +32,6 @@ public class RuleConstraints ...@@ -32,9 +32,6 @@ public class RuleConstraints
@Configurable(comment = "Bot speed in stop phases", defValue = "1.5") @Configurable(comment = "Bot speed in stop phases", defValue = "1.5")
private static double stopSpeed = 1.5; private static double stopSpeed = 1.5;
@Getter @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") @Configurable(comment = "Distance between bots and penalty area in standard situations", defValue = "200.0")
private static double botToPenaltyAreaMarginStandard = 200; private static double botToPenaltyAreaMarginStandard = 200;
@Getter @Getter
...@@ -56,13 +53,4 @@ public class RuleConstraints ...@@ -56,13 +53,4 @@ public class RuleConstraints
private 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