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

Resolve "Implement ssl-vision tracking protocol"

Closes #1470

See merge request main/Sumatra!1205

sumatra-commit: bcf62b610a008346dfad6d284e42864618a8b25b
parent 8baf2ec5
Pipeline #7380 failed with stage
in 1 minute and 55 seconds
......@@ -62,4 +62,13 @@
<implementation>edu.tigers.autoreferee.AutoRefRecordManager</implementation>
</module>
<module id="edu.tigers.sumatra.wp.exporter.VisionTrackerSender">
<implementation>edu.tigers.sumatra.wp.exporter.VisionTrackerSender</implementation>
<properties>
<address>224.5.23.2</address>
<port>10010</port>
</properties>
</module>
</centralSoftware>
......@@ -2,7 +2,8 @@
~ Copyright (c) 2009 - 2016, DHBW Mannheim - TIGERs Mannheim
-->
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
<modelVersion>4.0.0</modelVersion>
<artifactId>autoreferee-main</artifactId>
......@@ -83,6 +84,11 @@
<groupId>edu.tigers.sumatra</groupId>
<artifactId>sumatra-gui-visualizer</artifactId>
</dependency>
<dependency>
<groupId>edu.tigers.sumatra</groupId>
<artifactId>moduli-wp-export</artifactId>
</dependency>
</dependencies>
<build>
......@@ -124,6 +130,9 @@
<ignoredUnusedDeclaredDependency>
org.apache.logging.log4j:log4j-1.2-api:jar
</ignoredUnusedDeclaredDependency>
<ignoredUnusedDeclaredDependency>
edu.tigers.sumatra:moduli-wp-export
</ignoredUnusedDeclaredDependency>
</ignoredUnusedDeclaredDependencies>
</configuration>
</execution>
......
/*
* Copyright (c) 2009 - 2019, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.cam;
......@@ -211,4 +211,10 @@ public class SSLVisionCam extends ACam implements Runnable, IReceiverObserver, I
{
return address;
}
public TimeSync getTimeSync()
{
return timeSync;
}
}
/*
* Copyright (c) 2009 - 2019, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.cam;
......@@ -12,18 +12,17 @@ import org.apache.logging.log4j.Logger;
/**
* @author Nicolai Ommer <nicolai.ommer@gmail.com>
* Synchronize between vision time and Sumatra internal time
*/
public class TimeSync
{
@SuppressWarnings("unused")
private static final Logger log = LogManager.getLogger(TimeSync.class.getName());
private static final int BUFFER_SIZE = 30;
private static final int BUFFER_SIZE = 30;
private long offset = 0;
private final Queue<Long> offsetBuffer = new CircularFifoQueue<>(BUFFER_SIZE);
private final Queue<Long> diffBuffer = new CircularFifoQueue<>(BUFFER_SIZE);
private long offset = 0;
private final Queue<Long> offsetBuffer = new CircularFifoQueue<>(BUFFER_SIZE);
private final Queue<Long> diffBuffer = new CircularFifoQueue<>(BUFFER_SIZE);
/**
......@@ -45,7 +44,7 @@ public class TimeSync
offset = (long) average(offsetBuffer);
if (avgDiff < 100_000)
{
log.info("Synced with Vision clock. offset=" + offset + "ns diff=" + avgDiff + "ns");
log.info("Synced with Vision clock. offset={}ns diff={}ns", offset, avgDiff);
offsetBuffer.clear();
}
}
......@@ -53,13 +52,26 @@ public class TimeSync
/**
* Convert from unix seconds to internal nano seconds
*
* @param timestamp
* @return
*/
public long sync(final double timestamp)
{
return convertVision2LocalTime(timestamp, offset);
}
/**
* Convert from internal nano seconds to unix seconds
*
* @param timestamp
* @return
*/
public double reverseSync(final long timestamp)
{
return (timestamp + offset) * 1e-9;
}
......
/*
* Copyright (c) 2009 - 2017, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.cam.data;
......@@ -18,7 +18,7 @@ import edu.tigers.sumatra.math.vector.Vector2f;
* SSL_DetectionRobot from
* protobuf-protocol, coming from
* the SSL-Vision.
*
*
* @author Gero
*/
@Persistent
......@@ -28,8 +28,8 @@ public class CamRobot extends ACamObject
private final IVector2 pos;
private final double orientation;
private final double height;
/**
* Dummy constructor for persistence
*/
......@@ -42,14 +42,14 @@ public class CamRobot extends ACamObject
orientation = 0;
height = 0;
}
/**
* <p>
* <i>(Being aware of EJ-SE Item 2; but we prefer performance over readability - at least in this case. Objects are
* created at only one point in the system, but needs to be fast.</i>
* </p>
*
*
* @param confidence
* @param pixel
* @param tCapture
......@@ -77,11 +77,11 @@ public class CamRobot extends ACamObject
this.height = height;
this.botId = botId;
}
/**
* New CamRobot with adjusted tCapture timestamp.
*
*
* @param orig
* @param tCapture
*/
......@@ -93,8 +93,8 @@ public class CamRobot extends ACamObject
height = orig.height;
botId = orig.botId;
}
@Override
public String toString()
{
......@@ -119,8 +119,8 @@ public class CamRobot extends ACamObject
builder.append("]");
return builder.toString();
}
/**
* @return the robotID
*/
......@@ -128,8 +128,8 @@ public class CamRobot extends ACamObject
{
return botId.getNumber();
}
/**
* @return the pos
*/
......@@ -138,8 +138,8 @@ public class CamRobot extends ACamObject
{
return pos;
}
/**
* @return the orientation
*/
......@@ -147,17 +147,17 @@ public class CamRobot extends ACamObject
{
return orientation;
}
/**
* @return the yExtent
* @return the height
*/
public double getHeight()
{
return height;
}
@Override
public List<Number> getNumberList()
{
......@@ -168,8 +168,8 @@ public class CamRobot extends ACamObject
numbers.add(getHeight());
return numbers;
}
@Override
public List<String> getHeaders()
{
......@@ -180,8 +180,8 @@ public class CamRobot extends ACamObject
headers.add("height");
return headers;
}
/**
* @return the botId
*/
......
/*
* Copyright (c) 2009 - 2018, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.vision.data;
......@@ -33,6 +33,7 @@ public class FilteredVisionBot
private final double orientation;
/** [rad/s] */
private final double angularVel;
/** 0-1 */
private final double quality;
......@@ -167,6 +168,7 @@ public class FilteredVisionBot
private Double orientation;
/** [rad/s] */
private Double angularVel;
/** 0-1 */
private double quality = 0;
......
/*
* Copyright (c) 2009 - 2018, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.vision.tracker;
......@@ -29,7 +29,7 @@ import edu.tigers.sumatra.vision.data.FilteredVisionBot;
/**
* Tracks and filters a single robot.
*
*
* @author AndreR <andre@ryll.cc>
*/
public class RobotTracker
......@@ -39,16 +39,16 @@ public class RobotTracker
private final BotID botId;
private final int camId;
private final List<Long> updateTimestamps = new ArrayList<>();
private long lastUpdateTimestamp;
private double visionQuality;
private double lastCamOrientation;
private long orientationTurns = 0;
private int health = 2;
@Configurable(defValue = "100.0")
private static double initialCovarianceXY = 100.0;
@Configurable(defValue = "0.1")
......@@ -69,16 +69,16 @@ public class RobotTracker
private static double maxAngularVel = 30.0;
@Configurable(defValue = "20", comment = "Reciprocal health is used as uncertainty, increased on update, decreased on prediction")
private static int maxHealth = 20;
static
{
ConfigRegistration.registerClass("vision", RobotTracker.class);
}
/**
* Create a new tracker from a camera measurement.
*
*
* @param robot
*/
public RobotTracker(final CamRobot robot)
......@@ -87,18 +87,18 @@ public class RobotTracker
robot.gettCapture());
filterW = new TrackingFilterPosVel1D(robot.getOrientation(), initialCovarianceW, modelErrorW, measErrorW,
robot.gettCapture());
lastCamOrientation = robot.getOrientation();
lastUpdateTimestamp = robot.gettCapture();
botId = robot.getBotId();
camId = robot.getCameraId();
}
/**
* Create a new tracker from camera measurement and prior state knowledge.
* Position and velocity is initialized with filtered vision bot state.
*
*
* @param robot
* @param filtered
*/
......@@ -106,22 +106,22 @@ public class RobotTracker
{
RealVector xy = new ArrayRealVector(filtered.getPos().toArray(), filtered.getVel().toArray());
RealVector w = new ArrayRealVector(new double[] { filtered.getOrientation(), filtered.getAngularVel() });
filterXY = new TrackingFilterPosVel2D(xy, initialCovarianceXY, modelErrorXY, measErrorXY,
robot.gettCapture());
filterW = new TrackingFilterPosVel1D(w, initialCovarianceW, modelErrorW, measErrorW,
robot.gettCapture());
lastCamOrientation = robot.getOrientation();
lastUpdateTimestamp = robot.gettCapture();
botId = robot.getBotId();
camId = robot.getCameraId();
}
/**
* Do a prediction step on all filters to a specific time.
*
*
* @param timestamp time in [ns]
* @param avgFrameDt average frame delta time in [s]
*/
......@@ -129,21 +129,21 @@ public class RobotTracker
{
filterXY.predict(timestamp);
filterW.predict(timestamp);
if (health > 1)
{
health--;
}
updateTimestamps.removeIf(t -> (timestamp - t) > 1_000_000_000L);
visionQuality = (updateTimestamps.size() * avgFrameDt) + 0.01;
}
/**
* Update this tracker with a camera measurement.
*
*
* @param robot
*/
public void update(final CamRobot robot)
......@@ -155,55 +155,55 @@ public class RobotTracker
// measurement too far away => refuse update
return;
}
double angDiff = Math.abs(AngleMath.difference(filterW.getPositionEstimate(), robot.getOrientation()));
if (angDiff > (dtInSec * maxAngularVel))
{
// orientation mismatch, maybe a +-90° vision switch => refuse update
return;
}
// we have an update, increase health/certainty in this tracker
if (health < maxHealth)
{
health += 2;
}
lastUpdateTimestamp = robot.gettCapture();
updateTimestamps.add(lastUpdateTimestamp);
filterXY.correct(robot.getPos());
double orient = robot.getOrientation();
// multi-turn angle correction
if ((orient < -AngleMath.PI_HALF) && (lastCamOrientation > AngleMath.PI_HALF))
{
++orientationTurns;
}
if ((orient > AngleMath.PI_HALF) && (lastCamOrientation < -AngleMath.PI_HALF))
{
--orientationTurns;
}
lastCamOrientation = orient;
orient += orientationTurns * AngleMath.PI_TWO;
filterW.correct(orient);
}
public double getUncertainty()
{
return 1.0 / health;
}
/**
* Get position estimate at specific timestamp.
*
*
* @param timestamp Query time.
* @return Position in [mm]
*/
......@@ -211,11 +211,11 @@ public class RobotTracker
{
return filterXY.getPositionEstimate(timestamp);
}
/**
* Get normalized orientation estimate at specific timestamp.
*
*
* @param timestamp Query time.
* @return Orientation in [rad]
*/
......@@ -223,30 +223,30 @@ public class RobotTracker
{
return AngleMath.normalizeAngle(filterW.getPositionEstimate(timestamp));
}
/**
* Get linear velocity estimate.
*
*
* @return Velocity in [mm/s]
*/
public IVector2 getVelocity()
{
return filterXY.getVelocityEstimate();
}
/**
* Get angular velocity estimate.
*
*
* @return angular velocity in [rad/s]
*/
public double getAngularVelocity()
{
return filterW.getVelocityEstimate();
}
/**
* @return timestamp in [ns]
*/
......@@ -254,8 +254,8 @@ public class RobotTracker
{
return lastUpdateTimestamp;
}
/**
* @return the id
*/
......@@ -263,13 +263,13 @@ public class RobotTracker
{
return botId;
}
/**
* This function merges a variable number of robot trackers and makes a filtered vision bot out of them.
* Trackers are weighted according to their state uncertainties. A tracker with high uncertainty
* has less influence on the final merge result.
*
*
* @param id BotID of the final robot.
* @param robots List of robot trackers. Must not be empty.
* @param timestamp Extrapolation time stamp to use for the final robot.
......@@ -279,14 +279,14 @@ public class RobotTracker
final long timestamp)
{
Validate.notEmpty(robots);
double totalPosUnc = 0;
double totalVelUnc = 0;
double totalOrientUnc = 0;
double totalAVelUnc = 0;
double maxQuality = 0;
// calculate sum of all uncertainties
for (RobotTracker t : robots)
{
......@@ -300,21 +300,21 @@ public class RobotTracker
maxQuality = t.getVisionQuality();
}
}
// all uncertainties must be > 0, otherwise we found a bug