Skip to content

Commit

Permalink
Merge branch 'feature/alert-intrusion' into develop
Browse files Browse the repository at this point in the history
  • Loading branch information
@paulosousadias
paulosousadias committed Jul 25, 2024
2 parents 06981dc + 8393493 commit 13b41fd
Show file tree
Hide file tree
Showing 3 changed files with 266 additions and 122 deletions.
147 changes: 128 additions & 19 deletions ...ns-dev/alert-intrusion/src/java/pt/lsts/neptus/plugins/alertintrusion/AlertIntrusion.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@
import pt.lsts.neptus.systems.external.ExternalSystem;
import pt.lsts.neptus.systems.external.ExternalSystemsHolder;
import pt.lsts.neptus.types.coord.LocationType;
import pt.lsts.neptus.util.AngleUtils;
import pt.lsts.neptus.util.ColorUtils;
import pt.lsts.neptus.util.ImageUtils;
import pt.lsts.util.WGS84Utilities;
Expand All @@ -59,6 +60,7 @@
import java.awt.FontMetrics;
import java.awt.Graphics2D;
import java.awt.Image;
import java.awt.geom.GeneralPath;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.text.SimpleDateFormat;
Expand Down Expand Up @@ -92,13 +94,26 @@ public long getMicroseconds() {
}
}

@NeptusProperty(name = "Minimum distance allowed between AUVs and Ships (meters)", userLevel = NeptusProperty.LEVEL.REGULAR)
private static final GeneralPath shapeArrow = new GeneralPath();
static {
shapeArrow.moveTo(0, 0);
shapeArrow.curveTo(7, 3, 7, 3, 14, 0);
//shapeArrow.lineTo(14, 0);
shapeArrow.lineTo(7, -14);
shapeArrow.lineTo(0, 0);
}

private final Color shapeColor = new Color(0xFF, 0xD0, 0x46, 203);
private final Color shapeHighColor = new Color(255, 70, 70, 203);
private final Color blackTransparentColor = ColorUtils.setTransparencyToColor(Color.black, 100);

