Commit 8a488285 authored by Jean Meurice's avatar Jean Meurice
Browse files

Task Integration, Vis Updates (Inspector, Trajectory), Control update

parent 5f329ef4
Pipeline #371749 failed with stage
in 58 seconds
......@@ -3,25 +3,27 @@ package de.rwth.montisim.simulation.eecomponents.autopilots;
import java.time.Duration;
import java.time.Instant;
import java.util.*;
import de.rwth.montisim.commons.dynamicinterface.BasicType;
import de.rwth.montisim.commons.simulation.Inspectable;
import de.rwth.montisim.commons.utils.Geometry;
import de.rwth.montisim.commons.utils.IPM;
import de.rwth.montisim.commons.utils.Time;
import de.rwth.montisim.commons.utils.Vec2;
import de.rwth.montisim.simulation.eecomponents.navigation.Navigation;
import de.rwth.montisim.simulation.eesimulator.actuator.Actuator;
import de.rwth.montisim.simulation.eesimulator.components.EEComponent;
import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent;
import de.rwth.montisim.simulation.eesimulator.exceptions.EEMessageTypeException;
import de.rwth.montisim.simulation.eesimulator.message.Message;
import de.rwth.montisim.simulation.eesimulator.message.MessageInformation;
import de.rwth.montisim.simulation.vehicle.navigation.Navigation;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueVelocity;
import de.rwth.montisim.simulation.vehicle.powertrain.PowerTrainProperties;
public class JavaAutopilot extends EEComponent {
public class JavaAutopilot extends EEComponent implements Inspectable {
transient final JavaAutopilotProperties properties;
public static final double MAX_DEVIATION = 3; // max allowed deviation from the trajectory "corners" (in meters)
......@@ -32,6 +34,7 @@ public class JavaAutopilot extends EEComponent {
transient MessageInformation positionMsg;
transient MessageInformation compassMsg;
transient MessageInformation trajLengthMsg;
transient MessageInformation trajXMsg;
transient MessageInformation trajYMsg;
......@@ -44,9 +47,15 @@ public class JavaAutopilot extends EEComponent {
public double currentCompass = Double.NaN;
double newTrajX[] = null;
public int newTrajLength = 0;
public int trajLength = 0;
public double trajX[] = null;
public double trajY[] = null;
public double currentGas = 0;
public double currentSteering = 0;
public double currentBrakes = 0;
Instant lastTime = null;
transient final PID speedPid;
transient final PID turnPid;
......@@ -64,6 +73,7 @@ public class JavaAutopilot extends EEComponent {
this.positionMsg = addInput(TruePosition.VALUE_NAME, TruePosition.TYPE);
this.compassMsg = addInput(TrueCompass.VALUE_NAME, TrueCompass.TYPE);
this.trajLengthMsg = addInput(Navigation.TRAJECTORY_LENGTH_MSG, BasicType.N);
this.trajXMsg = addInput(Navigation.TRAJECTORY_X_MSG, Navigation.TRAJECTORY_X_TYPE);
this.trajYMsg = addInput(Navigation.TRAJECTORY_Y_MSG, Navigation.TRAJECTORY_Y_TYPE);
......@@ -84,11 +94,14 @@ public class JavaAutopilot extends EEComponent {
compute(msgRecvEvent.getEventTime());
} else if (msg.isMsg(compassMsg)) {
currentCompass = (Double) msg.message;
} else if (msg.isMsg(trajLengthMsg)){
newTrajLength = (int)msg.message;
} else if (msg.isMsg(trajXMsg)) {
// Assumes the x positions array of a new trajectory always arrives first
newTrajX = (double[]) msg.message;
} else if (msg.isMsg(trajYMsg)) {
// Assumes the y positions array of a new trajectory always arrives last
trajY = (double[]) msg.message;
trajLength = newTrajLength;
trajX = newTrajX;
}
}
......@@ -144,6 +157,19 @@ public class JavaAutopilot extends EEComponent {
int mode = 0;
int target = 0;
void setGas(Instant sendTime, double val) {
this.currentGas = val;
sendMessage(sendTime, accelMsg, val);
}
void setBrakes(Instant sendTime, double val) {
this.currentBrakes = val;
sendMessage(sendTime, brakeMsg, val);
}
void setSteering(Instant sendTime, double val) {
this.currentSteering = val;
sendMessage(sendTime, steeringMsg, val);
}
void compute(Instant startTime) {
if (currentPosition == null || Double.isNaN(currentCompass))
return;
......@@ -153,14 +179,14 @@ public class JavaAutopilot extends EEComponent {
int index = getNearestSegment(currentPosition);
if (index < 0) {
// No trajectory -> Stay in place
sendMessage(sendTime, accelMsg, 0.0);
sendMessage(sendTime, brakeMsg, 1.0);
setGas(sendTime, 0.0);
setBrakes(sendTime, 1.0);
return;
}
carDir.set(Math.cos(carAngle), Math.sin(carAngle));
if (trajX.length == 1 || index + 1 >= trajX.length) {
if (trajLength == 1 || index + 1 >= trajLength) {
// Only one point -> orient towards it
// If "behind" -> just stop
// Orient and try to stop at position
......@@ -230,8 +256,9 @@ public class JavaAutopilot extends EEComponent {
speedOutput /= 3.6; // Convert to m/s related space
speedOutput /= properties.maxVehicleAccel; // Convert to [0:1] actuator range
sendMessage(sendTime, steeringMsg, turnOutput);
sendMessage(sendTime, accelMsg, speedOutput);
setSteering(sendTime, turnOutput);
setGas(sendTime, speedOutput);
setBrakes(sendTime, 0);
}
private double getDeltaTime(Instant time) {
......@@ -246,7 +273,7 @@ public class JavaAutopilot extends EEComponent {
SegmentPos findTargetSegment(int index) {
currSeg.initFromTraj(index);
boolean hasNext = index + 2 < trajX.length;
boolean hasNext = index + 2 < trajLength;
if (hasNext) {
nextSeg.initFromTraj(index + 1);
......@@ -286,7 +313,7 @@ public class JavaAutopilot extends EEComponent {
return -1;
double currentNearestDistance = Double.POSITIVE_INFINITY;
int closestIndex = -1;
int count = trajX.length;
int count = trajLength;
double dist;
boolean hasLastPoint = false;
......@@ -338,4 +365,46 @@ public class JavaAutopilot extends EEComponent {
target.set(trajX[index], trajY[index]);
}
@Override
public String getType() {
return "autopilot";
}
void addEntry(List<String> entries, boolean output, MessageInformation msgInf, Object val) {
String res = output ? "output: " : "input: ";
res += msgInf.name + ": ";
if (val == null) entries.add(res + "null");
else {
List<String> toStr = msgInf.type.toString(val);
if (toStr.size() == 0) entries.add(res + "No toString()");
if (toStr.size() == 1) entries.add(res + toStr.get(0));
else {
entries.add(res);
for (String s : toStr) {
entries.add(" "+s);
}
}
}
}
@Override
public List<String> getEntries() {
List<String> entries = new ArrayList<>();
addEntry(entries, false, velocityMsg, currentVelocity);
addEntry(entries, false, compassMsg, currentCompass);
addEntry(entries, false, positionMsg, currentPosition);
addEntry(entries, false, trajLengthMsg, trajLength);
addEntry(entries, false, trajXMsg, trajX);
addEntry(entries, false, trajYMsg, trajY);
addEntry(entries, true, accelMsg, currentGas);
addEntry(entries, true, steeringMsg, currentSteering);
addEntry(entries, true, brakeMsg, currentBrakes);
return entries;
}
@Override
public String getName() {
return properties.name;
}
}
\ No newline at end of file
......@@ -5,6 +5,7 @@ import java.util.Optional;
import de.rwth.montisim.commons.map.Pathfinding;
import de.rwth.montisim.commons.physicalvalue.PhysicalValueRegistry;
import de.rwth.montisim.commons.simulation.Destroyer;
import de.rwth.montisim.commons.simulation.Updater;
import de.rwth.montisim.simulation.eesimulator.message.MessagePriorityComparator;
......@@ -39,12 +40,14 @@ public abstract class EEComponentProperties {
public static class ComponentBuildContext {
public final PhysicalValueRegistry physicalValues;
public final Updater componentUpdater;
public final Destroyer componentDestroyer;
public final MessagePriorityComparator comp;
public final Pathfinding pathfinding;
public ComponentBuildContext(PhysicalValueRegistry physicalValues, Updater componentUpdater, MessagePriorityComparator msgComp, Pathfinding pathfinding){
public ComponentBuildContext(PhysicalValueRegistry physicalValues, Updater componentUpdater, Destroyer componentDestroyer, MessagePriorityComparator msgComp, Pathfinding pathfinding){
this.physicalValues = physicalValues;
this.componentUpdater = componentUpdater;
this.componentDestroyer = componentDestroyer;
this.comp = msgComp;
this.pathfinding = pathfinding;
}
......
......@@ -175,6 +175,12 @@ public class OsmMap {
}
}
public Node getNode(long osmId) {
Integer i = nodeLocalIDByOsmID.get(osmId);
if (i == null) return null;
return nodeTable.elementAt(i);
}
// Returns a local id for the node even if the node itself was not encountered yet and added.
private int getNodeID(long osmID){
Integer i = nodeLocalIDByOsmID.get(osmID);
......
......@@ -25,13 +25,11 @@
<properties>
<!-- USE THIS VARIABLE TO SET THE VERSION OF THIS PROJECT AND ALL SUB PROJECTS -->
<revision>3.1.0</revision>
<revision>3.1.1</revision>
<!-- USE THESE TO SET THE VERSIONS OF THE DEPENDENCIES FOR ALL THE SUB-PROJECTS -->
<commons.version>2.0.8</commons.version>
<commons.version>2.0.10</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>
......
......@@ -3,11 +3,19 @@ package de.rwth.montisim.simulation.simulator;
import java.io.File;
import java.time.*;
import java.util.Objects;
import java.util.Vector;
import de.rwth.montisim.commons.map.Pathfinding;
import de.rwth.montisim.commons.utils.Time;
import de.rwth.montisim.commons.utils.json.Json;
import de.rwth.montisim.commons.utils.json.SerializationException;
import de.rwth.montisim.simulation.eesimulator.exceptions.EEMessageTypeException;
import de.rwth.montisim.simulation.eesimulator.exceptions.EESetupException;
import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
import de.rwth.montisim.simulation.environment.osmmap.OsmMap;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.vehicle.Vehicle;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
public class SimulationConfig {
......@@ -22,4 +30,22 @@ public class SimulationConfig {
public static SimulationConfig fromFile(File file) throws SerializationException {
return Json.instantiateFromJson(file, SimulationConfig.class);
}
public Simulator build(World world, Pathfinding pathfinding, MessageTypeManager mtManager, OsmMap map) {
Objects.requireNonNull(world);
Objects.requireNonNull(pathfinding);
Objects.requireNonNull(mtManager);
Simulator sim = new Simulator(this, world, pathfinding, mtManager, map);
// Add Vehicles
for (VehicleProperties v : cars) {
Vehicle vehicle;
try {
vehicle = sim.getVehicleBuilder(v).build();
} catch (SerializationException | EEMessageTypeException | EESetupException e) {
throw new IllegalStateException(e);
}
sim.addSimulationObject(vehicle);
}
return sim;
}
}
\ No newline at end of file
......@@ -3,6 +3,7 @@ package de.rwth.montisim.simulation.simulator;
import java.time.Instant;
import de.rwth.montisim.commons.simulation.TaskStatus;
import de.rwth.montisim.commons.simulation.TimeUpdate;
public class SimulationLoop {
......@@ -15,17 +16,20 @@ public class SimulationLoop {
this.simulationTime = config.start_time;
}
public boolean run(){
public TaskStatus run() {
try {
while (!simulator.finished()){
do {
TaskStatus res = simulator.status();
if (res != TaskStatus.RUNNING) return res;
TimeUpdate tu = new TimeUpdate(simulationTime, config.tick_duration);
simulator.update(tu);
simulationTime = tu.newTime;
}
} while(true);
} catch(Exception e){
e.printStackTrace();
return false;
return TaskStatus.FAILED;
}
return true;
}
}
\ No newline at end of file
......@@ -2,6 +2,8 @@
package de.rwth.montisim.simulation.simulator;
import java.time.Duration;
import java.util.Collection;
import java.util.HashMap;
import java.util.Vector;
import de.rwth.montisim.commons.map.Pathfinding;
......@@ -9,13 +11,26 @@ import de.rwth.montisim.commons.simulation.*;
import de.rwth.montisim.commons.utils.json.Json;
import de.rwth.montisim.simulation.eecomponents.autopilots.JavaAutopilotProperties;
import de.rwth.montisim.simulation.eecomponents.autopilots.TestAutopilotProperties;
import de.rwth.montisim.simulation.eecomponents.navigation.NavigationProperties;
import de.rwth.montisim.simulation.eesimulator.actuator.ActuatorProperties;
import de.rwth.montisim.simulation.eesimulator.bridge.BridgeProperties;
import de.rwth.montisim.simulation.eesimulator.bus.can.CANProperties;
import de.rwth.montisim.simulation.eesimulator.bus.constant.ConstantBusProperties;
import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
import de.rwth.montisim.simulation.eesimulator.testcomponents.TestCompProperties;
import de.rwth.montisim.simulation.environment.osmmap.OsmMap;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.simulator.vehicleconfigs.DefaultVehicleConfig;
import de.rwth.montisim.simulation.vehicle.Vehicle;
import de.rwth.montisim.simulation.vehicle.VehicleBuilder;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.VehicleProperties.BuildContext;
import de.rwth.montisim.simulation.vehicle.navigation.NavigationProperties;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.fuel.FuelPTProperties;
import de.rwth.montisim.simulation.vehicle.task.metric.MetricGoalProperties;
import de.rwth.montisim.simulation.vehicle.task.path.PathGoalProperties;
public class Simulator implements ISimulator, Updatable {
public final SimulationConfig config;
......@@ -33,21 +48,36 @@ public class Simulator implements ISimulator, Updatable {
Vector<Destroyable> destroyables = new Vector<>();
Vector<TaskRunner> taskRunners = new Vector<>();
HashMap<String, Vehicle> vehicles = new HashMap<>();
boolean timeout = false;
public Simulator(SimulationConfig config, World world, Pathfinding pathfinding, MessageTypeManager mtManager) {
// OsmMap can be null
public Simulator(SimulationConfig config, World world, Pathfinding pathfinding, MessageTypeManager mtManager, OsmMap map) {
this.config = config;
this.world = world;
this.pathfinding = pathfinding;
this.mtManager = mtManager;
this.buildContext = new BuildContext(pathfinding, mtManager);
this.buildContext = new BuildContext(pathfinding, mtManager, world, map);
// TODO load static objects of the World
}
public void addSimulationObject(SimulationObject obj) {
obj.state = new SimulatorState();
obj.registerComponents(this);
if (obj instanceof Vehicle) {
Vehicle v = (Vehicle) obj;
if (vehicles.containsKey(v.properties.vehicleName)) throw new IllegalArgumentException("Error on adding Vehicle '"+v.properties.vehicleName+"' to the simulation: a vehicle with this name is already registered.");
vehicles.put(v.properties.vehicleName, v);
}
}
public Vehicle getVehicle(String name) {
return vehicles.get(name);
}
public Collection<Vehicle> getVehicles() {
return vehicles.values();
}
public void removeSimulationObject(SimulationObject obj) {
......@@ -65,6 +95,10 @@ public class Simulator implements ISimulator, Updatable {
taskRunners.set(state.taskRunnerId, null);
state.reset();
if (obj instanceof Vehicle) {
Vehicle v = (Vehicle) obj;
vehicles.remove(v.properties.vehicleName);
}
}
public VehicleBuilder getVehicleBuilder(VehicleProperties config) {
......@@ -86,17 +120,14 @@ public class Simulator implements ISimulator, Updatable {
timeout = simulatedTime.compareTo(config.max_duration) > 0;
}
public boolean finished() {
/**
* @return SUCCEEDED if all tasks succeeded, FAILED if timeout and RUNNING else.
*/
public TaskStatus status() {
if (timeout) {
return true;
}
for (TaskRunner r : taskRunners) {
if (r == null)
continue;
if (r.status() != TaskStatus.SUCCEEDED)
return false;
return TaskStatus.FAILED;
}
return true;
return allTasksSucceeded() ? TaskStatus.SUCCEEDED : TaskStatus.RUNNING;
}
public boolean allTasksSucceeded() {
......@@ -113,6 +144,13 @@ public class Simulator implements ISimulator, Updatable {
return true;
}
// Call to clean-up all object that require explicit clean-up (ex: hardware_emulator)
public void destroy() {
for (Destroyable d : destroyables) {
d.destroy();
}
}
private static class SimulatorState implements ISimulatorState {
int staticId;
int dynamicId;
......@@ -172,10 +210,24 @@ public class Simulator implements ISimulator, Updatable {
taskRunners.add(runner);
}
static {
public static void registerJsonTypes() {
Json.registerType(NavigationProperties.class);
Json.registerType(JavaAutopilotProperties.class);
Json.registerType(TestAutopilotProperties.class);
Json.registerType(ElectricalPTProperties.class);
Json.registerType(FuelPTProperties.class);
Json.registerType(NavigationProperties.class);
Json.registerType(JavaAutopilotProperties.class);
Json.registerType(TestAutopilotProperties.class);
Json.registerType(RigidbodyPhysicsProperties.class);
Json.registerType(ActuatorProperties.class);
Json.registerType(BridgeProperties.class);
Json.registerType(CANProperties.class);
Json.registerType(ConstantBusProperties.class);
Json.registerType(SensorProperties.class);
Json.registerType(TestCompProperties.class);
Json.registerType(PathGoalProperties.class);
Json.registerType(MetricGoalProperties.class);
}
public Vector<Updatable> getUpdatables() {
......
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;
}
}
<
......@@ -4,17 +4,18 @@ 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;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.navigation.NavigationProperties;
import de.rwth.montisim.simulation.vehicle.physicalvalues.*;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
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;
import de.rwth.montisim.simulation.vehicle.task.TaskProperties;
import de.rwth.montisim.simulation.vehicle.task.path.PathGoalProperties;
public class DefaultVehicleConfig {
public final VehicleProperties properties;
......@@ -64,7 +65,7 @@ public class DefaultVehicleConfig {
);
properties.addComponent(
new NavigationProperties().setName("Navigation")