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 5fc0de6d authored by Jean Meurice's avatar Jean Meurice
Browse files

Merge branch 'sim_dev' into 'master'

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

See merge request !35
parents 5f329ef4 1df193bc
Pipeline #371753 passed with stage
in 1 minute and 13 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;