Commit 4c76a70f authored by Jean Meurice's avatar Jean Meurice
Browse files

IntegrationTest & JavaAutopilot

parent f90eb60d
Pipeline #292324 passed with stage
in 1 minute and 12 seconds
......@@ -11,4 +11,5 @@
*logs*
.flattened-pom.xml
.vscode/
./lib/
\ No newline at end of file
./lib/
**/vis
\ No newline at end of file
......@@ -115,7 +115,7 @@ public class JavaAutopilot extends EEComponent {
/** Assumes posStart & posEnd are set */
void init() {
IPM.subtractToVec(posEnd, posStart, dir);
IPM.subtractTo(dir, posEnd, posStart);
length = dir.magnitude();
if (length > 0.001){
IPM.multiply(dir, 1/length);
......@@ -124,7 +124,7 @@ public class JavaAutopilot extends EEComponent {
}
normal.set(-dir.y, dir.x);
IPM.subtractToVec(currentPosition, posStart, relPos);
IPM.subtractTo(relPos, currentPosition, posStart);
projPos = IPM.dot(dir, relPos);
orthoPos = IPM.dot(normal, relPos);
dist = Math.abs(orthoPos);
......@@ -295,7 +295,7 @@ public class JavaAutopilot extends EEComponent {
// Check segment
// 1) Get segment "normal"
IPM.subtractToVec(point, lastPoint, dir);
IPM.subtractTo(dir, point, lastPoint);
// Manual normalization to keep the length
double length = dir.magnitude();
if (length > 0.001){
......@@ -305,7 +305,7 @@ public class JavaAutopilot extends EEComponent {
}
// 2) check if in segment bounds
IPM.subtractToVec(pos, lastPoint, delta);
IPM.subtractTo(delta, pos, lastPoint);
double projPos = IPM.dot(dir, delta);
if (projPos > 0 && projPos < length) {
......
......@@ -177,7 +177,7 @@ public class Navigation extends EEComponent {
// Check segment
// 1) Get segment "normal"
IPM.subtractToVec(point, lastPoint, dir);
IPM.subtractTo(dir, point, lastPoint);
// Manual normalization to keep the length
double length = dir.magnitude();
if (length > 0.001){
......@@ -187,7 +187,7 @@ public class Navigation extends EEComponent {
}
// 2) check if in segment bounds
IPM.subtractToVec(pos, lastPoint, delta);
IPM.subtractTo(delta, pos, lastPoint);
double projPos = IPM.dot(dir, delta);
if (projPos > 0 && projPos < length) {
......
......@@ -88,7 +88,7 @@ public class PathfindingImpl implements Pathfinding {
// Check segment
// 1) Get segment "normal"
IPM.subtractToVec(p, lastPoint, dir);
IPM.subtractTo(dir, p, lastPoint);
// Manual normalization to keep the length
double length = dir.magnitude();
if (length > 0.001){
......@@ -98,7 +98,7 @@ public class PathfindingImpl implements Pathfinding {
}
// 2) check if in segment bounds
IPM.subtractToVec(pos, lastPoint, delta);
IPM.subtractTo(delta, pos, lastPoint);
double projPos = IPM.dot(dir, delta);
if (projPos > 0 && projPos < length) {
......@@ -109,7 +109,7 @@ public class PathfindingImpl implements Pathfinding {
currentNearestDistance = dist;
ref.roadSegmentID = rsID;
ref.isPoint = false;
IPM.multiplyToVec(dir, projPos, ref.point);
IPM.multiplyTo(ref.point, dir, projPos);
IPM.add(ref.point, lastPoint);
ref.roadPointID = i-inc;
ref.distance = totalDist + projPos;
......
......@@ -28,7 +28,7 @@
<revision>3.0.0</revision>
<!-- USE THESE TO SET THE VERSIONS OF THE DEPENDENCIES FOR ALL THE SUB-PROJECTS -->
<commons.version>2.0.0</commons.version>
<commons.version>2.0.1</commons.version>
<controller.version>1.1.0</controller.version>
<server.version>2.0.1</server.version>
<hardwareemulator.version>1.0.6</hardwareemulator.version>
......
......@@ -9,4 +9,6 @@ public class SimulationConfig {
Duration maxSimulationDuration = Duration.ofSeconds(60);
Duration tickDuration = Duration.ofNanos(Time.SECOND_TO_NANOSEC/100);
Instant simulationStart = Instant.now();
}
\ No newline at end of file
......@@ -8,6 +8,7 @@ import de.rwth.montisim.commons.simulation.*;
import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
import de.rwth.montisim.simulation.environment.pathfinding.Pathfinding;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.simulator.vehicleconfigs.DefaultVehicleConfig;
import de.rwth.montisim.simulation.vehicle.VehicleBuilder;
import de.rwth.montisim.simulation.vehicle.config.VehicleConfig;
......@@ -25,7 +26,9 @@ public class Simulator implements ISimulator, Updatable {
Vector<Updatable> updatables = new Vector<>();
Vector<Destroyable> destroyables = new Vector<>();
Vector<TaskRunner> taskRunners = new Vector<>();
boolean timeout = false;
public Simulator(SimulationConfig config, World world, Pathfinding pathfinding, MessageTypeManager mtManager) {
this.config = config;
......@@ -43,8 +46,8 @@ public class Simulator implements ISimulator, Updatable {
return new VehicleBuilder(mtManager, pathfinding, config);
}
public VehicleBuilder getDefaultVehicleBuilder(VehicleConfig config) {
return new VehicleBuilder(mtManager, pathfinding);
public VehicleBuilder getDefaultVehicleBuilder() {
return new VehicleBuilder(mtManager, pathfinding, new DefaultVehicleConfig());
}
@Override
......@@ -52,12 +55,25 @@ public class Simulator implements ISimulator, Updatable {
// TODO: OBSERVERS
updatables.forEach(x -> x.update(newTime));
simulatedTime = simulatedTime.plus(newTime.deltaTime);
timeout = simulatedTime.compareTo(config.maxSimulationDuration) > 0;
}
public boolean finished() {
if (simulatedTime.compareTo(config.maxSimulationDuration) > 0) return true;
// TODO check scenario conditions
return false;
if (timeout) {
return true;
}
for (TaskRunner r : taskRunners){
if (r.status() != TaskStatus.SUCCEEDED) return false;
}
return true;
}
public boolean allTasksSucceeded() {
for (TaskRunner r : taskRunners){
if (r.status() != TaskStatus.SUCCEEDED) return false;
}
if (timeout) return taskRunners.size() == 0;
return true;
}
@Override
......@@ -80,4 +96,9 @@ public class Simulator implements ISimulator, Updatable {
destroyables.add(destroyable);
}
@Override
public void registerTaskRunner(TaskRunner runner) {
taskRunners.add(runner);
}
}
\ No newline at end of file
......@@ -3,6 +3,7 @@ package de.rwth.montisim.simulation.simulator.vehicleconfigs;
import java.time.Duration;
import de.rwth.montisim.simulation.eecomponents.autopilots.JavaAutopilotProperties;
import de.rwth.montisim.simulation.eecomponents.navigation.NavigationProperties;
import de.rwth.montisim.simulation.eesimulator.bus.constant.ConstantBusProperties;
import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
......@@ -70,4 +71,15 @@ public class DefaultVehicleConfig extends VehicleConfig {
);
}
public static DefaultVehicleConfig withJavaAutopilot() {
DefaultVehicleConfig config = new DefaultVehicleConfig();
double maxForce = config.electricalPTProperties.motorProperties.motorPeekTorque *
config.electricalPTProperties.transmissionRatio * 2 /
config.properties.wheels.wheelDiameter;
config.eeConfig.addComponent(
new JavaAutopilotProperties(maxForce/config.properties.body.mass).setName("TestAutopilot")
);
return config;
}
}
\ No newline at end of file
/* (c) https://github.com/MontiCore/monticore */
package de.rwth.montisim.simulation.vehicle.config;
package de.rwth.montisim.simulation.simulator.vehicleconfigs;
import java.time.Duration;
import de.rwth.montisim.simulation.eecomponents.autopilots.TestAutopilotProperties;
import de.rwth.montisim.simulation.eesimulator.bus.constant.ConstantBusProperties;
import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.config.*;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueVelocity;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
......@@ -42,6 +44,30 @@ public class TestVehicleConfig extends VehicleConfig {
eeConfig.addComponent(
ConstantBusProperties.instantBus().setName("DefaultBus")
);
}
static public TestVehicleConfig newCircleAutopilotConfig(double maxSpeed, double turnRadius){
TestVehicleConfig config = new TestVehicleConfig();
double maxForce = config.electricalPTProperties.motorProperties.motorPeekTorque *
config.electricalPTProperties.transmissionRatio * 2 /
config.properties.wheels.wheelDiameter;
config.eeConfig.addComponent(
TestAutopilotProperties.circleAutopilot(Duration.ofMillis(1), maxForce/config.properties.body.mass, maxSpeed, turnRadius).setName("TestAutopilot")
);
return config;
}
static public TestVehicleConfig newStartStopAutopilotConfig(double targetSpeed){
TestVehicleConfig config = new TestVehicleConfig();
double maxForce = config.electricalPTProperties.motorProperties.motorPeekTorque *
config.electricalPTProperties.transmissionRatio * 2 /
config.properties.wheels.wheelDiameter;
config.eeConfig.addComponent(
TestAutopilotProperties.startStopAutopilot(Duration.ofMillis(1), maxForce/config.properties.body.mass, targetSpeed).setName("TestAutopilot")
);
return config;
}
}
\ No newline at end of file
......@@ -10,7 +10,7 @@ import de.rwth.montisim.commons.simulation.TimeUpdate;
import de.rwth.montisim.commons.utils.*;
import de.rwth.montisim.simulation.eesimulator.exceptions.*;
import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
import de.rwth.montisim.simulation.eecomponents.autopilots.TestAutopilotProperties;
import de.rwth.montisim.simulation.simulator.vehicleconfigs.TestVehicleConfig;
import de.rwth.montisim.simulation.simulator.visualization.car.CarRenderer;
import de.rwth.montisim.simulation.simulator.visualization.plotter.TimePlotter;
import de.rwth.montisim.simulation.simulator.visualization.ui.*;
......@@ -104,21 +104,11 @@ public class PhysicsDebug extends JFrame implements SimulationRunner {
}
private VehicleConfig setupTurningCar(){
TestVehicleConfig config = new TestVehicleConfig();
double maxForce = config.electricalPTProperties.motorProperties.motorPeekTorque *
config.electricalPTProperties.transmissionRatio * 2 /
config.properties.wheels.wheelDiameter;
double turnRadius = 30;
double maxSpeed = 0.8*270*Math.pow(turnRadius, -0.5614);
System.out.println("MaxSpeed: "+Double.toString(maxSpeed));
config.eeConfig.addComponent(
TestAutopilotProperties.circleAutopilot(Duration.ofMillis(1), maxForce/config.properties.body.mass, maxSpeed, turnRadius).setName("TestAutopilot")
);
return config;
return TestVehicleConfig.newCircleAutopilotConfig(maxSpeed, turnRadius);
}
@Override
......
......@@ -15,9 +15,7 @@ import de.rwth.montisim.simulation.environment.osmmap.*;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.environment.pathfinding.*;
import de.rwth.montisim.simulation.simulator.*;
import de.rwth.montisim.simulation.eecomponents.autopilots.JavaAutopilotProperties;
import de.rwth.montisim.simulation.eecomponents.autopilots.TestAutopilotProperties;
import de.rwth.montisim.simulation.simulator.vehicleconfigs.DefaultVehicleConfig;
import de.rwth.montisim.simulation.simulator.vehicleconfigs.*;
import de.rwth.montisim.simulation.simulator.visualization.car.CarRenderer;
import de.rwth.montisim.simulation.simulator.visualization.map.*;
import de.rwth.montisim.simulation.simulator.visualization.plotter.TimePlotter;
......@@ -48,7 +46,6 @@ public class SimulationVisualizer extends JFrame implements SimulationRunner {
final Viewer2D viewer;
final Control control;
final CarRenderer cr;
final DefaultVehicleConfig setup;
final TimePlotter plotter;
MessageTypeManager mtManager;
......@@ -72,7 +69,6 @@ public class SimulationVisualizer extends JFrame implements SimulationRunner {
vis1(args);
setup = new DefaultVehicleConfig();
cr = new CarRenderer();
viewer.addRenderer(cr);
viewer.setZoom(20);
......@@ -116,7 +112,7 @@ public class SimulationVisualizer extends JFrame implements SimulationRunner {
simulator = new Simulator(new SimulationConfig(), world, pathfinding, mtManager);
//VehicleConfig config = setupTurningCar();
VehicleConfig config = setupJavaAutopilotCar();
VehicleConfig config = DefaultVehicleConfig.withJavaAutopilot();
try {
vehicle = simulator.getVehicleBuilder(config).setName("TestVehicle").build();
simulator.addSimulationObject(vehicle);
......@@ -126,7 +122,10 @@ public class SimulationVisualizer extends JFrame implements SimulationRunner {
cr.setCar(vehicle);
Navigation nav = (Navigation) vehicle.eesimulator.getComponentManager().getComponent("Navigation").get();
nav.pushTargetPos(new Vec2(384.77, -283.72));
Vec2 TARGET_POS = new Vec2(-63.83, -171.96);
//nav.pushTargetPos(new Vec2(384.77, -283.72));
nav.pushTargetPos(TARGET_POS);
vehicle.addTarget(TARGET_POS);
} catch (EEMessageTypeException e) {
// TODO Auto-generated catch block
......@@ -134,36 +133,12 @@ public class SimulationVisualizer extends JFrame implements SimulationRunner {
}
}
private VehicleConfig setupJavaAutopilotCar(){
DefaultVehicleConfig config = new DefaultVehicleConfig();
double maxForce = config.electricalPTProperties.motorProperties.motorPeekTorque *
config.electricalPTProperties.transmissionRatio * 2 /
config.properties.wheels.wheelDiameter;
config.eeConfig.addComponent(
new JavaAutopilotProperties(maxForce/config.properties.body.mass).setName("TestAutopilot")
);
return config;
}
private VehicleConfig setupTurningCar(){
DefaultVehicleConfig config = new DefaultVehicleConfig();
double maxForce = config.electricalPTProperties.motorProperties.motorPeekTorque *
config.electricalPTProperties.transmissionRatio * 2 /
config.properties.wheels.wheelDiameter;
double turnRadius = 30;
double maxSpeed = 0.8*270*Math.pow(turnRadius, -0.5614);
System.out.println("MaxSpeed: "+Double.toString(maxSpeed));
config.eeConfig.addComponent(
TestAutopilotProperties.circleAutopilot(Duration.ofMillis(1), maxForce/config.properties.body.mass, maxSpeed, turnRadius).setName("TestAutopilot")
);
return config;
return TestVehicleConfig.newCircleAutopilotConfig(maxSpeed, turnRadius);
}
@Override
......
......@@ -53,7 +53,7 @@ public class CarRenderer extends Renderer {
public void setCar(Vehicle vehicle){
VehicleProperties p = vehicle.properties;
this.car = Optional.of(vehicle.physicalObject);
IPM.multiplyToVec(new Vec3(p.body.length, p.body.width, p.body.height), 0.5, half_size);
IPM.multiplyTo(half_size, new Vec3(p.body.length, p.body.width, p.body.height), 0.5);
for (int i = 0; i < points.length; ++i){
points[i] = new Vec3();
}
......@@ -64,7 +64,7 @@ public class CarRenderer extends Renderer {
public void setCar(DynamicObject car, Vec3 size){
this.car = Optional.of(car);
IPM.multiplyToVec(size, 0.5, half_size);
IPM.multiplyTo(half_size, size, 0.5);
for (int i = 0; i < points.length; ++i){
points[i] = new Vec3();
}
......
......@@ -126,7 +126,7 @@ public class WorldRenderer extends Renderer {
//Get displacement
if (i == s.pointsStart) displacement.set(normals.elementAt(j));
else if (i == s.pointsEnd) displacement.set(normals.elementAt(j-1));
else IPM.midpointToVec(normals.elementAt(j), normals.elementAt(j-1), displacement);
else IPM.midpointTo(displacement, normals.elementAt(j), normals.elementAt(j-1));
p.points[j] = new Vec3(pi.x + displacement.x, pi.y + displacement.y, 1); // Transform to extended 2D coordinates
j++;
}
......
package de.rwth.montisim.simulation.simulator;
import java.io.File;
import java.time.Duration;
import org.junit.*;
import de.rwth.montisim.commons.utils.LibraryService;
import de.rwth.montisim.commons.utils.Vec2;
import de.rwth.montisim.commons.utils.Vec3;
import de.rwth.montisim.simulation.eecomponents.navigation.Navigation;
import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
import de.rwth.montisim.simulation.environment.osmmap.*;
import de.rwth.montisim.simulation.environment.pathfinding.*;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.simulator.vehicleconfigs.DefaultVehicleConfig;
import de.rwth.montisim.simulation.vehicle.Vehicle;
public class IntegrationTest {
@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);
//System.out.println("Working Dir: " + LibraryService.getWorkingDirectory());
World world = new OsmToWorldLoader(new OsmMap("aachen", new File("src/test/resources/aachen.osm"))).getWorld();
Pathfinding pathfinding = new PathfindingImpl(world);
MessageTypeManager mtManager = new MessageTypeManager();
SimulationConfig config = new SimulationConfig();
config.maxSimulationDuration = Duration.ofSeconds(300);
Simulator simulator = new Simulator(config, world, pathfinding, mtManager);
SimulationLoop loop = new SimulationLoop(simulator, config);
DefaultVehicleConfig vConf = DefaultVehicleConfig.withJavaAutopilot();
Vehicle vehicle = simulator.getVehicleBuilder(vConf).setName("TestVehicle").build();
simulator.addSimulationObject(vehicle);
vehicle.physicsModel.setGroundPosition(new Vec3(0, 0, 0), new Vec2(START_DIR.x, START_DIR.y));
Navigation nav = (Navigation) vehicle.eesimulator.getComponentManager().getComponent("Navigation").get();
nav.pushTargetPos(TARGET_POS);
vehicle.addTarget(TARGET_POS);
Assert.assertTrue("Simulation Error", loop.run());
Assert.assertTrue("Vehicle did not reach target", simulator.allTasksSucceeded());
// TODO start at (0,0)
// Go to (-63.83, -171.96) in less than 45 secs
// TODO start at better position & orientation
// TODO add temporary "objective checker"
}
}
\ No newline at end of file
This diff is collapsed.
......@@ -2,14 +2,16 @@
package de.rwth.montisim.simulation.vehicle;
import java.util.HashMap;
import java.util.Optional;
import de.rwth.montisim.commons.simulation.*;
import de.rwth.montisim.commons.utils.Vec2;
import de.rwth.montisim.simulation.eesimulator.EESimulator;
import de.rwth.montisim.simulation.vehicle.physicalvalues.*;
import de.rwth.montisim.simulation.vehicle.physicsmodel.PhysicsModel;
import de.rwth.montisim.simulation.vehicle.powertrain.PowerTrain;
public class Vehicle extends Updater implements SimulationObject, Updatable, Destroyable {
public class Vehicle extends Updater implements SimulationObject, Updatable, Destroyable, TaskRunner {
// Register all the Physical Values statically HERE so that they are guaranteed to be loaded for the calls to 'getPhysicalValue()'
static {
PhysicalValueBuilder.registerPhysicalValueBuilder(TrueVelocity.VALUE_NAME, (Vehicle vehicle) -> new TrueVelocity(vehicle.physicalObject));
......@@ -24,6 +26,10 @@ public class Vehicle extends Updater implements SimulationObject, Updatable, Des
HashMap<String, PhysicalValue> physicalValues = new HashMap<>();
TaskStatus carStatus = TaskStatus.SUCCEEDED;
Optional<Vec2> target = Optional.empty();
Vec2 a = new Vec2();
/** Should only be for RESOLVING the physical values, not repeated access. */
public PhysicalValue getPhysicalValue(String name) {
PhysicalValue res = physicalValues.get(name);
......@@ -41,6 +47,7 @@ public class Vehicle extends Updater implements SimulationObject, Updatable, Des
protected Vehicle() {
}
int c = 0;
@Override
public void update(TimeUpdate newTime) {
// Update sensors & actuators WITH OLD TIME -> Gen new msgs & update actuation
......@@ -49,6 +56,28 @@ public class Vehicle extends Updater implements SimulationObject, Updatable, Des
// Update Physics & EESim (can be async)
eesimulator.update(newTime);
physicsModel.update(newTime);
// 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;
}
}
}
}
// TODO this is temp
public void addTarget(Vec2 t) {
carStatus = TaskStatus.RUNNING;
target = Optional.of(t);
}
@Override
......@@ -56,6 +85,7 @@ public class Vehicle extends Updater implements SimulationObject, Updatable, Des
simulator.registerUpdatable(this);
simulator.registerDynamicObject(physicalObject);
simulator.registerDestroyable(this);
simulator.registerTaskRunner(this);
}
@Override
......@@ -64,4 +94,10 @@ public class Vehicle extends Updater implements SimulationObject, Updatable, Des
}
@Override
public TaskStatus status() {
// TODO this is temp
return carStatus;
}
}
\ No newline at end of file
......@@ -27,13 +27,13 @@ public class VehicleBuilder {
final VehicleConfig config;
String vehicleName;
public VehicleBuilder(MessageTypeManager mtManager, Pathfinding pathfinding) {
this.mtManager = mtManager;
this.pathfinding = pathfinding;
// Load default setup
target = new Vehicle();
this.config = new TestVehicleConfig();
}
// public VehicleBuilder(MessageTypeManager mtManager, Pathfinding pathfinding) {
// this.mtManager = mtManager;
// this.pathfinding = pathfinding;
// // Load default setup
// target = new Vehicle();
// this.config = new TestVehicleConfig();
// }
public VehicleBuilder(MessageTypeManager mtManager, Pathfinding pathfinding, VehicleConfig config) {
this.mtManager = mtManager;
......@@ -89,7 +89,7 @@ public class VehicleBuilder {
// TODO
throw new IllegalArgumentException("Missing Modelica implementation");
case RIGIDBODY:
target.physicsModel = new RigidbodyPhysics(target);
target.physicsModel = new RigidbodyPhysics(target.powerTrain, target.properties);
break;
default:
break;
......