Unverified Commit 380962ea authored by NicolaiO's avatar NicolaiO 🐼
Browse files

Initial version of autoRef CI implementation

parent 0018f555
Pipeline #17772 passed with stage
in 4 minutes and 45 seconds
......@@ -4,12 +4,15 @@
<option name="executionName" />
<option name="externalProjectPath" value="$PROJECT_DIR$" />
<option name="externalSystemIdString" value="GRADLE" />
<option name="scriptParameters" value="run --args=&quot;-a&quot;" />
<option name="scriptParameters" value="" />
<option name="taskDescriptions">
<list />
</option>
<option name="taskNames">
<list />
<list>
<option value="run" />
<option value="--args=&quot;-c&quot;" />
</list>
</option>
<option name="vmOptions" value="" />
</ExternalSystemSettings>
......
......@@ -37,6 +37,7 @@ dependencies {
implementation project(':moduli-record')
implementation project(':moduli-autoreferee')
runtimeOnly project(':moduli-autoreferee-ci')
runtimeOnly project(':moduli-wp')
runtimeOnly project(':moduli-referee')
......
<?xml version="1.0" encoding="UTF-8" ?>
<centralSoftware>
<!--
This configuration is copied to the AutoReferee when synchronizing source code. It must not be removed or renamed!
-->
<globalConfiguration>
<environment>ROBOCUP</environment>
<geometry>DIV_A</geometry>
</globalConfiguration>
<module id="edu.tigers.sumatra.cam.ACam">
<implementation>edu.tigers.autoreferee.ci.AutoRefereeCiCamModule</implementation>
<properties>
<port>10013</port>
</properties>
</module>
<module id="edu.tigers.sumatra.geometry.GeometryUpdater">
<dependency>edu.tigers.sumatra.cam.ACam</dependency>
</module>
<module id="edu.tigers.sumatra.vision.AVisionFilter">
<implementation>edu.tigers.sumatra.vision.VisionFilterImpl</implementation>
<dependency>edu.tigers.sumatra.referee.AReferee</dependency>
<dependency>edu.tigers.sumatra.cam.ACam</dependency>
</module>
<module id="edu.tigers.sumatra.wp.AWorldPredictor">
<implementation>edu.tigers.sumatra.wp.WorldInfoCollector</implementation>
<dependency>edu.tigers.sumatra.referee.AReferee</dependency>
<dependency>edu.tigers.sumatra.vision.AVisionFilter</dependency>
<dependency>edu.tigers.sumatra.persistence.RecordManager</dependency>
<dependency>edu.tigers.sumatra.cam.ACam</dependency>
</module>
<module id="edu.tigers.sumatra.referee.AReferee">
<implementation>edu.tigers.sumatra.referee.Referee</implementation>
<properties>
<source>INTERNAL_FORWARDER</source>
</properties>
</module>
<module id="edu.tigers.autoreferee.module.AutoRefModule">
<properties>
<gameControllerPort>10007</gameControllerPort>
</properties>
<dependency>edu.tigers.sumatra.wp.AWorldPredictor</dependency>
</module>
<module id="edu.tigers.sumatra.persistence.RecordManager">
<implementation>edu.tigers.autoreferee.AutoRefRecordManager</implementation>
</module>
</centralSoftware>
......@@ -51,6 +51,7 @@ public class BallState implements IMirrorable<BallState>
/**
* The spin of the ball in [rad/s], positive spin corresponds to positive linear velocity
*/
@NonNull
IVector2 spin;
......
/*
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
plugins {
id 'sumatra.java'
id 'java-library'
id 'sumatra.protobuf'
}
dependencies {
implementation project(':common')
implementation project(':common-math')
implementation project(':sumatra-model')
implementation project(':moduli-wp')
implementation project(':moduli-vision')
implementation project(':moduli-geometry')
implementation project(':moduli-referee')
implementation project(':moduli-cam')
implementation 'com.github.TIGERs-Mannheim:moduli:4.1'
implementation 'org.apache.logging.log4j:log4j-api:2.17.1'
implementation 'commons-configuration:commons-configuration:1.10'
implementation 'com.google.protobuf:protobuf-java:3.19.4'
}
/*
* Copyright (c) 2009 - 2022, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.autoreferee.ci;
import edu.tigers.moduli.exceptions.StartModuleException;
import edu.tigers.sumatra.cam.ACam;
import edu.tigers.sumatra.cam.SSLVisionCamGeometryTranslator;
import edu.tigers.sumatra.cam.data.CamGeometry;
import edu.tigers.sumatra.cam.proto.SslVisionDetection;
import edu.tigers.sumatra.cam.proto.SslVisionGeometry;
import edu.tigers.sumatra.model.SumatraModel;
import edu.tigers.sumatra.referee.AReferee;
import edu.tigers.sumatra.referee.proto.SslGcRefereeMessage;
import edu.tigers.sumatra.referee.source.DirectRefereeMsgForwarder;
import edu.tigers.sumatra.referee.source.ERefereeMessageSource;
import edu.tigers.sumatra.vision.AVisionFilter;
import edu.tigers.sumatra.vision.data.FilteredVisionFrame;
import edu.tigers.sumatra.wp.IWorldFrameObserver;
import edu.tigers.sumatra.wp.TrackerPacketGenerator;
import edu.tigers.sumatra.wp.WorldInfoCollector;
import edu.tigers.sumatra.wp.data.WorldFrameWrapper;
import edu.tigers.sumatra.wp.proto.SslVisionWrapperTracked;
import edu.tigers.sumatra.wp.proto.SslVisionWrapperTracked.TrackerWrapperPacket;
public class AutoRefereeCiCamModule extends ACam implements IWorldFrameObserver
{
private final SSLVisionCamGeometryTranslator geometryTranslator = new SSLVisionCamGeometryTranslator();
private final TrackedFrameToFilteredVisionMapper trackedFrameToFilteredVisionMapper = new TrackedFrameToFilteredVisionMapper();
private final TrackerPacketGenerator trackerPacketGenerator = new TrackerPacketGenerator("TIGERs");
private final AutoRefereeCiServer autoRefereeCiServer = new AutoRefereeCiServer(
this::publishDetection,
this::publishGeometry,
this::publishReferee,
this::publishTrackedWrapperFrame
);
private DirectRefereeMsgForwarder refForwarder;
@Override
public void startModule() throws StartModuleException
{
super.startModule();
SumatraModel.getInstance().getModule(WorldInfoCollector.class).addObserver(this);
AReferee ref = SumatraModel.getInstance().getModule(AReferee.class);
refForwarder = (DirectRefereeMsgForwarder) ref.getSource(ERefereeMessageSource.INTERNAL_FORWARDER);
int port = getSubnodeConfiguration().getInt("port", 10013);
autoRefereeCiServer.setPort(port);
autoRefereeCiServer.start();
}
@Override
public void stopModule()
{
autoRefereeCiServer.stop();
super.stopModule();
}
@Override
public void onNewWorldFrame(final WorldFrameWrapper wfw)
{
SslVisionWrapperTracked.TrackerWrapperPacket packet = trackerPacketGenerator.generate(wfw.getSimpleWorldFrame());
autoRefereeCiServer.publish(packet);
}
private void publishReferee(SslGcRefereeMessage.Referee referee)
{
refForwarder.send(referee);
}
private void publishGeometry(SslVisionGeometry.SSL_GeometryData geometryData)
{
CamGeometry geometry = geometryTranslator.fromProtobuf(geometryData);
notifyNewCameraCalibration(geometry);
}
private void publishDetection(SslVisionDetection.SSL_DetectionFrame detectionFrame)
{
notifyNewCameraFrame(detectionFrame);
}
private void publishTrackedWrapperFrame(TrackerWrapperPacket wrapper)
{
FilteredVisionFrame filteredVisionFrame = trackedFrameToFilteredVisionMapper.map(wrapper.getTrackedFrame());
SumatraModel.getInstance().getModule(AVisionFilter.class).publishFilteredVisionFrame(filteredVisionFrame);
}
}
/*
* Copyright (c) 2009 - 2022, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.autoreferee.ci;
import edu.tigers.autoreferee.proto.SslAutorefCi.AutoRefCiInput;
import edu.tigers.autoreferee.proto.SslAutorefCi.AutoRefCiOutput;
import edu.tigers.sumatra.cam.proto.SslVisionDetection;
import edu.tigers.sumatra.cam.proto.SslVisionGeometry;
import edu.tigers.sumatra.referee.proto.SslGcRefereeMessage;
import edu.tigers.sumatra.util.Safe;
import edu.tigers.sumatra.wp.proto.SslVisionWrapperTracked;
import lombok.RequiredArgsConstructor;
import lombok.Setter;
import lombok.extern.log4j.Log4j2;
import java.io.IOException;
import java.net.ServerSocket;
import java.net.Socket;
import java.util.function.Consumer;
@Log4j2
@RequiredArgsConstructor
public class AutoRefereeCiServer
{
private final Consumer<SslVisionDetection.SSL_DetectionFrame> detectionFrameConsumer;
private final Consumer<SslVisionGeometry.SSL_GeometryData> geometryDataConsumer;
private final Consumer<SslGcRefereeMessage.Referee> refereeConsumer;
private final Consumer<SslVisionWrapperTracked.TrackerWrapperPacket> trackerWrapperPacketConsumer;
@Setter
private int port;
private Thread thread;
private boolean running;
private ServerSocket serverSocket;
private Socket currentSocket;
public void start()
{
if (running)
{
throw new IllegalStateException("Server is already running");
}
running = true;
try
{
serverSocket = new ServerSocket(port);
} catch (IOException e)
{
log.error("Could not listen on port " + port, e);
return;
}
thread = new Thread(() -> Safe.run(this::listen));
thread.setName("AutoRef CI Server");
thread.start();
}
public void stop()
{
if (!running)
{
throw new IllegalStateException("Server is already stopped");
}
running = false;
try
{
serverSocket.close();
} catch (IOException e)
{
log.warn("Failed to close server socket", e);
}
thread.interrupt();
thread = null;
serverSocket = null;
}
public void publish(SslVisionWrapperTracked.TrackerWrapperPacket trackerWrapperPacket)
{
Socket socket = currentSocket;
if (socket == null)
{
return;
}
AutoRefCiOutput autoRefCiOutput = AutoRefCiOutput.newBuilder()
.setTrackerWrapperPacket(trackerWrapperPacket)
.build();
try
{
autoRefCiOutput.writeDelimitedTo(socket.getOutputStream());
} catch (IOException e)
{
log.warn("Failed to publish tracker wrapper packet", e);
}
}
private void listen()
{
while (running)
{
try
{
currentSocket = accept();
while (running)
{
if (!consume(currentSocket))
{
break;
}
}
} catch (IOException e)
{
log.warn("Connection failed", e);
}
currentSocket = null;
}
}
private boolean consume(Socket socket) throws IOException
{
if (socket == null)
{
return false;
}
AutoRefCiInput autoRefCiInput = AutoRefCiInput.parseDelimitedFrom(socket.getInputStream());
if (autoRefCiInput == null)
{
return false;
}
if (autoRefCiInput.hasGeometry())
{
geometryDataConsumer.accept(autoRefCiInput.getGeometry());
}
autoRefCiInput.getDetectionList().forEach(detectionFrameConsumer);
if (autoRefCiInput.hasRefereeMessage())
{
refereeConsumer.accept(autoRefCiInput.getRefereeMessage());
}
if (autoRefCiInput.hasTrackerWrapperPacket())
{
trackerWrapperPacketConsumer.accept(autoRefCiInput.getTrackerWrapperPacket());
}
return true;
}
private Socket accept() throws IOException
{
Socket socket = serverSocket.accept();
socket.setTcpNoDelay(true);
return socket;
}
}
/*
* Copyright (c) 2009 - 2022, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.autoreferee.ci;
import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.drawable.ShapeMap;
import edu.tigers.sumatra.geometry.Geometry;
import edu.tigers.sumatra.ids.BotID;
import edu.tigers.sumatra.ids.ETeamColor;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.math.vector.Vector3;
import edu.tigers.sumatra.referee.proto.SslGcCommon;
import edu.tigers.sumatra.referee.proto.SslGcGeometry;
import edu.tigers.sumatra.vision.data.FilteredVisionBall;
import edu.tigers.sumatra.vision.data.FilteredVisionBot;
import edu.tigers.sumatra.vision.data.FilteredVisionFrame;
import edu.tigers.sumatra.vision.data.FilteredVisionKick;
import edu.tigers.sumatra.wp.proto.SslVisionDetectionTracked;
import java.util.List;
public class TrackedFrameToFilteredVisionMapper
{
private long ballLastVisibleTimestamp;
public FilteredVisionFrame map(SslVisionDetectionTracked.TrackedFrame trackedFrame)
{
long timestamp = (long) (trackedFrame.getTimestamp() * 1e9);
return FilteredVisionFrame.builder()
.withId((long) trackedFrame.getFrameNumber())
.withTimestamp(timestamp)
.withBall(mapBalls(trackedFrame.getBallsList(), timestamp))
.withBots(mapRobots(trackedFrame.getRobotsList(), timestamp))
.withShapeMap(new ShapeMap())
.withKick(map(trackedFrame.getKickedBall()))
.build();
}
private FilteredVisionKick map(SslVisionDetectionTracked.KickedBall kickedBall)
{
long kickTimestamp = (long) (kickedBall.getStartTimestamp() * 1e9);
return FilteredVisionKick.builder()
.withKickTimestamp(kickTimestamp)
.withTrajectoryStartTime(kickTimestamp)
.withKickingBot(map(kickedBall.getRobotId()))
.withKickingBotPosition(map(kickedBall.getPos()))
.withKickingBotOrientation(map(kickedBall.getVel()).getXYVector().getAngle())
.withNumBallDetectionsSinceKick(100)
.withBallTrajectory(Geometry.getBallFactory().createTrajectoryFromState(BallState.builder()
.withPos(Vector3.from2d(map(kickedBall.getPos()), 0))
.withVel(map(kickedBall.getVel()))
.withAcc(Vector3.zero())
.withSpin(Vector2.zero())
.build()))
.build();
}
private List<FilteredVisionBot> mapRobots(List<SslVisionDetectionTracked.TrackedRobot> robots, long timestamp)
{
return robots.stream().map(robot -> map(robot, timestamp)).toList();
}
private FilteredVisionBot map(SslVisionDetectionTracked.TrackedRobot robot, long timestamp)
{
return FilteredVisionBot.builder()
.withBotID(map(robot.getRobotId()))
.withTimestamp(timestamp)
.withPos(map(robot.getPos()))
.withVel(map(robot.getVel()))
.withOrientation((double) robot.getOrientation())
.withAngularVel((double) robot.getVelAngular())
.withQuality(robot.getVisibility())
.build();
}
private BotID map(SslGcCommon.RobotId robotId)
{
if (robotId == null)
{
return BotID.noBot();
}
return BotID.createBotId(
robotId.getId(),
switch (robotId.getTeam())
{
case BLUE -> ETeamColor.BLUE;
case YELLOW -> ETeamColor.YELLOW;
default -> ETeamColor.NEUTRAL;
}
);
}
private FilteredVisionBall mapBalls(List<SslVisionDetectionTracked.TrackedBall> ballsList, long timestamp)
{
if (ballsList.isEmpty())
{
return FilteredVisionBall.builder()
.withTimestamp(timestamp)
.withLastVisibleTimestamp(ballLastVisibleTimestamp)
.withBallState(BallState.builder()
.withPos(Vector3.zero())
.withVel(Vector3.zero())
.withAcc(Vector3.zero())
.withSpin(Vector2.zero())
.build())
.build();
}
SslVisionDetectionTracked.TrackedBall ball = ballsList.get(0);
ballLastVisibleTimestamp = timestamp;
return FilteredVisionBall.builder()
.withTimestamp(timestamp)
.withLastVisibleTimestamp(ballLastVisibleTimestamp)
.withBallState(map(ball))
.build();
}
private BallState map(SslVisionDetectionTracked.TrackedBall ball)
{
return BallState.builder()
.withPos(map(ball.getPos()))
.withVel(map(ball.getVel()))
.withAcc(Vector3.zero())
.withSpin(Vector2.zero())
.build();
}
private IVector3 map(SslGcGeometry.Vector3 v)
{
if (v == null)
{
return Vector3.zero();
}
return Vector3.fromXYZ(v.getX(), v.getY(), v.getZ()).multiply(1000);
}
private IVector2 map(SslGcGeometry.Vector2 v)
{
if (v == null)
{
return Vector2.zero();
}
return Vector2.fromXY(v.getX(), v.getY()).multiply(1000);
}
}
syntax = "proto2";
option java_package = "edu.tigers.autoreferee.proto";
import "ssl_vision_wrapper_tracked.proto";
import "ssl_vision_detection.proto";
import "ssl_vision_geometry.proto";
import "ssl_gc_referee_message.proto";
// The AutoRefCiInput contains all packets/messages that would otherwise be received through multicast by the auto-referee
// It may contain either a raw or a tracked SSL-vision packet. If both are given, the implementation may choose either one.
message AutoRefCiInput {
// Latest referee message
optional Referee referee_message = 1;
// A tracked SSL-Vision packet to be processed without filtering
optional TrackerWrapperPacket tracker_wrapper_packet = 2;
// A list of unfiltered SSL-Vision packets (for multiple cameras) to be filtered and processed
repeated SSL_DetectionFrame detection = 3;
// Current geometry data, to be sent at least once at the beginning of the connection
optional SSL_GeometryData geometry = 4;
}