Commit 5f329ef4 authored by Evgeny Kusmenko's avatar Evgeny Kusmenko
Browse files

Merge branch 'feature-vehicle-task' into 'master'

Vehicle task

See merge request !34
parents bef97194 4fbef379
Pipeline #362722 passed with stage
in 1 minute and 14 seconds
......@@ -218,4 +218,8 @@ public class Navigation extends EEComponent {
return closestIndex;
}
public Stack<Vec2> getTargets() {
return targets;
}
}
\ No newline at end of file
......@@ -25,17 +25,17 @@
<properties>
<!-- USE THIS VARIABLE TO SET THE VERSION OF THIS PROJECT AND ALL SUB PROJECTS -->
<revision>3.0.1</revision>
<revision>3.1.0</revision>
<!-- USE THESE TO SET THE VERSIONS OF THE DEPENDENCIES FOR ALL THE SUB-PROJECTS -->
<commons.version>2.0.6</commons.version>
<commons.version>2.0.8</commons.version>
<controller.version>1.1.0</controller.version>
<server.version>2.0.1</server.version>
<hardwareemulator.version>1.0.6</hardwareemulator.version>
<assembly.plugin>2.5.4</assembly.plugin>
<compiler.plugin>3.3</compiler.plugin>
<source.plugin>2.4</source.plugin>
<source.plugin>3.2.0</source.plugin>
<java.version>1.8</java.version>
<jacoco.plugin>0.8.2</jacoco.plugin>
</properties>
......@@ -50,6 +50,13 @@
<scope>test</scope>
</dependency>
<dependency>
<groupId>org.mockito</groupId>
<artifactId>mockito-core</artifactId>
<version>3.5.13</version>
<scope>test</scope>
</dependency>
<dependency>
<groupId>montisim</groupId>
<artifactId>commons</artifactId>
......
......@@ -168,7 +168,7 @@ public class Simulator implements ISimulator, Updatable {
@Override
public void registerTaskRunner(SimulationObject obj, TaskRunner runner) {
SimulatorState state = (SimulatorState) obj.state;
state.updatableId = taskRunners.size();
state.taskRunnerId = taskRunners.size();
taskRunners.add(runner);
}
......@@ -177,4 +177,8 @@ public class Simulator implements ISimulator, Updatable {
Json.registerType(JavaAutopilotProperties.class);
Json.registerType(TestAutopilotProperties.class);
}
public Vector<Updatable> getUpdatables() {
return updatables;
}
}
\ No newline at end of file
package de.rwth.montisim.simulation.simulator;
import de.rwth.montisim.commons.map.Pathfinding;
import de.rwth.montisim.commons.utils.json.Json;
import de.rwth.montisim.commons.utils.json.SerializationException;
import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
import de.rwth.montisim.simulation.environment.world.World;
import java.util.Objects;
public class SimulatorBuilder {
boolean fromJson = false;
SimulationConfig config;
String jsonConfig;
World world;
Pathfinding pathfinding;
MessageTypeManager mtManager;
public static SimulatorBuilder fromJsonConfig(String jsonConfig) {
SimulatorBuilder builder = new SimulatorBuilder();
builder.fromJson = true;
builder.jsonConfig = jsonConfig;
return builder;
}
public static SimulatorBuilder withDefaultConfig() {
SimulatorBuilder bd = new SimulatorBuilder();
bd.config = new SimulationConfig();
return bd;
}
public Simulator build() throws SerializationException {
if (fromJson){
Objects.requireNonNull(jsonConfig);
config = Json.instantiateFromJson(jsonConfig, SimulationConfig.class);
fromJson = false;
return build();
}
// throw runtime exception if there are anything missing
Objects.requireNonNull(config);
Objects.requireNonNull(world);
Objects.requireNonNull(pathfinding);
Objects.requireNonNull(mtManager);
return new Simulator(config, world, pathfinding, mtManager);
}
public SimulatorBuilder setConfig(SimulationConfig config) {
this.config = config;
return this;
}
public SimulatorBuilder setWorld(World world) {
this.world = world;
return this;
}
public SimulatorBuilder setPathfinding(Pathfinding pathfinding) {
this.pathfinding = pathfinding;
return this;
}
public SimulatorBuilder setMtManager(MessageTypeManager mtManager) {
this.mtManager = mtManager;
return this;
}
}
......@@ -14,6 +14,7 @@ import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTPro
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.battery.BatteryProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.battery.BatteryProperties.BatteryType;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.motor.ElectricMotorProperties;
import de.rwth.montisim.simulation.vehicle.task.Task;
public class DefaultVehicleConfig {
public final VehicleProperties properties;
......@@ -87,4 +88,9 @@ public class DefaultVehicleConfig {
return this;
}
public DefaultVehicleConfig setTask(Task task) {
this.properties.setTask(task);
return this;
}
}
\ No newline at end of file
......@@ -2,7 +2,16 @@ 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.commons.utils.Coordinates;
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.MetricGoal;
import de.rwth.montisim.simulation.vehicle.task.goal.PathGoal;
import org.junit.*;
import de.rwth.montisim.commons.map.Pathfinding;
......@@ -17,34 +26,131 @@ import de.rwth.montisim.simulation.simulator.vehicleconfigs.DefaultVehicleConfig
import de.rwth.montisim.simulation.vehicle.Vehicle;
public class IntegrationTest {
final private String MAP_PATH = "src/test/resources/aachen.osm";
private OsmMap osmMap;
private World world;
@Before
public void initResource() throws Exception {
osmMap = new OsmMap("myMap", new File(MAP_PATH));
world = new OsmToWorldLoader(osmMap).getWorld();
world.finalizeGraph();
}
@Test
public void driveToTargetLocalCoordinate() throws Exception {
Vec3 startPos = new Vec3(1, 0, 0);
Vec2 targetPos = new Vec2(-63.83, -171.96);
Task task = new Task();
task.addGoal(PathGoal.newBuilder()
.eventually()
.arrive(startPos.asVec2())
.arrive(targetPos)
.withInRange(10)
.build());
task.addGoal(MetricGoal.newBuilder()
.setProperty("speed")
.never()
.greater(1000, "m/s")
.build());
driveToTarget(task);
}
@Test
public void driveToTarget() throws Exception {
final Vec3 START_DIR = new Vec3(1, 0, 0);
final Vec2 TARGET_POS = new Vec2(-63.83, -171.96);
public void driveToTargetGlobalCoordinate() throws Exception {
Coordinates startCoord = new Coordinates(6.0721450,50.7738916);
Coordinates targetCoord = new Coordinates(6.0690220,50.773491);
Task task = new Task();
task.addGoal(PathGoal.newBuilder()
.eventually()
.arrive(startCoord)
.arrive(targetCoord)
.withInRange(10)
.build());
task.getPathGoals().forEach(g -> g.convertCoordinate(osmMap, world));
driveToTarget(task);
}
@Test
public void driveToTargetGlobalOsmId() throws Exception {
long startOsmId = 3369847019l;
long targetOsmId = 444540939l;
Task task = new Task();
task.addGoal(PathGoal.newBuilder()
.eventually()
.arrive(startOsmId)
.arrive(targetOsmId)
.withInRange(10)
.build());
//System.out.println("Working Dir: " + LibraryService.getWorkingDirectory());
World world = new OsmToWorldLoader(new OsmMap("aachen", new File("src/test/resources/aachen.osm"))).getWorld();
task.getPathGoals().forEach(g -> g.convertCoordinate(osmMap, world));
driveToTarget(task);
}
public void driveToTarget(Task task) throws Exception {
Pathfinding pathfinding = new PathfindingImpl(world);
MessageTypeManager mtManager = new MessageTypeManager();
SimulationConfig config = new SimulationConfig();
config.max_duration = Duration.ofSeconds(300);
config.max_duration = Duration.ofSeconds(1000);
Simulator simulator = new Simulator(config, world, pathfinding, mtManager);
SimulationLoop loop = new SimulationLoop(simulator, config);
DefaultVehicleConfig vConf = DefaultVehicleConfig.withJavaAutopilot().setTask(task);
Vehicle vehicle = simulator.getVehicleBuilder(vConf.properties).setName("TestVehicle").build();
// Read positions from task
PathGoal pg = task.getPathGoals().get(0);
Vec2 startPos = pg.getPath().get(0);
Vec2 targetPos = pg.getPath().get(pg.getPath().size() - 1);
vehicle.physicsModel.setGroundPosition(new Vec3(0, 0, 0), new Vec2(startPos.x, startPos.y));
DefaultVehicleConfig vConf = DefaultVehicleConfig.withJavaAutopilot();
Vehicle vehicle = simulator.getVehicleBuilder(vConf.properties).setName("TestVehicle").build();
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);
nav.pushTargetPos(targetPos);
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(targetPos.x, targetPos.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(targetPos.x, targetPos.y, 0));
System.out.printf("dist: %.2fm, pos: %s\n", dist, vehicle.physicalObject.pos);
vehicle.addTarget(TARGET_POS);
// vehicle.addTarget(TARGET_POS);
Assert.assertTrue("Simulation Error", loop.run());
Assert.assertTrue("Vehicle did not reach target", simulator.allTasksSucceeded());
......
package de.rwth.montisim.simulation.simulator;
import de.rwth.montisim.commons.map.Pathfinding;
import de.rwth.montisim.commons.utils.json.Json;
import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
import de.rwth.montisim.simulation.environment.osmmap.OsmMap;
import de.rwth.montisim.simulation.environment.osmmap.OsmToWorldLoader;
import de.rwth.montisim.simulation.environment.pathfinding.PathfindingImpl;
import de.rwth.montisim.simulation.environment.world.World;
import junit.framework.TestCase;
import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
public class SimulatorBuilderTest extends TestCase {
public void testJson() throws Exception {
World world = new OsmToWorldLoader(new OsmMap("aachen", new File("src/test/resources/aachen.osm"))).getWorld();
Pathfinding pathfinding = new PathfindingImpl(world);
MessageTypeManager mtManager = new MessageTypeManager();
String configStr = new String(Files.readAllBytes(Paths.get("src/test/resources/simmulation_config.json")));
SimulationConfig config = Json.instantiateFromJson(configStr, SimulationConfig.class);
Simulator simulator = SimulatorBuilder
.fromJsonConfig(configStr)
.setWorld(world)
.setPathfinding(pathfinding)
.setMtManager(mtManager)
.build();
simulator.config.equals(config);
}
public void testWithDefaultConfig() throws Exception {
World world = new OsmToWorldLoader(new OsmMap("aachen", new File("src/test/resources/aachen.osm"))).getWorld();
Pathfinding pathfinding = new PathfindingImpl(world);
MessageTypeManager mtManager = new MessageTypeManager();
Simulator simulator = SimulatorBuilder
.withDefaultConfig()
.setWorld(world)
.setPathfinding(pathfinding)
.setMtManager(mtManager)
.build();
simulator.world.equals(world);
simulator.pathfinding.equals(pathfinding);
simulator.mtManager.equals(mtManager);
simulator.config.equals(new SimulationConfig());
}
}
\ No newline at end of file
{"max_duration":[60,0],"tick_duration":[0,10000000],"start_time":[1603901018,875625900],"cars":[]}
\ No newline at end of file
......@@ -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,27 @@ 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 Task getTask() {
return task;
}
/**
* For tests
*
* @return
*/
public PhysicalValueRegistry getPhysicalValues() {
return physicalValues;
}
}
\ No newline at end of file
/* (c) https://github.com/MontiCore/monticore */
package de.rwth.montisim.simulation.vehicle;
import java.util.Objects;
import java.util.Optional;
import java.util.Vector;
......@@ -23,6 +24,7 @@ import de.rwth.montisim.simulation.vehicle.physicsmodel.PhysicsProperties;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.PowerTrainProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
import de.rwth.montisim.simulation.vehicle.task.Task;
/**
* General properties of the Vehicle. All lengths are given in Meters. All
......@@ -46,6 +48,8 @@ public class VehicleProperties /* implements JsonSerializable */ {
public Vec2 start_coords = new Vec2();
public Vec2 end_coords;
public Task task;
public VehicleProperties(){
body = new BodyProperties();
wheels = new WheelProperties();
......@@ -118,6 +122,10 @@ public class VehicleProperties /* implements JsonSerializable */ {
components.add(properties);
}
public void setTask(Task task) {
this.task = task;
}
public static class BuildContext {
public final MessageTypeManager mtManager;
public final Pathfinding pathfinding;
......@@ -174,7 +182,9 @@ public class VehicleProperties /* implements JsonSerializable */ {
target.powerTrain.steeringActuator = cm.getActuator(PowerTrainProperties.STEERING_ACTUATOR_NAME).get();
target.eesystem.finalizeSetup();
Objects.requireNonNull(task);
target.task = task;
return target;
}
}
\ 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;
import java.util.stream.Collectors;
@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;
}
public List<PathGoal> getPathGoals() {
return goals.stream()
.filter(g -> g instanceof PathGoal)
.map(g -> (PathGoal) g)
.collect(Collectors.toList());
}
public List<MetricGoal> getMetricGoals() {
return goals.stream()
.filter(g -> g instanceof MetricGoal)
.map(g -> (MetricGoal) g)
.collect(Collectors.toList());
}
}