Skip to content
GitLab
Commit f722eb73 authored by Jean Meurice's avatar Jean Meurice
Browse files

EESimulator rework, Environment Rework (basic-sim parser), Vehicle Rework (WIP)

parent 75ed53b4
Pipeline #261238 failed with stage
Expand all Hide whitespace changes
Inline Side-by-side
bus/src/main/java/simulation/EESimulator/AbstractEEComponent.java deleted 100644 → 0
View file @ 75ed53b4
/**
* (c) https://github.com/MontiCore/monticore
*
* The license generally applicable for this project
* can be found under https://github.com/MontiCore/monticore.
*/
package simulation.EESimulator;
import java.time.Instant;
import java.util.Collections;
import java.util.List;
import java.util.UUID;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.commons.BusEntry;
import org.apache.commons.math3.exception.NullArgumentException;
import simulation.bus.BusMessage;
/**
* Abstract class that implements all common functionality of EEComponents.
*/
abstract class AbstractEEComponent implements EEComponent{
private final EESimulator simulator;
private final EEComponentType componentType;
private final UUID Id;
public AbstractEEComponent(EESimulator simulator, EEComponentType type) {
if (simulator == null || type == null) {
throw new NullArgumentException();
}
this.simulator = simulator;
this.componentType = type;
this.Id = UUID.randomUUID();
}
@Override
public EEComponentType getComponentType() {
return componentType;
}
@Override
public EESimulator getSimulator() {
return simulator;
}
@Override
public UUID getId() {
return Id;
}
/**
* Send message to all targets that are registered for this message in targetsByMessageId
* @param message message to be send
* @param messageLen length of message in bytes
* @param messageId message id of the message
* @param eventTime time when the message is sent
*/
@Override
public void sendMessage(Object message, int messageLen , BusEntry messageId, Instant eventTime) {
List<EEComponent> targets = this.getTargetsByMessageId().getOrDefault(messageId, Collections.emptyList());
for(EEComponent target : targets) {
BusMessage msg = new BusMessage(message, 6, messageId,
eventTime, this.getId(), target);
this.getSimulator().addEvent(msg);
}
}
}
bus/src/main/java/simulation/EESimulator/Bridge.java deleted 100644 → 0
View file @ 75ed53b4
/**
* (c) https://github.com/MontiCore/monticore
*
* The license generally applicable for this project
* can be found under https://github.com/MontiCore/monticore.
*/
package simulation.EESimulator;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.commons.BusEntry;
import simulation.bus.Bus;
import simulation.bus.BusMessage;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.time.Duration;
import org.apache.commons.lang3.tuple.Pair;
/**
* Used to connect to buses.
*/
public class Bridge extends MutableEEComponent{
/**
* The two buses that should be connected
*/
protected Pair<Bus, Bus> connected;
/**
* The delay that is added to each event if it is transmitted over this bride.
*/
private final Duration delay;
public Bridge (Pair<Bus, Bus> connected, Duration delay){
super(connected.getLeft().getSimulator(), EEComponentType.BRIDGE);
if(connected.getLeft().getSimulator() != connected.getRight().getSimulator()) {
throw new IllegalArgumentException("Bus with different simulators can not be connected");
}
this.connected = connected;
this.delay = delay;
//initialize subscribed messages and targetsByMessageId
Set<BusEntry> subscribedMessages = new HashSet<BusEntry>(connected.getKey().getSubscribedMessages());
subscribedMessages.addAll(connected.getValue().getSubscribedMessages());
this.subscribedMessages.addAll(subscribedMessages);
for(BusEntry messageId : subscribedMessages) {
List<EEComponent> targets = new ArrayList<EEComponent>();
if(connected.getKey().getSubscribedMessages().contains(messageId)) {
targets.add(connected.getKey());
}
if(connected.getValue().getSubscribedMessages().contains(messageId)) {
targets.add(connected.getValue());
}
this.targetsByMessageId.put(messageId, targets);
}
connected.getKey().registerComponent(this);
connected.getValue().registerComponent(this);
}
@Override
public void processEvent(EEDiscreteEvent event){
if(event.getEventType() == EEDiscreteEventTypeEnum.BUSMESSAGE){
BusMessage msg = (BusMessage) event;
msg.setFinishTime(msg.getEventTime().plus(delay));
msg.transmitBytes(msg.getMessageLen(), 0);
if(msg.getControllerID() == connected.getLeft().getId()) {
this.getSimulator().addEvent(msg.forwardTo(connected.getRight()));
}
else if(msg.getControllerID() == connected.getRight().getId()) {
this.getSimulator().addEvent(msg.forwardTo(connected.getLeft()));
}
else {
throw new IllegalArgumentException("Message from invalid controller" + msg.getControllerID() + "received at: " + this.toString());
}
}
else{
throw new IllegalArgumentException("Only BusMessages events expected at " + this.toString() + " but was: " + event.getEventType());
}
}
/**
* Updates the subscribed messages of this and the other connected buses.
* Add bus as target for messages ids.
* @param bus Bus which needs the messages with messageIds
* @param messageIds Ids of messages that bus wants to receive.
*/
public void update(Bus bus, List<BusEntry> messageIds){
for(BusEntry messageId : messageIds) {
List<EEComponent> targets = this.targetsByMessageId.getOrDefault(messageId, new ArrayList<EEComponent>());
targets.add(bus);
this.targetsByMessageId.put(messageId, targets);
if(!subscribedMessages.contains(messageId)) {
this.subscribedMessages.add(messageId);
}
}
if(bus.equals(connected.getKey())){
connected.getValue().addSubscribedMessages(this, messageIds);
}
else{
if(bus.equals(connected.getValue())){
connected.getKey().addSubscribedMessages(this, messageIds);
}
else{
throw new IllegalArgumentException("Message send by unknown Bus.");
}
}
}
public Pair<Bus, Bus> getConnectedBuses() {
return this.connected;
}
public Duration getDelay() {
return delay;
}
}
bus/src/main/java/simulation/EESimulator/ConstantFunctionBlockAsEEComponent.java deleted 100644 → 0
View file @ 75ed53b4
/**
* (c) https://github.com/MontiCore/monticore
*
* The license generally applicable for this project
* can be found under https://github.com/MontiCore/monticore.
*/
package simulation.EESimulator;
import com.google.common.collect.Sets;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.commons.BusEntry;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.interfaces.FunctionBlockInterface;
import simulation.bus.Bus;
import java.util.*;
/**
* Wrapper for a function block that always sends the messages.
*/
public class ConstantFunctionBlockAsEEComponent extends ImmutableEEComponent{
/**
* BusEntries that are send by this component by their name.
*/
private Map<String, BusEntry> busEntryByName = new HashMap<>();
/**
* Size of the message by BusEntry
*/
private final HashMap<BusEntry, Integer> sizeByMessageId;
/**
* Create a constant function block as EEComponent with default settings.
* @param buses Buses that the EEComponent should be connected
* @param sizeByMessageId Size of the messages that are transmitted
* @param functionBlock The function block that is wrapped.
* @return ConstantFunctionBlockAsEEComponent
*/
public static ConstantFunctionBlockAsEEComponent createConstantFunctionBlockAsEEComponent(List<Bus> buses, HashMap<BusEntry, Integer> sizeByMessageId, FunctionBlockInterface functionBlock){
if(buses == null || buses.isEmpty()){
throw new IllegalArgumentException("Buses can not be null or empty");
}
Set<String> outputMessageNames = functionBlock.getOutputs().keySet();
List<EEComponent> targets = new ArrayList<>(buses);
HashMap<BusEntry, List<EEComponent>> targetsByMessageId = new HashMap<>();
for(BusEntry entry : BusEntry.values()){
if(outputMessageNames.contains(entry.toString())){
targetsByMessageId.put(entry, targets);
}
}
return new ConstantFunctionBlockAsEEComponent(buses.get(0).getSimulator(), targetsByMessageId, sizeByMessageId, functionBlock);
}
/**
* Create a constant function block as EEComponent with default settings.
* @param bus Bus that the EEComponent should be connected
* @param sizeByMessageId Size of the messages that are transmitted
* @param functionBlock The function block that is wrapped.
* @return ConstantFunctionBlockAsEEComponent
*/
public static ConstantFunctionBlockAsEEComponent createConstantFunctionBlockAsEEComponent(Bus bus, HashMap<BusEntry, Integer> sizeByMessageId, FunctionBlockInterface functionBlock){
return createConstantFunctionBlockAsEEComponent(Collections.singletonList(bus), sizeByMessageId, functionBlock);
}
public ConstantFunctionBlockAsEEComponent(EESimulator simulator, HashMap<BusEntry, List<EEComponent>> targetsByMessageId, HashMap<BusEntry, Integer> sizeByMessageId, FunctionBlockInterface functionBlock) {
super(simulator, EEComponentType.FUNCTION_BLOCK, Collections.emptyList(), targetsByMessageId);
if(Sets.symmetricDifference(targetsByMessageId.keySet(), sizeByMessageId.keySet()).size() != 0){
throw new IllegalArgumentException("targetsByMessageId and sizeByMessageId have to have the same keys.");
}
for(BusEntry entry : targetsByMessageId.keySet()){
busEntryByName.put(entry.toString(), entry);
}
this.sizeByMessageId = sizeByMessageId;
this.setConstantOutput(functionBlock.getOutputs());
}
/**
* Send the constant messages by the wrapped function block
* @param outputs Messages that should be send. Indexed by the names of the message Ids.
*/
private void setConstantOutput(Map<String, Object> outputs) {
for(Map.Entry<String, Object> entry : outputs.entrySet()){
BusEntry messageId = busEntryByName.get(entry.getKey());
this.sendMessage(entry.getValue(), sizeByMessageId.get(messageId), messageId, getSimulator().getSimulationTime());
}
}
@Override
public void processEvent(EEDiscreteEvent event) {
throw new UnsupportedOperationException("Constant function block does not expect any event. Event was: " + event);
}
}
bus/src/main/java/simulation/EESimulator/ControllerAsEEComponent.java deleted 100644 → 0
View file @ 75ed53b4
/**
* (c) https://github.com/MontiCore/monticore
*
* The license generally applicable for this project
* can be found under https://github.com/MontiCore/monticore.
*/
package simulation.EESimulator;
import de.rwth.monticore.EmbeddedMontiArc.simulators.commons.controller.commons.BusEntry;
import de.rwth.monticore.EmbeddedMontiArc.simulators.hardware_emulator.interfaces.*;
import de.rwth.monticore.EmbeddedMontiArc.simulators.hardware_emulator.config.ControllerConfig;
import org.apache.commons.math3.linear.RealVector;
import simulation.bus.Bus;
import simulation.bus.BusMessage;
import java.io.Serializable;
import java.time.Duration;
import java.time.Instant;
import java.util.*;
import static com.vividsolutions.jts.util.Memory.free;
/**
* Autopilot that communicate directly with the HardwareEmulatorServer.
* Controls the velocity and path of the vehicle.
* Calculates actuator values for brakes, engine, steering.
*/
public class ControllerAsEEComponent extends ImmutableEEComponent {
private static final int MAX_TRAJECTORY_LENGTH = 100;
/**
* Last values that were send indexed by the bus entry
*/
private Map<Object, BusEntry> lastValueByMessageId = new HashMap<>();
/**
* Necessary messages that the controller needs
*/
private static final ArrayList<BusEntry> STANDARD_SUBSCRIBED_MESSAGES = new ArrayList<BusEntry>() {{
add(BusEntry.SENSOR_VELOCITY);
add(BusEntry.SENSOR_GPS_COORDINATES);
add(BusEntry.SENSOR_COMPASS);
add(BusEntry.ACTUATOR_ENGINE_CURRENT);
add(BusEntry.ACTUATOR_BRAKE_CURRENT);
add(BusEntry.ACTUATOR_STEERING_CURRENT);
add(BusEntry.PLANNED_TRAJECTORY_X);
add(BusEntry.PLANNED_TRAJECTORY_Y);
}};
/**
* Outputs when created for a MassPointVehicle
*/
public static final ArrayList<BusEntry> MASSPOINT_OUTPUT_MESSAGES = new ArrayList<BusEntry>() {{
add(BusEntry.ACTUATOR_STEERING);
add(BusEntry.ACTUATOR_BRAKE);
add(BusEntry.ACTUATOR_ENGINE);
}};
/**
* Determines if the controller has received new inputs
*/
boolean newInputs = false;
/**
* Determines if the controller needs to start the calculation again afer receiving the next message.
*/
boolean wakeUpNeeded = false;
/**
* The duration of the an update cycle of this controller
*/
Duration cycleTime;
/**
* Direct reference to the hardware emulator
*/
SoftwareSimulator softwareSimulator;
int modelId;
HashMap<String, Serializable> inputs = new HashMap<String, Serializable>();
HashMap<String, Serializable> outputs = new HashMap<String, Serializable>();
Optional<Object> trajectoryX = Optional.empty();
Optional<Object> trajectoryY = Optional.empty();
/**
* Time when the controller was executed last
*/
Instant lastFinishTime = Instant.EPOCH;
/**
* Create a ControllerAsEEComponent that is connected to bus.
* @param bus Bus that the ControllerAsEEComponent is connected to
* @return ControllerAsEEComponent with default configuration
*/
public static ControllerAsEEComponent createControllerAsEEComponent(Bus bus) {
return createControllerAsEEComponent(Collections.singletonList(bus));
}
/**
* Create a ControllerAsEEComponent that is connected to bus.
* @param buses Buses that the ControllerAsEEComponent is connected to
* @return ControllerAsEEComponent with default configuration
*/
public static ControllerAsEEComponent createControllerAsEEComponent(List<Bus> buses){
HashMap<BusEntry, List<EEComponent>> targetsByMessageId = new HashMap<>();
targetsByMessageId.put(BusEntry.ACTUATOR_BRAKE, new LinkedList<>());
targetsByMessageId.put(BusEntry.ACTUATOR_STEERING, new LinkedList<>());
targetsByMessageId.put(BusEntry.ACTUATOR_ENGINE, new LinkedList<>());
for (Bus bus : buses) {
if (bus.subscribedMessages.contains(BusEntry.ACTUATOR_ENGINE)) {
targetsByMessageId.get(BusEntry.ACTUATOR_ENGINE).add(bus);
}
if (bus.subscribedMessages.contains(BusEntry.ACTUATOR_STEERING)) {
targetsByMessageId.get(BusEntry.ACTUATOR_STEERING).add(bus);
}
if (bus.subscribedMessages.contains(BusEntry.ACTUATOR_BRAKE)) {
targetsByMessageId.get(BusEntry.ACTUATOR_BRAKE).add(bus);
}
}
return new ControllerAsEEComponent(buses.get(0).getSimulator(), targetsByMessageId);
}
public ControllerAsEEComponent(EESimulator simulator, HashMap<BusEntry, List<EEComponent>> targetsByMessageId) {
super(simulator, EEComponentType.AUTOPILOT, STANDARD_SUBSCRIBED_MESSAGES, targetsByMessageId);
}
public void setCycleTime(Duration cycleTime) {
this.cycleTime = cycleTime;
}
public Duration getCycleTime() {
return cycleTime;
}
public void free(){
try {
softwareSimulator.free();
} catch(Exception e){
e.printStackTrace();
}
}
/**
* Set the SoftwareSimulator interface, create the autopilot and set the cycle time.
* @param softwareSimManager
* @param controllerConfig
* @param cycleTime
* @throws Exception
*/
public void initializeController(SoftwareSimulatorManager softwareSimManager, ControllerConfig controllerConfig, Duration cycleTime) throws Exception {
this.softwareSimulator = softwareSimManager.newSimulator(controllerConfig);
this.cycleTime = cycleTime;
//add controller execute event
this.getSimulator().addEvent(new ControllerExecuteEvent(this.getSimulator().getSimulationTime().plus(cycleTime), this));
}
@Override
protected void finalize(){
free();
}
@Override
public void processEvent(EEDiscreteEvent event) {
//buffer new values arrived by busMessage
if (event.getEventType() == EEDiscreteEventTypeEnum.BUSMESSAGE) {
BusMessage currentMessage = (BusMessage) event;
newInputs = true;
switch (currentMessage.getMessageID()) {
case SENSOR_VELOCITY:
double currentVelocity = (double) currentMessage.getMessage();
this.inputs.put("currentVelocity", currentVelocity);
break;
case SENSOR_GPS_COORDINATES:
double x = ((RealVector) currentMessage.getMessage()).getEntry(0);
double y = ((RealVector) currentMessage.getMessage()).getEntry(1);
this.inputs.put("x", x);
this.inputs.put("y", y);
break;
case SENSOR_COMPASS:
double compass = (double) currentMessage.getMessage();
this.inputs.put("compass", compass);
break;
case ACTUATOR_ENGINE_CURRENT:
double engine = (double) currentMessage.getMessage();
this.inputs.put("currentEngine", engine);
break;
case ACTUATOR_BRAKE_CURRENT:
double brakes = (double) currentMessage.getMessage();
this.inputs.put("currentBrakes", brakes);
break;
case ACTUATOR_STEERING_CURRENT:
double steering = (double) currentMessage.getMessage();
this.inputs.put("currentSteering", steering);
break;
case PLANNED_TRAJECTORY_X:
this.trajectoryX = Optional.of(currentMessage.getMessage());
break;
case PLANNED_TRAJECTORY_Y:
this.trajectoryY = Optional.of(currentMessage.getMessage());
break;
default:
throw new IllegalArgumentException("Received unsubscribed Message. Message type was: " + currentMessage.getMessageID().toString());
}
if (trajectoryX.isPresent() && trajectoryY.isPresent()) {
if (trajectoryX.get() instanceof List<?> && trajectoryY.get() instanceof List<?>) {
int trajectoryLength = processTrajectory((List<Double>) trajectoryX.get(), (List<Double>) trajectoryY.get());
this.inputs.put("trajectory_length", trajectoryLength);
}
}
if(wakeUpNeeded) {
try {
executeController(event);
} catch (Exception e){
e.printStackTrace();
}
}
} else if (event.getEventType() == EEDiscreteEventTypeEnum.CONTROLLER_EXECUTE_EVENT) {
if(newInputs) {
try {
executeController(event);
} catch (Exception e){
e.printStackTrace();
}
}
else {
wakeUpNeeded = true;
}
} else {
throw new IllegalArgumentException("ControllerAsEEComponent expect BusMessage or ControllerExecuteEvent as event. Event was: " + event.getEventType());
}
}
/**
* Execute the controller once. Add a ControllerExecuteEvent for the next cycle.
* @param event
*/
private void executeController(EEDiscreteEvent event) throws Exception {
double timeIncrement = Duration.between(event.getEventTime(), lastFinishTime).toMillis();
this.inputs.put("timeIncrement", timeIncrement);
this.softwareSimulator.setInputs(inputs);
Duration delay = this.softwareSimulator.runCycle();
this.outputs = this.softwareSimulator.getOutputs();
lastFinishTime = event.getEventTime().plus(delay);
Object engine = outputs.get("engine");
if (engine != null) {
if(!lastValueByMessageId.containsKey(BusEntry.ACTUATOR_ENGINE) || !lastValueByMessageId.get(BusEntry.ACTUATOR_ENGINE).equals(engine)) {
this.sendMessage(engine, 8, BusEntry.ACTUATOR_ENGINE, lastFinishTime);
}
}
Object brakes = outputs.get("brakes");
if (brakes != null) {
if(!lastValueByMessageId.containsKey(BusEntry.ACTUATOR_BRAKE) || !lastValueByMessageId.get(BusEntry.ACTUATOR_BRAKE).equals(brakes)){
this.sendMessage(brakes, 8, BusEntry.ACTUATOR_BRAKE, lastFinishTime);
}
}
Object steering = outputs.get("steering");
if (steering != null) {
if(!lastValueByMessageId.containsKey(BusEntry.ACTUATOR_STEERING) || !lastValueByMessageId.get(BusEntry.ACTUATOR_STEERING).equals(steering)) {
this.sendMessage(steering, 8, BusEntry.ACTUATOR_STEERING, lastFinishTime);
}
}
//set next execute event
Instant nextExecuteTime = event.getEventTime().plus(cycleTime);
if(lastFinishTime.isBefore(nextExecuteTime)) {
this.getSimulator().addEvent(new ControllerExecuteEvent(nextExecuteTime, this));
}
else {
this.getSimulator().addEvent(new ControllerExecuteEvent(lastFinishTime, this));
}
wakeUpNeeded = false;
newInputs = false;
}