Die Migration der Bereiche "Docker Registry" und "Artifiacts" ist fast abgeschlossen. Die letzten Daten werden im Laufe des heutigen Abend (05.08.2021) noch vollständig hochgeladen. Das Anlegen neuer Images und Artifacts funktioniert bereits wieder.

Commit bc46e9e4 authored by Hengwen Zhang's avatar Hengwen Zhang
Browse files

Add logic for correctly handling "never" and "eventually" path goals

parent 81d09be8
......@@ -3,6 +3,7 @@ package de.rwth.montisim.simulation.vehicle.task.goal;
import de.rwth.montisim.commons.simulation.TaskStatus;
import de.rwth.montisim.commons.utils.LTLOperator;
import de.rwth.montisim.commons.utils.Vec2;
import de.rwth.montisim.commons.utils.Vec3;
import de.rwth.montisim.commons.utils.json.Typed;
import de.rwth.montisim.simulation.vehicle.Vehicle;
......@@ -18,14 +19,24 @@ public class PathGoal extends Goal {
public PathGoal() {
}
public PathGoal(LTLOperator ltlOperator, List<Vec2> path, double range) {
public PathGoal(LTLOperator ltlOperator, List<Vec2> path, double radius) {
this.ltlOperator = ltlOperator;
this.path = path;
this.radius = range;
this.radius = radius;
this.currDestIdx = 0;
}
public void update(Vehicle v) {
if (ltlOperator.equals(LTLOperator.EVENTUALLY)){
handleEventually(v);
} else if (ltlOperator.equals(LTLOperator.NEVER)) {
handleNever(v);
}
}
private void handleEventually(Vehicle v) {
if (currDestIdx >= path.size()) return; // skip if all destinations have been reached
Vec2 dest = path.get(currDestIdx);
double dist = v.physicalObject.pos.asVec2().distance(dest);
......@@ -47,6 +58,22 @@ public class PathGoal extends Goal {
}
}
private void handleNever(Vehicle v) {
// "never" operator expect the expression to always be false (FAILED)
// i.e. the vehicle should never arrive in the area of any given destination
TaskStatus status = TaskStatus.FAILED;
for (Vec2 vec: path) {
if (v.physicalObject.pos.distance(new Vec3(vec.x, vec.y, 0)) < radius) {
// succeeded if arrived in the area of vec
status = TaskStatus.SUCCEEDED;
break;
}
}
updateStatus(status);
}
@Override
public boolean equals(Object obj) {
if (obj == this)
......@@ -66,5 +93,14 @@ public class PathGoal extends Goal {
public static PathGoalBuilder newBuilder() {
return new PathGoalBuilder();
}
public List<Vec2> getPath() {
return path;
}
public double getRadius() {
return radius;
}
}
......@@ -42,4 +42,30 @@ public class PathGoalTest {
goal.update(v);
assertEquals(TaskStatus.SUCCEEDED, goal.getStatus());
}
@Test
public void testNever() {
Goal goal = PathGoal.newBuilder()
.never()
.arrive(new Vec2(100, 0))
.arrive(new Vec2(200, 0))
.arrive(new Vec2(300, 0))
.withInRange(10)
.build();
Vehicle v = mock(Vehicle.class);
v.physicalObject = mock(DynamicObject.class);
v.physicalObject.pos = new Vec3(0, 0, 0);
goal.update(v);
assertEquals(TaskStatus.SUCCEEDED, goal.getStatus());
v.physicalObject.pos = new Vec3(95, 0, 0);
goal.update(v);
assertEquals(TaskStatus.FAILED, goal.getStatus());
v.physicalObject.pos = new Vec3(0, 0, 0);
goal.update(v);
assertEquals(TaskStatus.FAILED, goal.getStatus());
}
}
\ No newline at end of file
Markdown is supported
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