@NeptusProperty(name = "Minimum distance allowed between vehicle and ships (meters)", userLevel = NeptusProperty.LEVEL.REGULAR)
public int collisionDistance = 100;
@NeptusProperty(name = "Percentage of the critical distance to trigger the alert")
public int collisionCriticalDistancePercentage = 20;
@NeptusProperty(name = "Use course for calculation", userLevel = NeptusProperty.LEVEL.REGULAR)
public boolean useCourseForCalculation = true;
@NeptusProperty(name = "Minimum Speed To Be Stopped", description = "Configures the maximum speed (m/s) for the system to be considered stopped (affects the drawing of the course/speed vector on the renderer)",
@NeptusProperty(name = "Minimum Speed To Be Stopped", description = "Configures the maximum speed (m/s) for the system to be considered stopped.",
category = "Renderer", userLevel = NeptusProperty.LEVEL.REGULAR)
public double minimumSpeedToBeStopped = 0.2;
@NeptusProperty(name = "Minutes To Hide Systems Without Known Location", description = "Minutes after which systems disappear from render if inactive (0 to disable)",
Expand All @@ -123,6 +138,8 @@ public long getMicroseconds() {

private final JLabel infoLabel = new JLabel("");

private final LookIndicatorSymbol lookIndicatorSymbol = new LookIndicatorSymbol();

public AlertIntrusion() {
super();
}
Expand Down Expand Up @@ -170,12 +187,18 @@ void updateCollisions() {
// (vehicle, ship) -> (distance, timestamp)
final ConcurrentHashMap<Pair<String, String>, Pair<Double, Date>> collisions = new ConcurrentHashMap<>();

final Date datetimeNow = new Date();
long timeSpanMillis = projectionTimeWindowValue * projectionTimeWindowUnit.getMicroseconds();
for (long timeOffset = 0; timeOffset < timeSpanMillis; timeOffset += 1_000L * collisionDistance / 4) {
final long deltaTimeMillis = timeOffset;
Arrays.stream(ImcSystemsHolder.lookupAllSystems())
.filter(system -> !system.getName().equalsIgnoreCase(mainSystemName))
.forEach(system -> {
Date dataAge = new Date(system.getLocationTimeMillis());
if (dataAge.before(datetimeNow) && datetimeNow.getTime() - dataAge.getTime()
> minutesToHideSystemsWithoutKnownLocation * 60_000L) {
return;
}
Date t = new Date(System.currentTimeMillis() + deltaTimeMillis);
LocationType locationSystem = system.getLocation().getNewAbsoluteLatLonDepth();
LocationType locationMain = mainSystem.getLocation().getNewAbsoluteLatLonDepth();
Expand All @@ -187,6 +210,11 @@ void updateCollisions() {
Arrays.stream(ExternalSystemsHolder.lookupAllSystems())
.filter(system -> !system.getName().equalsIgnoreCase(mainSystemName))
.forEach(system -> {
Date dataAge = new Date(system.getLocationTimeMillis());
if (dataAge.before(datetimeNow) && datetimeNow.getTime() - dataAge.getTime()
> minutesToHideSystemsWithoutKnownLocation * 60_000L) {
return;
}
Date t = new Date(System.currentTimeMillis() + deltaTimeMillis);
LocationType locationSystem = system.getLocation().getNewAbsoluteLatLonDepth();
LocationType locationMain = mainSystem.getLocation().getNewAbsoluteLatLonDepth();
Expand Down Expand Up @@ -214,7 +242,7 @@ void updateCollisions() {
});

long diff = System.currentTimeMillis() - start;
NeptusLog.pub().info("RiskAnalysis detected {} collisions in {} milliseconds.", collisions.size(), diff);
NeptusLog.pub().info("Risk detected {} collisions in {} milliseconds.", collisions.size(), diff);

if (changed.get()) {
layerPainter.triggerImageRebuild();
Expand Down Expand Up @@ -271,27 +299,31 @@ public void paint(Graphics2D g, StateRenderer2D renderer) {
SimpleDateFormat sdf = new SimpleDateFormat("HH:mm");
sdf.setTimeZone(TimeZone.getTimeZone("UTC"));

boolean askForLaterRepaint = false;
AtomicBoolean askForLaterRepaint = new AtomicBoolean(false);

boolean recreateImage = layerPainter.paintPhaseStartTestRecreateImageAndRecreate(g, renderer);
if (recreateImage) {
Graphics2D g2 = layerPainter.getImageGraphics();
// Paint what you want in the graphics
if (!collisionsTree.isEmpty() && collisionsTree.get(lastMainVehicle) != null && !collisionsTree.get(lastMainVehicle).isEmpty()) {
if (!collisionsTree.isEmpty() && collisionsTree.get(lastMainVehicle) != null
&& !collisionsTree.get(lastMainVehicle).isEmpty()) {
Graphics2D gg = (Graphics2D) g2.create();
gg.translate(20, 100);
askForLaterRepaint |= !gg.drawImage(colregImage, null, null);
boolean res = !gg.drawImage(colregImage, null, null);
if (res) {
askForLaterRepaint.compareAndSet(false, true);
}
gg.dispose();

int collisionSize = collisionsTree.get(lastMainVehicle).size();
infoLabel.setText("# " + collisionSize);
infoLabel.setHorizontalTextPosition(JLabel.CENTER);
infoLabel.setHorizontalAlignment(JLabel.CENTER);
infoLabel.setBackground(ColorUtils.setTransparencyToColor(Color.black, 100));
infoLabel.setBackground(blackTransparentColor);
infoLabel.setBorder(BorderFactory.createEmptyBorder(5, 5, 5, 5));
infoLabel.setOpaque(true);
gg = (Graphics2D) g2.create();
gg.translate(20 + 50 + 2, 100 + 25);
gg.translate(20 + 50 + 5 + 2, 100 + 25);
FontMetrics fontMetrics = gg.getFontMetrics();
Rectangle2D rectBounds = fontMetrics.getStringBounds(infoLabel.getText(), gg);
infoLabel.setBounds(0, 0, (int) rectBounds.getWidth() + 10, (int) rectBounds.getHeight() + 10);
Expand All @@ -304,12 +336,34 @@ public void paint(Graphics2D g, StateRenderer2D renderer) {
AtomicDouble distanceClosest = new AtomicDouble(Double.MAX_VALUE);
AtomicReference<Date> timeClosest = new AtomicReference<>(null);
collisionsTree.get(lastMainVehicle).forEach((time, pair) -> {
String ship = pair.first();
String sysName = pair.first();
double distance = pair.second();
if (distance < distanceClosest.get()) {
shipClosest.set(ship);
shipClosest.set(sysName);
distanceClosest.set(distance);
timeClosest.set(time);

LocationType loc = null;
ImcSystem sys = ImcSystemsHolder.lookupSystemByName(sysName);
if (sys != null) {
loc = sys.getLocation().getNewAbsoluteLatLonDepth();
} else {
ExternalSystem esys = ExternalSystemsHolder.lookupSystem(sysName);
if (esys != null) {
loc = esys.getLocation().getNewAbsoluteLatLonDepth();
}
}
if (loc != null) {
Graphics2D gg = (Graphics2D) g2.create();
Point2D spos = renderer.getScreenPosition(loc);
gg.translate(spos.getX() - 20 - 8, spos.getY());
boolean res = !gg.drawImage(colregImageSmall, null, null);
if (res) {
askForLaterRepaint.compareAndSet(false, true);
}
gg.dispose();
}

}
//Point2D pt = renderer.getScreenPosition(loc);
});
Expand All @@ -320,7 +374,7 @@ public void paint(Graphics2D g, StateRenderer2D renderer) {
infoLabel.setForeground(Color.white);
infoLabel.setHorizontalAlignment(JLabel.LEFT);
Graphics2D gg = (Graphics2D) g2.create();
gg.translate(20, 100 + 50 + 5);
gg.translate(20, 100 + 50 + 5 + 5);
FontMetrics fontMetrics = gg.getFontMetrics();
Rectangle2D rectBounds = fontMetrics.getStringBounds(line1, gg);
Rectangle2D rectBounds2 = fontMetrics.getStringBounds(line2, gg);
Expand All @@ -330,9 +384,11 @@ public void paint(Graphics2D g, StateRenderer2D renderer) {
gg.dispose();
}


if (shipClosest.get() != null) {
String sysName = shipClosest.get();
Point2D indicatorPoint = new Point2D.Double(20 + 25, 100 + 25);
AtomicReference<Short> mainlookAngle = new AtomicReference<>((short) 0);
boolean[] lookAngle = {false, false, false, false};
collisionsTree.get(lastMainVehicle).forEach((time, pair) -> {
String sysName = pair.first();
LocationType loc = null;
ImcSystem sys = ImcSystemsHolder.lookupSystemByName(sysName);
if (sys != null) {
Expand All @@ -344,20 +400,73 @@ public void paint(Graphics2D g, StateRenderer2D renderer) {
}
}
if (loc != null) {
Graphics2D gg = (Graphics2D) g2.create();
Point2D spos = renderer.getScreenPosition(loc);
gg.translate(spos.getX() - 20 - 8, spos.getY());
askForLaterRepaint |= !gg.drawImage(colregImageSmall, null, null);
Point2D pointShipClosest = renderer.getScreenPosition(loc);
Pair<Double, Short> angleRadAndQuadrant = calculateAngleToRotate(indicatorPoint, pointShipClosest);
if (angleRadAndQuadrant.second() == 0) {
lookAngle[0] = true;
} else if (angleRadAndQuadrant.second() == 1) {
lookAngle[1] = true;
} else if (angleRadAndQuadrant.second() == 2) {
lookAngle[2] = true;
} else if (angleRadAndQuadrant.second() == 3) {
lookAngle[3] = true;
}

if (shipClosest.get() != null) {
String closestSysName = shipClosest.get();
if (closestSysName.equals(sysName)) {
mainlookAngle.set(angleRadAndQuadrant.second());
}
}
}
});
for (int i = 0; i < lookAngle.length; i++) {
if (lookAngle[i]) {
Graphics2D gg = (Graphics2D) g2.create();
gg.translate(20 + 25, 100 + 25);
gg.rotate(Math.PI / 2 + Math.PI / 4 * (2 * i + 1));
gg.translate( -7,-25 - 2);
if (mainlookAngle.get() == i) {
gg.setColor(shapeHighColor);
} else {
gg.setColor(shapeColor);
}
gg.fill(shapeArrow);
gg.setColor(Color.black);
gg.draw(shapeArrow);
gg.dispose();
}
}
}

layerPainter.paintPhaseEndFinishImageRecreateAndPaintImageCacheToRenderer(g, renderer);
if (askForLaterRepaint) {
if (askForLaterRepaint.get()) {
layerPainter.triggerImageRebuild();
renderer.invalidate();
renderer.repaint(10);
}
}

private Pair<Double, Short> calculateAngleToRotate(Point2D indicatorPoint, Point2D pointShipClosest) {
double angleRad = AngleUtils.nomalizeAngleRadsPi(AngleUtils.calcAngle(
indicatorPoint.getY(), indicatorPoint.getX(),
pointShipClosest.getY(), pointShipClosest.getX()));
double angleDeg = Math.toDegrees(angleRad);
short quadrant = 0;
if (angleRad >= 0 && angleRad <= Math.PI / 2) {
angleRad = Math.PI / 4;
} else if (angleRad >= Math.PI / 2 && angleRad <= Math.PI) {
angleRad = 3 * Math.PI / 4;
quadrant = 1;
} else if (angleRad <= 0 && angleRad >= -Math.PI / 2) {
angleRad = -Math.PI / 4;
quadrant = 3;
} else if (angleRad <= -Math.PI / 2 && angleRad >= -Math.PI) {
angleRad = -3 * Math.PI / 4;
quadrant = 2;
}
double angleDeg1 = Math.toDegrees(angleRad);
System.out.println("angleDeg: " + angleDeg + " :: angleDeg1: " + angleDeg1);
return new Pair<>(angleRad, quadrant);
}
}
Loading

0 comments on commit 13b41fd

Please sign in to comment.