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

Resolve "Store and retrieve berkeley frames with multiple accessors per class"

Closes #1614

See merge request main/Sumatra!1359

sumatra-commit: 62ef5a1569bdda1403a23f259b6058390c4d46ba
parent aafd56a6
Pipeline #8823 passed with stage
in 4 minutes and 33 seconds
......@@ -38,11 +38,15 @@ public class CamFrameShapeMapProducer
}
public ShapeMap updateCamFrameShapes(final ExtendedCamDetectionFrame frame)
public void updateCamFrameShapes(final ExtendedCamDetectionFrame frame)
{
camFrames.put(frame.getCameraId(), frame);
camFrames.values().removeIf(f -> Math.abs(frame.gettCapture() - f.gettCapture()) / 1e9 > 1);
}
public ShapeMap createShapeMap()
{
List<IDrawableShape> shapes = new ArrayList<>();
var mergedFrames = camFrames.values().stream()
.sorted(Comparator.comparing(CamDetectionFrame::gettCapture))
......
/*
* Copyright (c) 2009 - 2018, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.wp;
......@@ -9,22 +9,26 @@ import edu.tigers.sumatra.drawable.ShapeMapSource;
import edu.tigers.sumatra.model.SumatraModel;
import edu.tigers.sumatra.persistence.BerkeleyDb;
import edu.tigers.sumatra.persistence.IBerkeleyRecorder;
import lombok.Value;
import lombok.extern.log4j.Log4j2;
import java.util.Deque;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.ConcurrentLinkedDeque;
/**
* Berkeley storage for cam frames
*/
@Log4j2
public class ShapeMapBerkeleyRecorder implements IBerkeleyRecorder
{
private static final long BUFFER_TIME = 1_000_000_000L;
private final WfwObserver wfwObserver = new WfwObserver();
private final BerkeleyDb db;
private final Deque<ShapeMapWithSource> buffer = new ConcurrentLinkedDeque<>();
private List<ShapeMapWithSource> globalBuffer = new ArrayList<>();
private long latestWrittenTimestamp = 0;
private long latestReceivedTimestamp = 0;
private boolean running = false;
......@@ -62,8 +66,13 @@ public class ShapeMapBerkeleyRecorder implements IBerkeleyRecorder
{
Map<Long, BerkeleyShapeMapFrame> toSave = new HashMap<>();
ShapeMapWithSource s = buffer.pollFirst();
while (s != null)
List<ShapeMapWithSource> newBuffer = new ArrayList<>(globalBuffer.size());
List<ShapeMapWithSource> localBuffer = globalBuffer;
globalBuffer = newBuffer;
Collections.sort(localBuffer);
for (ShapeMapWithSource s : localBuffer)
{
// drop frame, if it is too old
if (s.timestamp > latestWrittenTimestamp)
......@@ -73,14 +82,16 @@ public class ShapeMapBerkeleyRecorder implements IBerkeleyRecorder
{
// this frame is still within the buffering time and so will all following
// we have to re-add it to the buffer for next time
buffer.addFirst(s);
int currentIndex = localBuffer.indexOf(s);
globalBuffer.addAll(localBuffer.subList(currentIndex, localBuffer.size()));
break;
}
BerkeleyShapeMapFrame f = toSave.computeIfAbsent(s.timestamp, BerkeleyShapeMapFrame::new);
f.putShapeMap(s.source, s.shapeMap);
} else
{
log.warn("Dropping too old shape map ({})", (s.timestamp - latestReceivedTimestamp));
}
s = buffer.poll();
}
latestWrittenTimestamp = toSave.keySet().stream().mapToLong(i -> i).max().orElse(latestWrittenTimestamp);
......@@ -94,18 +105,19 @@ public class ShapeMapBerkeleyRecorder implements IBerkeleyRecorder
return running && timestamp >= latestReceivedTimestamp - BUFFER_TIME;
}
private static class ShapeMapWithSource
@Value
private static class ShapeMapWithSource implements Comparable<ShapeMapWithSource>
{
long timestamp;
ShapeMap shapeMap;
ShapeMapSource source;
public ShapeMapWithSource(final long timestamp, final ShapeMap shapeMap, final ShapeMapSource source)
@Override
public int compareTo(ShapeMapWithSource o)
{
this.timestamp = timestamp;
this.shapeMap = shapeMap;
this.source = source;
return Long.compare(timestamp, o.timestamp);
}
}
......@@ -117,8 +129,8 @@ public class ShapeMapBerkeleyRecorder implements IBerkeleyRecorder
ShapeMap shapeMapCopy = new ShapeMap();
shapeMapCopy.addAll(shapeMap);
shapeMapCopy.removeNonPersistent();
buffer.addLast(new ShapeMapWithSource(timestamp, shapeMapCopy, source));
latestReceivedTimestamp = timestamp;
globalBuffer.add(new ShapeMapWithSource(timestamp, shapeMapCopy, source));
latestReceivedTimestamp = Math.max(timestamp, latestReceivedTimestamp);
}
}
}
......@@ -257,9 +257,12 @@ public class WorldInfoCollector extends AWorldPredictor
private void visualize(final WorldFrameWrapper wfw)
{
ShapeMap shapeMap = new ShapeMap();
worldFrameVisualization.process(wfw, shapeMap);
notifyNewShapeMap(lastWFTimestamp, shapeMap, ShapeMapSource.of("World Frame"));
ShapeMap wfShapeMap = new ShapeMap();
worldFrameVisualization.process(wfw, wfShapeMap);
notifyNewShapeMap(lastWFTimestamp, wfShapeMap, ShapeMapSource.of("World Frame"));
ShapeMap visionShapeMap = camFrameShapeMapProducer.createShapeMap();
notifyNewShapeMap(lastWFTimestamp, visionShapeMap, ShapeMapSource.of("Vision"));
}
......@@ -518,8 +521,7 @@ public class WorldInfoCollector extends AWorldPredictor
CamBall ball = currentBallDetector.findCurrentBall(camDetectionFrame.getBalls());
ExtendedCamDetectionFrame eFrame = new ExtendedCamDetectionFrame(camDetectionFrame, ball);
observers.forEach(o -> o.onNewCamDetectionFrame(eFrame));
var shapeMap = camFrameShapeMapProducer.updateCamFrameShapes(eFrame);
notifyNewShapeMap(eFrame.gettCapture(), shapeMap, ShapeMapSource.of("Vision"));
camFrameShapeMapProducer.updateCamFrameShapes(eFrame);
}
......
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