Commit 4ee98443 authored by Hengwen Zhang's avatar Hengwen Zhang
Browse files

Add vehicle task

parent 124a6fa2
......@@ -2,7 +2,17 @@ package de.rwth.montisim.simulation.simulator;
import java.io.File;
import java.time.Duration;
import java.time.Instant;
import de.rwth.montisim.commons.simulation.SimulationObject;
import de.rwth.montisim.commons.simulation.TimeUpdate;
import de.rwth.montisim.simulation.vehicle.VehicleBuilder;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.task.Task;
import de.rwth.montisim.simulation.vehicle.task.goal.GoalBuilder;
import de.rwth.montisim.simulation.vehicle.task.goal.MetricGoal;
import de.rwth.montisim.simulation.vehicle.task.goal.PathGoal;
import de.rwth.montisim.simulation.vehicle.task.goal.VehicleProperty;
import org.junit.*;
import de.rwth.montisim.commons.map.Pathfinding;
......@@ -32,19 +42,64 @@ public class IntegrationTest {
Simulator simulator = new Simulator(config, world, pathfinding, mtManager);
SimulationLoop loop = new SimulationLoop(simulator, config);
DefaultVehicleConfig vConf = DefaultVehicleConfig.withJavaAutopilot();
Vehicle vehicle = simulator.getVehicleBuilder(vConf.properties).setName("TestVehicle").build();
Task task = new Task();
task.addGoal(PathGoal.newBuilder()
.eventually()
.arrive(TARGET_POS.x, TARGET_POS.y)
.withInRange(10)
.build());
task.addGoal(MetricGoal.newBuilder()
.setProperty(VehicleProperty.SPEED)
.never()
.greater(1000, "m/s")
.build());
vehicle.setTask(task);
vehicle.physicsModel.setGroundPosition(new Vec3(0, 0, 0), new Vec2(START_DIR.x, START_DIR.y));
simulator.addSimulationObject(vehicle);
Navigation nav = (Navigation) vehicle.eesystem.getComponentManager().getComponent("Navigation").get();
nav.pushTargetPos(TARGET_POS);
vehicle.addTarget(TARGET_POS);
Assert.assertFalse(simulator.allTasksSucceeded());
// dump and reload vehicle every 1000 steps. test if it is able to reach the destination.
Instant simulationTime = config.start_time;
int cnt = 0;
while (!simulator.finished()) {
// cnt++;
TimeUpdate tu = new TimeUpdate(simulationTime, config.tick_duration);
simulator.update(tu);
simulationTime = tu.newTime;
if (++cnt % 1000 != 0) {
continue;
}
double dist = vehicle.physicalObject.pos.distance(new Vec3(TARGET_POS.x, TARGET_POS.y, 0));
System.out.printf("dist: %.2fm, pos: %s\n", dist, vehicle.physicalObject.pos);
System.out.println("Dumping and reloading vehicle...");
// simulator = new Simulator(config, world, pathfinding, mtManager);
simulator.getUpdatables().stream()
.filter(x -> x instanceof Vehicle)
.forEach(x -> simulator.removeSimulationObject((SimulationObject) x));
String json = vehicle.stateToJson();
VehicleProperties.BuildContext ctx = new VehicleProperties.BuildContext(pathfinding, mtManager);
vehicle = VehicleBuilder.fromJsonState(ctx, json).build();
simulator.addSimulationObject(vehicle);
}
double dist = vehicle.physicalObject.pos.distance(new Vec3(TARGET_POS.x, TARGET_POS.y, 0));
System.out.printf("dist: %.2fm, pos: %s\n", dist, vehicle.physicalObject.pos);
// vehicle.addTarget(TARGET_POS);
Assert.assertTrue("Simulation Error", loop.run());
Assert.assertTrue("Vehicle did not reach target", simulator.allTasksSucceeded());
......
......@@ -13,6 +13,7 @@ import de.rwth.montisim.simulation.eesimulator.EESystem;
import de.rwth.montisim.simulation.vehicle.physicalvalues.*;
import de.rwth.montisim.simulation.vehicle.physicsmodel.PhysicsModel;
import de.rwth.montisim.simulation.vehicle.powertrain.PowerTrain;
import de.rwth.montisim.simulation.vehicle.task.Task;
public class Vehicle extends SimulationObject implements Updatable, Destroyable, TaskRunner {
public static final boolean SERIALIZE_FORMATTED = true;
......@@ -26,9 +27,10 @@ public class Vehicle extends SimulationObject implements Updatable, Destroyable,
public transient final PhysicalValueRegistry physicalValues = new PhysicalValueRegistry();
public transient final Updater updater = new Updater();
TaskStatus carStatus = TaskStatus.SUCCEEDED;
TaskStatus carStatus = TaskStatus.RUNNING;
Optional<Vec2> target = Optional.empty();
Vec2 a = new Vec2();
Task task;
protected Vehicle(VehicleProperties properties) {
this.properties = properties;
......@@ -45,21 +47,24 @@ public class Vehicle extends SimulationObject implements Updatable, Destroyable,
eesystem.simulator.update(newTime);
physicsModel.update(newTime);
task.update(this);
carStatus = task.status();
// TODO this is temp
double DIST_TO_TARGET = 5;
if (carStatus == TaskStatus.RUNNING) {
if (target.isPresent()) {
a.set(physicalObject.pos);
double dist = a.distance(target.get());
// if (c %100 == 0){
// System.out.println("Dist to target: "+dist);
// }
// c++;
if (dist < DIST_TO_TARGET) {
carStatus = TaskStatus.SUCCEEDED;
}
}
}
// double DIST_TO_TARGET = 5;
// if (carStatus == TaskStatus.RUNNING) {
// if (target.isPresent()) {
// a.set(physicalObject.pos);
// double dist = a.distance(target.get());
// // if (c %100 == 0){
// // System.out.println("Dist to target: "+dist);
// // }
// // c++;
// if (dist < DIST_TO_TARGET) {
// carStatus = TaskStatus.SUCCEEDED;
// }
// }
// }
}
// TODO this is temp
......@@ -100,11 +105,15 @@ public class Vehicle extends SimulationObject implements Updatable, Destroyable,
public String stateToJson() throws SerializationException {
JsonWriter w = new JsonWriter(SERIALIZE_FORMATTED);
w.startObject();
w.writeKey(K_CONFIG);
w.writeKey(K_CONFIG);
Json.toJson(w, properties, null);
w.writeKey(K_STATE);
Json.toJson(w, this, null);
w.endObject();
return w.getString();
}
public void setTask(Task task) {
this.task = task;
}
}
\ No newline at end of file
package de.rwth.montisim.simulation.vehicle.task;
import de.rwth.montisim.commons.simulation.TaskStatus;
import de.rwth.montisim.commons.utils.json.*;
import de.rwth.montisim.simulation.vehicle.Vehicle;
import de.rwth.montisim.simulation.vehicle.task.goal.*;
import java.util.LinkedList;
import java.util.List;
@Typed
public class Task {
private List<Goal> goals;
public Task() {
goals = new LinkedList<>();
}
public void addGoal(Goal goal) {
goals.add(goal);
}
public void update(Vehicle v) {
goals.forEach(g -> g.update(v));
}
public TaskStatus status() {
TaskStatus status = TaskStatus.SUCCEEDED;
for (Goal g : goals) {
TaskStatus goalStatus = g.getStatus();
if (goalStatus == TaskStatus.RUNNING) {
status = TaskStatus.RUNNING;
} else if (goalStatus == TaskStatus.FAILED) {
status = TaskStatus.FAILED;
break;
}
}
return status;
}
public List<Goal> getGoals() {
return goals;
}
}
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.simulation.vehicle.Vehicle;
/**
* Goal is an abstraction of a LTL expression: LTLOperator expr.
* <p>
* Examples:
* LTLOperator.NEVER expr: expr must never be true
* LTLOperator.ALWAYS expr: expr must always be true
* LTLOperator.EVENTUALLY expr: expr must be true before the simulation ends
* <p>
* Goal has a status which can be updated by calling update method while the simulation
* is running.
*/
public abstract class Goal {
TaskStatus status = TaskStatus.RUNNING;
protected LTLOperator ltlOperator;
public abstract void update(Vehicle v);
/**
* Update goal status depending on the LTL operator and the new status
*
* @param status new status
*/
void updateStatus(TaskStatus status) {
if (ltlOperator == LTLOperator.ALWAYS) {
handleAlways(status);
} else if (ltlOperator == LTLOperator.NEVER) {
handleNever(status);
} else if (ltlOperator == LTLOperator.EVENTUALLY) {
handleEventually(status);
}
}
private void handleAlways(TaskStatus status) {
if (status == TaskStatus.FAILED)
this.status = TaskStatus.FAILED;
else if (status == TaskStatus.SUCCEEDED && this.status == TaskStatus.RUNNING)
this.status = TaskStatus.SUCCEEDED;
}
private void handleNever(TaskStatus status) {
if (status == TaskStatus.SUCCEEDED)
this.status = TaskStatus.FAILED;
else if (status == TaskStatus.FAILED && this.status != TaskStatus.FAILED)
this.status = TaskStatus.SUCCEEDED;
}
private void handleEventually(TaskStatus status) {
if (status == TaskStatus.SUCCEEDED)
this.status = TaskStatus.SUCCEEDED;
}
public LTLOperator getLtlOperator() {
return ltlOperator;
}
public TaskStatus getStatus() {
return status;
}
}
package de.rwth.montisim.simulation.vehicle.task.goal;
import de.rwth.montisim.commons.utils.LTLOperator;
import de.rwth.montisim.commons.utils.json.Json;
import de.rwth.montisim.commons.utils.json.SerializationException;
public abstract class GoalBuilder<T extends GoalBuilder<T>> {
public Goal fromJson(String json) throws SerializationException {
return Json.instantiateFromJson(json, Goal.class);
}
public LTLOperator ltlOperator;
protected abstract T getThis();
public abstract Goal build();
public T never() {
ltlOperator = LTLOperator.NEVER;
return (T) this;
}
public T always() {
ltlOperator = LTLOperator.ALWAYS;
return (T) this;
}
public T eventually() {
ltlOperator = LTLOperator.EVENTUALLY;
return (T) this;
}
public T until() {
// TODO implement
// this is only a placeholder
ltlOperator = LTLOperator.UNTIL;
return (T) this;
}
}
package de.rwth.montisim.simulation.vehicle.task.goal;
import de.rwth.montisim.commons.simulation.TaskStatus;
import de.rwth.montisim.commons.utils.Comparator;
import de.rwth.montisim.commons.utils.LTLOperator;
import de.rwth.montisim.commons.utils.json.Typed;
import de.rwth.montisim.simulation.vehicle.Vehicle;
@Typed
public class MetricGoal extends Goal {
private VehicleProperty property;
private Comparator comparator;
private double value;
private String unit;
public MetricGoal() {
}
public MetricGoal(LTLOperator ltlOperator, VehicleProperty property, Comparator comparator, double value, String unit) {
this.ltlOperator = ltlOperator;
this.property = property;
this.comparator = comparator;
this.value = value;
this.unit = unit;
}
public void update(Vehicle v) {
double currValue = 0;
if (property.equals(VehicleProperty.SPEED)) {
currValue = v.physicalObject.velocity.magnitude();
}
boolean result = compare(currValue);
if (result) {
updateStatus(TaskStatus.SUCCEEDED);
} else {
updateStatus(TaskStatus.FAILED);
}
}
private boolean compare(double currValue) {
// TODO take unit into consideration
if (comparator.equals(Comparator.EQUAL) && currValue == value) {
return true;
} else if (comparator.equals(Comparator.GREATER) && currValue > value) {
return true;
} else if (comparator.equals(Comparator.GREATER_EQUAL) && currValue >= value) {
return true;
} else if (comparator.equals(Comparator.LESS) && currValue < value) {
return true;
} else if (comparator.equals(Comparator.LESS_EQUAL) && currValue <= value) {
return true;
}
return false;
}
@Override
public boolean equals(Object obj) {
if (obj == this)
return true;
if (obj == null || obj.getClass() != getClass())
return false;
MetricGoal vg = (MetricGoal) obj;
if (vg.getStatus() != status)
return false;
if (!vg.property.equals(property))
return false;
if (vg.value != value)
return false;
if (!vg.comparator.equals(comparator))
return false;
return true;
}
public static MetricGoalBuilder newBuilder() {
return new MetricGoalBuilder();
}
public VehicleProperty getProperty() {
return property;
}
public Comparator getComparator() {
return comparator;
}
public double getValue() {
return value;
}
public String getUnit() {
return unit;
}
}
package de.rwth.montisim.simulation.vehicle.task.goal;
import de.rwth.montisim.commons.utils.Comparator;
public class MetricGoalBuilder extends GoalBuilder<MetricGoalBuilder> {
VehicleProperty property;
Comparator comparator;
double targetValue;
String targetUnit;
public MetricGoalBuilder setProperty(VehicleProperty property) {
this.property = property;
return getThis();
}
public MetricGoalBuilder lessEqual(double value, String unit) {
comparator = Comparator.LESS_EQUAL;
targetValue = value;
targetUnit = unit;
return getThis();
}
public MetricGoalBuilder less(double value, String unit) {
comparator = Comparator.LESS;
targetValue = value;
targetUnit = unit;
return getThis();
}
public MetricGoalBuilder greaterEqual(double value, String unit) {
comparator = Comparator.GREATER_EQUAL;
targetValue = value;
targetUnit = unit;
return getThis();
}
public MetricGoalBuilder greater(double value, String unit) {
comparator = Comparator.GREATER;
targetValue = value;
targetUnit = unit;
return getThis();
}
public MetricGoalBuilder equals(double value, String unit) {
comparator = Comparator.EQUAL;
targetValue = value;
targetUnit = unit;
return getThis();
}
@Override
protected MetricGoalBuilder getThis() {
return this;
}
@Override
public Goal build() {
return new MetricGoal(ltlOperator, property, comparator, targetValue, targetUnit);
}
}
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.json.Typed;
import de.rwth.montisim.simulation.vehicle.Vehicle;
import java.util.List;
@Typed
public class PathGoal extends Goal {
private List<Vec2> path;
private double radius;
private int currDestIdx;
public PathGoal() {
}
public PathGoal(LTLOperator ltlOperator, List<Vec2> path, double range) {
this.ltlOperator = ltlOperator;
this.path = path;
this.radius = range;
this.currDestIdx = 0;
}
public void update(Vehicle v) {
Vec2 dest = path.get(currDestIdx);
double dist = v.physicalObject.pos.asVec2().distance(dest);
// nothing to update if current destination have not been reached yet
if (dist > radius) {
return;
}
// System.out.printf("%.2fm to target\n", dist);
// a coordinate in path is reached,
// update next destination and goal status
currDestIdx += 1;
if (currDestIdx < path.size()) {
updateStatus(TaskStatus.RUNNING);
} else {
// goal is successful if all destinations have been reached
updateStatus(TaskStatus.SUCCEEDED);
}
}
@Override
public boolean equals(Object obj) {
if (obj == this)
return true;
if (obj == null || obj.getClass() != getClass())
return false;
PathGoal goal = (PathGoal) obj;
if (goal.getStatus() != status)
return false;
if (!goal.path.equals(path))
return false;
return true;
}
public static PathGoalBuilder newBuilder() {
return new PathGoalBuilder();
}
}
package de.rwth.montisim.simulation.vehicle.task.goal;
import de.rwth.montisim.commons.utils.Vec2;
import java.util.LinkedList;
import java.util.List;
import java.util.Optional;
public class PathGoalBuilder extends GoalBuilder<PathGoalBuilder> {
final static double DEFAULT_RANGE = 10d;
List<Vec2> coords;
Optional<Double> range;
@Override
protected PathGoalBuilder getThis() {