Commit 32031fbf authored by Frederik's avatar Frederik
Browse files

integrated vehicle

parent d532c16a
Pipeline #184895 failed with stage
in 1 minute and 7 seconds
......@@ -18,11 +18,11 @@
<properties>
<!-- USE THESE TO SET THE VERSIONS OF THE DEPENDENCIES FOR ALL THE SUB-PROJECTS -->
<montisim.commons.version>1.0.9</montisim.commons.version>
<montisim.controller.version>1.0.3</montisim.controller.version>
<montisim.commons.version>1.0.9-EE-SNAPSHOT</montisim.commons.version>
<montisim.controller.version>1.0.3-EE-SNAPSHOT</montisim.controller.version>
<montisim.server.version>1.0.7</montisim.server.version>
<montisim.hardware_emulator.version>1.0.0</montisim.hardware_emulator.version>
<montisim.simulation.version>2.0.2</montisim.simulation.version>
<montisim.simulation.version>2.0.2-EE-SNAPSHOT</montisim.simulation.version>
<project.build.sourceEncoding>UTF-8</project.build.sourceEncoding>
......
......@@ -23,6 +23,8 @@ import simulation.simulator.SimulationType;
import java.io.File;
import java.io.FileInputStream;
import java.time.Duration;
import java.time.Instant;
import java.util.List;
public class BasicController extends SimulationLoopNotifiable implements Runnable {
......@@ -70,7 +72,7 @@ public class BasicController extends SimulationLoopNotifiable implements Runnabl
//Setup simulator
Simulator.resetSimulator();
simulator = Simulator.getSharedInstance();
simulator.setSimulationDuration(config.max_sim_duration*1000);
simulator.setSimulationDuration(Duration.ofMillis(config.max_sim_duration*1000));
simulator.setSimulationLoopFrequency((int) config.sim_frequ);
simulator.setSimulationType(SimulationType.SIMULATION_TYPE_FIXED_TIME);
simulator.setStartDaytime(config.time.toDate());
......@@ -90,18 +92,18 @@ public class BasicController extends SimulationLoopNotifiable implements Runnabl
result.export();
}
@Override
public void willExecuteLoop(List<SimulationLoopExecutable> simulationObjects, long totalTime, long deltaTime) {
this.result.setup_next_frame(totalTime);
System.out.print("T: "+totalTime);
}
@Override
public void didExecuteLoopForObject(SimulationLoopExecutable simulationObject, long totalTime, long deltaTime) {
if (simulationObject instanceof PhysicalVehicle){
PhysicalVehicle vehicle = ((PhysicalVehicle) simulationObject);
if (this.result.add_car_frame(vehicle)){
simulator.setSimulationDuration(1000+simulator.getSimulationTime());
simulator.setSimulationDuration(Duration.between(Instant.EPOCH, simulator.getSimulationTime().plusMillis(1000)));
}
}
}
......
......@@ -4,145 +4,151 @@
* The license generally applicable for this project
* can be found under https://github.com/MontiCore/monticore
*/
package de.rwth.monticore.EmbeddedMontiArc.simulators.basic_simulator.controller;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.commons.BusEntry;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.interfaces.FunctionBlockInterface;
import org.apache.commons.math3.linear.RealVector;
import simulation.vehicle.PhysicalVehicle;
import de.rwth.monticore.EmbeddedMontiArc.simulators.hardware_emulator.HardwareEmulatorInterface;
import simulation.vehicle.VehicleActuatorType;
import java.io.Serializable;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
public class DirectModelAsFunctionBlock implements FunctionBlockInterface {
private static final int MAX_TRAJECTORY_LENGTH = 100;
HardwareEmulatorInterface model_server;
int model_id;
HashMap<String, Serializable> inputs = new HashMap<String, Serializable>();
HashMap<String, Serializable> outputs = new HashMap<String, Serializable>();
PhysicalVehicle vehicle;
public DirectModelAsFunctionBlock(HardwareEmulatorInterface model_server, String autopilot_config) throws Exception {
this.model_server = model_server;
this.model_id = model_server.alloc_autopilot(autopilot_config);
if (this.model_id < 0){
String error_msg = model_server.query("get_error_msg");
throw new Exception("Error allocating autopilot. Config:\n"+autopilot_config+"\n"+error_msg);
}
}
public void set_vehicle(PhysicalVehicle vehicle){
this.vehicle = vehicle;
}
public void free(){
if (model_id >= 0){
model_server.free_autopilot(model_id);
model_id = -1;
}
}
@Override
protected void finalize(){
free();
}
@Override
public void execute(double timeDelta) {
this.outputs = model_server.old_execute(model_id, (long)(timeDelta*1000000), inputs);
}
/*
TODOS:
- Let the sensors interact with the bus
- Let the actuators send their updated state
- Use the same names for the messages and Autopilot ports
*/
@Override
public void setInputs(Map<String, Object> inputs) {
this.inputs.clear();
double timeIncrement = (Double) inputs.get(BusEntry.SIMULATION_DELTA_TIME.toString());
double currentVelocity = (Double) inputs.get(BusEntry.SENSOR_VELOCITY.toString());
RealVector gps = (RealVector) inputs.get(BusEntry.SENSOR_GPS_COORDINATES.toString());
double x = gps.getEntry(0);
double y = gps.getEntry(1);
double compass = (Double) inputs.get(BusEntry.SENSOR_COMPASS.toString());
double engine = getCurrentActuatorValue(VehicleActuatorType.VEHICLE_ACTUATOR_TYPE_MOTOR);
double steering = getCurrentActuatorValue(VehicleActuatorType.VEHICLE_ACTUATOR_TYPE_STEERING);
double brakes = getCurrentActuatorValue(VehicleActuatorType.VEHICLE_ACTUATOR_TYPE_BRAKES_BACK_LEFT);
this.inputs.put("timeIncrement", timeIncrement);
this.inputs.put("currentVelocity", currentVelocity);
this.inputs.put("x", x);
this.inputs.put("y", y);
this.inputs.put("compass", compass);
this.inputs.put("currentEngine", engine);
this.inputs.put("currentSteering", steering);
this.inputs.put("currentBrakes", brakes);
Object ptx = inputs.get(BusEntry.PLANNED_TRAJECTORY_X.toString());
Object pty = inputs.get(BusEntry.PLANNED_TRAJECTORY_Y.toString());
if (ptx instanceof List<?> && pty instanceof List<?>) {
int trajectoryLength = processTrajectory((List<Double>) ptx, (List<Double>) pty);
this.inputs.put("trajectory_length", trajectoryLength);
}
}
private double getCurrentActuatorValue(VehicleActuatorType type) {
return vehicle.getSimulationVehicle().getVehicleActuator(type).getActuatorValueCurrent();
}
private int processTrajectory(List<Double> xCoords, List<Double> yCoords) {
if (xCoords.size() != yCoords.size()) {
return 0;
}
int len = xCoords.size();
if (len > MAX_TRAJECTORY_LENGTH) {
len = MAX_TRAJECTORY_LENGTH;
}
double[] trajectoryX = new double[len];
double[] trajectoryY = new double[len];
for (int i = 0; i < len; i++) {
Double x = xCoords.get(i);
Double y = yCoords.get(i);
if (x == null || y == null) {
return 0;
}
trajectoryX[i] = x;
trajectoryY[i] = y;
}
inputs.put("trajectory_x", trajectoryX);
inputs.put("trajectory_y", trajectoryY);
return len;
}
@Override
public Map<String, Object> getOutputs() {
Map<String, Object> result = new HashMap<>();
Object engine = outputs.get("engine");
if (engine != null)
//result.put(BusEntry.ACTUATOR_ENGINE.toString(), ((double)engine) / 2.5); //TODO REMOVE SUPER JANKY TEMP TEST
result.put(BusEntry.ACTUATOR_ENGINE.toString(), ((double)engine)); //TODO REMOVE SUPER JANKY TEMP TEST
Object brakes = outputs.get("brakes");
if (brakes != null)
result.put(BusEntry.ACTUATOR_BRAKE.toString(), brakes);
Object steering = outputs.get("steering");
if ( steering != null)
result.put(BusEntry.ACTUATOR_STEERING.toString(), steering );
outputs.clear();
return result;
}
@Override
public String[] getImportNames() {
return new String[0];
}
}
///**
// * (c) https://github.com/MontiCore/monticore
// *
// * The license generally applicable for this project
// * can be found under https://github.com/MontiCore/monticore
// */
//package de.rwth.monticore.EmbeddedMontiArc.simulators.basic_simulator.controller;
//
//import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.commons.BusEntry;
//import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.interfaces.FunctionBlockInterface;
//import org.apache.commons.math3.linear.RealVector;
//import simulation.vehicle.PhysicalVehicle;
//import de.rwth.monticore.EmbeddedMontiArc.simulators.hardware_emulator.HardwareEmulatorInterface;
//import simulation.vehicle.VehicleActuatorType;
//
//import java.io.Serializable;
//import java.util.HashMap;
//import java.util.List;
//import java.util.Map;
//
//public class DirectModelAsFunctionBlock implements FunctionBlockInterface {
// private static final int MAX_TRAJECTORY_LENGTH = 100;
//
// HardwareEmulatorInterface model_server;
// int model_id;
// HashMap<String, Serializable> inputs = new HashMap<String, Serializable>();
// HashMap<String, Serializable> outputs = new HashMap<String, Serializable>();
// PhysicalVehicle vehicle;
//
//
// public DirectModelAsFunctionBlock(HardwareEmulatorInterface model_server, String autopilot_config) throws Exception {
// this.model_server = model_server;
// this.model_id = model_server.alloc_autopilot(autopilot_config);
// if (this.model_id < 0){
// String error_msg = model_server.query("get_error_msg");
// throw new Exception("Error allocating autopilot. Config:\n"+autopilot_config+"\n"+error_msg);
// }
// }
//
// public void set_vehicle(PhysicalVehicle vehicle){
// this.vehicle = vehicle;
// }
//
// public void free(){
// if (model_id >= 0){
// model_server.free_autopilot(model_id);
// model_id = -1;
// }
// }
//
// @Override
// protected void finalize(){
// free();
// }
//
// @Override
// public void execute(double timeDelta) {
// this.outputs = model_server.old_execute(model_id, (long)(timeDelta*1000000), inputs);
// }
// /*
// TODOS:
// - Let the sensors interact with the bus
// - Let the actuators send their updated state
// - Use the same names for the messages and Autopilot ports
//
//
// */
// @Override
// public void setInputs(Map<String, Object> inputs) {
// this.inputs.clear();
// double timeIncrement = (Double) inputs.get(BusEntry.SIMULATION_DELTA_TIME.toString());
// double currentVelocity = (Double) inputs.get(BusEntry.SENSOR_VELOCITY.toString());
// RealVector gps = (RealVector) inputs.get(BusEntry.SENSOR_GPS_COORDINATES.toString());
// double x = gps.getEntry(0);
// double y = gps.getEntry(1);
// double compass = (Double) inputs.get(BusEntry.SENSOR_COMPASS.toString());
// double engine = getCurrentActuatorValue(VehicleActuatorType.VEHICLE_ACTUATOR_TYPE_MOTOR);
// double steering = getCurrentActuatorValue(VehicleActuatorType.VEHICLE_ACTUATOR_TYPE_STEERING);
// double brakes = getCurrentActuatorValue(VehicleActuatorType.VEHICLE_ACTUATOR_TYPE_BRAKES_BACK_LEFT);
// this.inputs.put("timeIncrement", timeIncrement);
// this.inputs.put("currentVelocity", currentVelocity);
// this.inputs.put("x", x);
// this.inputs.put("y", y);
// this.inputs.put("compass", compass);
// this.inputs.put("currentEngine", engine);
// this.inputs.put("currentSteering", steering);
// this.inputs.put("currentBrakes", brakes);
// Object ptx = inputs.get(BusEntry.PLANNED_TRAJECTORY_X.toString());
// Object pty = inputs.get(BusEntry.PLANNED_TRAJECTORY_Y.toString());
// if (ptx instanceof List<?> && pty instanceof List<?>) {
// int trajectoryLength = processTrajectory((List<Double>) ptx, (List<Double>) pty);
// this.inputs.put("trajectory_length", trajectoryLength);
// }
// }
//
// private double getCurrentActuatorValue(VehicleActuatorType type) {
// return vehicle.getSimulationVehicle().getVehicleActuator(type).getActuatorValueCurrent();
// }
//
// private int processTrajectory(List<Double> xCoords, List<Double> yCoords) {
// if (xCoords.size() != yCoords.size()) {
// return 0;
// }
// int len = xCoords.size();
// if (len > MAX_TRAJECTORY_LENGTH) {
// len = MAX_TRAJECTORY_LENGTH;
// }
// double[] trajectoryX = new double[len];
// double[] trajectoryY = new double[len];
// for (int i = 0; i < len; i++) {
// Double x = xCoords.get(i);
// Double y = yCoords.get(i);
// if (x == null || y == null) {
// return 0;
// }
// trajectoryX[i] = x;
// trajectoryY[i] = y;
// }
// inputs.put("trajectory_x", trajectoryX);
// inputs.put("trajectory_y", trajectoryY);
// return len;
// }
//
// @Override
// public Map<String, Object> getOutputs() {
// Map<String, Object> result = new HashMap<>();
// Object engine = outputs.get("engine");
// if (engine != null)
// //result.put(BusEntry.ACTUATOR_ENGINE.toString(), ((double)engine) / 2.5); //TODO REMOVE SUPER JANKY TEMP TEST
// result.put(BusEntry.ACTUATOR_ENGINE.toString(), ((double)engine)); //TODO REMOVE SUPER JANKY TEMP TEST
//
// Object brakes = outputs.get("brakes");
// if (brakes != null)
// result.put(BusEntry.ACTUATOR_BRAKE.toString(), brakes);
//
// Object steering = outputs.get("steering");
// if ( steering != null)
// result.put(BusEntry.ACTUATOR_STEERING.toString(), steering );
//
// outputs.clear();
// return result;
// }
//
// @Override
// public String[] getImportNames() {
// return new String[0];
// }
//}
......@@ -6,14 +6,17 @@
*/
package de.rwth.monticore.EmbeddedMontiArc.simulators.basic_simulator.controller;
import commons.controller.commons.BusEntry;
import de.rwth.monticore.EmbeddedMontiArc.simulators.basic_simulator.filesystem.MapData;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.utils.Point2D;
import sensors.StaticPlannedTrajectoryXSensor;
import sensors.StaticPlannedTrajectoryYSensor;
import sensors.util.SensorUtil;
import simulation.EESimulator.DirectModelAsEEComponent;
import simulation.EESimulator.EEComponent;
import simulation.simulator.Simulator;
import simulation.vehicle.*;
import de.rwth.monticore.EmbeddedMontiArc.simulators.hardware_emulator.HardwareEmulatorInterface;
import simulation.vehicle.*;
import sensors.*;
import java.util.*;
......@@ -53,13 +56,7 @@ public class VehicleBuilder {
PhysicalVehicleBuilder vehicleBuilder = getVehicleBuilder(config.physics_model);
DirectModelAsFunctionBlock controller = new DirectModelAsFunctionBlock(model_server, config.autopilot_config);
vehicleBuilder.setController(Optional.of(controller));
vehicleBuilder.setControllerBus(Optional.of(new TempBus()));
PhysicalVehicle physicalVehicle = vehicleBuilder.buildPhysicalVehicle();
controller.set_vehicle(physicalVehicle);
SensorUtil.sensorAdder(physicalVehicle);
// compute trajectory for the vehicle
VehicleTrajectory trajectory = new VehicleTrajectory(config.start_coords, config.target_coords, map);
......@@ -72,13 +69,27 @@ public class VehicleBuilder {
// setup the vehicle's trajectory
Map<String, List<Double>> trajectoryCoordinates = getTrajectoryCoordinates(trajectory.trajectory);
Vehicle simVehicle = physicalVehicle.getSimulationVehicle();
simVehicle.addSensor(new StaticPlannedTrajectoryXSensor(trajectoryCoordinates.get("x")));
simVehicle.addSensor(new StaticPlannedTrajectoryYSensor(trajectoryCoordinates.get("y")));
Vehicle simVehicle = new Vehicle(physicalVehicle);
DirectModelAsEEComponent controller = DirectModelAsEEComponent.createDirectModelAsEEComponent(simVehicle.getEEVehicle().getBusList().get(0), model_server, config.autopilot_config);
simVehicle.getEEVehicle().setAutoPilot(controller);
HashMap<BusEntry, LinkedList<EEComponent>> targetsByMessageIdX = new HashMap<>();
LinkedList<EEComponent> compListX = new LinkedList<>();
compListX.add(simVehicle.getEEVehicle().getBusList().get(0));
targetsByMessageIdX.put(BusEntry.PLANNED_TRAJECTORY_X, compListX);
simVehicle.getEEVehicle().addSensor(new StaticPlannedTrajectoryXSensor(physicalVehicle, simVehicle.getEEVehicle().getEESimulator(), new LinkedList<>(), targetsByMessageIdX, trajectoryCoordinates.get("x")));
HashMap<BusEntry, LinkedList<EEComponent>> targetsByMessageIdY = new HashMap<>();
LinkedList<EEComponent> compListY = new LinkedList<>();
compListY.add(simVehicle.getEEVehicle().getBusList().get(0));
targetsByMessageIdX.put(BusEntry.PLANNED_TRAJECTORY_Y, compListY);
simVehicle.getEEVehicle().addSensor(new StaticPlannedTrajectoryYSensor(physicalVehicle, simVehicle.getEEVehicle().getEESimulator(), new LinkedList<>(), targetsByMessageIdY, trajectoryCoordinates.get("Y")));
result.register_car(physicalVehicle.getId(), config.name, config.config, trajectory);
}
public static PhysicalVehicleBuilder getVehicleBuilder(String physics_model){
if (physics_model.equalsIgnoreCase(VehicleConfig.PhysicsModel.MODELICA.name)){
return new ModelicaPhysicalVehicleBuilder();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment