Properties Structure, Vehicle Builder and Navigation

eecomponents/pom.xml

@@ -41,6 +41,11 @@
             <artifactId>environment</artifactId>
             <version>${revision}</version>
         </dependency>
+        <dependency>
+            <groupId>montisim</groupId>
+            <artifactId>vehicle</artifactId>
+            <version>${revision}</version>
+        </dependency>
     </dependencies>
     
 </project>
\ No newline at end of file

eecomponents/src/main/java/de/rwth/montisim/simulation/eecomponents/navigation/Navigation.java

+/**
+ * (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.montisim.simulation.eecomponents.navigation;
 
+import java.time.Duration;
+import java.time.Instant;
+import java.util.Optional;
+import java.util.Stack;
+
 import de.rwth.montisim.commons.dynamicinterface.ArrayType;
 import de.rwth.montisim.commons.dynamicinterface.DataType;
 import de.rwth.montisim.commons.dynamicinterface.ArrayType.Dimensionality;
-import de.rwth.montisim.simulation.eesimulator.EEComponent;
-import de.rwth.montisim.simulation.eesimulator.EEComponentType;
-import de.rwth.montisim.simulation.eesimulator.EESimulator;
+import de.rwth.montisim.commons.utils.IPM;
+import de.rwth.montisim.commons.utils.Vec2;
+import de.rwth.montisim.simulation.environment.pathfinding.Path;
+import de.rwth.montisim.simulation.environment.pathfinding.Pathfinding;
+import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
+import de.rwth.montisim.simulation.eesimulator.components.EEComponent;
+import de.rwth.montisim.simulation.eesimulator.components.EEComponentType;
 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;
 
+// TODOS:
+// Send Empty "Overwrite" trajectory on errors
+// Only send trajectory every X seconds
+
 public class Navigation extends EEComponent {
     public static final String GPS_POS_MSG = "gps_pos";
     public static final String PUSH_TARGET_POS_MSG = "push_target_pos";
     public static final String POP_TARGET_POS_MSG = "push_target_pos";
+
     public static final String AT_TARGET_POS_MSG = "at_target_pos";
     public static final String CURRENT_TARGET_POS_MSG = "current_target_pos";
-    // The following messages are sent regularly by the Navigation and give the one waypoint behind the vehicle and 4 after (targets)
+    // The following messages are sent regularly by the Navigation and give the one
+    // waypoint behind the vehicle and 4 after (targets)
     // In coordinate or local space (latlon - xy)
     // Might give less points if almost at target (or none if at target)
     public static final String TRAJECTORY_X_MSG = "trajectory_x";
@@ -24,54 +46,188 @@ public class Navigation extends EEComponent {
     public static final String TRAJECTORY_LON_MSG = "trajectory_lon";
     public static final String TRAJECTORY_LAT_MSG = "trajectory_lat";
 
-    final MessageInformation gpsPosMsg;
-    
-    final MessageInformation pushTargetPosMsg;
-    final MessageInformation popTargetPosMsg;
-    
-    final MessageInformation atTargetPosMsg;
-    final MessageInformation currentTargetPosMsg;
+    public static final DataType AT_TARGET_POS_TYPE = DataType.BOOLEAN;
+    public static final DataType CURRENT_TARGET_POS_TYPE = DataType.VEC2;
+    public static final DataType TRAJECTORY_X_TYPE = new ArrayType(DataType.DOUBLE, Dimensionality.DYNAMIC, 10);
+    public static final DataType TRAJECTORY_Y_TYPE = TRAJECTORY_X_TYPE;
+    public static final DataType TRAJECTORY_LON_TYPE = TRAJECTORY_X_TYPE;
+    public static final DataType TRAJECTORY_LAT_TYPE = TRAJECTORY_X_TYPE;
 
-    final MessageInformation trajectoryXMsg;
-    final MessageInformation trajectoryYMsg;
-    final MessageInformation trajectoryLonMsg;
-    final MessageInformation trajectoryLatMsg;
+    MessageInformation gpsPosMsg;
+    MessageInformation truePosMsg;
 
-    public Navigation(EESimulator simulator, int priority) {
-        super(simulator, "Navigation", priority);
+    MessageInformation pushTargetPosMsg;
+    MessageInformation popTargetPosMsg;
 
+    MessageInformation atTargetPosMsg;
+    MessageInformation currentTargetPosMsg;
+
+    MessageInformation trajectoryXMsg;
+    MessageInformation trajectoryYMsg;
+    MessageInformation trajectoryLonMsg;
+    MessageInformation trajectoryLatMsg;
+
+    final Pathfinding pathfinding;
+
+    final Stack<Vec2> targets = new Stack<>();
+    Optional<Path> currentPath = Optional.empty();
+    Optional<Vec2> currentPos = Optional.empty();
+
+    public Navigation(NavigationProperties properties, Pathfinding pathfinding) {
+        super(properties);
+        this.pathfinding = pathfinding;
+    }
+
+    @Override
+    protected void init() throws EEMessageTypeException {
         this.gpsPosMsg = addInput(GPS_POS_MSG, DataType.VEC2);
+        this.truePosMsg = addInput(TruePosition.VALUE_NAME, TruePosition.TYPE);
 
         this.pushTargetPosMsg = addInput(PUSH_TARGET_POS_MSG, DataType.VEC2, true);
         this.popTargetPosMsg = addInput(POP_TARGET_POS_MSG, DataType.EMPTY, true);
-        
-        this.atTargetPosMsg = addOutput(AT_TARGET_POS_MSG, DataType.BOOLEAN);
-        this.currentTargetPosMsg = addOutput(CURRENT_TARGET_POS_MSG, DataType.VEC2);
-
-        ArrayType at = new ArrayType(DataType.DOUBLE, Dimensionality.DYNAMIC, 5);
-        this.trajectoryXMsg = addOutput(TRAJECTORY_X_MSG, at);
-        this.trajectoryYMsg = addOutput(TRAJECTORY_Y_MSG, at);
-        this.trajectoryLonMsg = addOutput(TRAJECTORY_LON_MSG, at);
-        this.trajectoryLatMsg = addOutput(TRAJECTORY_LAT_MSG, at);
-        // TODO Auto-generated constructor stub
+
+        this.atTargetPosMsg = addOutput(AT_TARGET_POS_MSG, AT_TARGET_POS_TYPE);
+        this.currentTargetPosMsg = addOutput(CURRENT_TARGET_POS_MSG, CURRENT_TARGET_POS_TYPE);
+
+        this.trajectoryXMsg = addOutput(TRAJECTORY_X_MSG, TRAJECTORY_X_TYPE);
+        this.trajectoryYMsg = addOutput(TRAJECTORY_Y_MSG, TRAJECTORY_Y_TYPE);
+        this.trajectoryLonMsg = addOutput(TRAJECTORY_LON_MSG, TRAJECTORY_LON_TYPE);
+        this.trajectoryLatMsg = addOutput(TRAJECTORY_LAT_MSG, TRAJECTORY_LAT_TYPE);
     }
 
     @Override
     protected void receive(MessageReceiveEvent msgRecvEvent) {
         // TODO Auto-generated method stub
-        Message msg =  msgRecvEvent.getMessage();
-        if (msg.msgId == gpsPosMsg.messageId){
+        Message msg = msgRecvEvent.getMessage();
+        Instant time = msgRecvEvent.getEventTime();
+        if (msg.msgId == gpsPosMsg.messageId) {
             // Update position -> Check current routing
-        } else if (msg.msgId == pushTargetPosMsg.messageId){
+            // TODO
+        } else if (msg.msgId == truePosMsg.messageId) {
+            currentPos = Optional.of((Vec2) msg.message);
+            if (!currentPath.isPresent() && !targets.empty()){
+                newTrajectory(time);
+            } else {
+                updateTrajectory(time);
+            }
+            // TODO
+        } else if (msg.msgId == pushTargetPosMsg.messageId) {
             // Add new position to routing stack -> compute new routing
-        } else if (msg.msgId == popTargetPosMsg.messageId){
+            // TODO
+            pushTargetPos((Vec2)msg.message, time);
+        } else if (msg.msgId == popTargetPosMsg.messageId) {
             // Remove target from routing stack -> compute new routing
+            // TODO
+            popTargetPos();
+        }
+    }
+
+    public void pushTargetPos(Vec2 target){
+        pushTargetPos(target, simulator.getSimulationTime());
+    }
+
+    public void pushTargetPos(Vec2 target, Instant time){
+        targets.push(target);
+        send(time, new Message(currentTargetPosMsg, target, currentTargetPosMsg.type.dataSize, id));
+    }
+    public void popTargetPos() {
+        if (!targets.empty()) targets.pop();
+    }
+
+    private void newTrajectory(Instant time){
+        currentPath = Optional.empty();
+        if (!currentPos.isPresent()) return;
+        if (targets.empty()){
+            send(time, new Message(atTargetPosMsg, new Boolean(true), atTargetPosMsg.type.dataSize, id));
+            return;
+        }
+        try {
+            currentPath = Optional.of(pathfinding.findShortestPath(currentPos.get(), targets.peek()));
+            updateTrajectory(time);
+        } catch(Exception e){
+            e.printStackTrace();
         }
     }
 
+    public void updateTrajectory(Instant time){
+        if (!currentPos.isPresent()) return;
+        if (!currentPath.isPresent()) return;
+        int index = getNearestSegment(currentPos.get());
+        if (index < 0) return;
+        Path p = currentPath.get();
+        int size = Math.min(10, p.getLength()-index);
+        double x[] = new double[size];
+        double y[] = new double[size];
+        for (int i = 0; i < size; ++i){
+            x[i] = p.trajectoryX[index+i];
+            y[i] = p.trajectoryY[index+i];
+        }
+        send(time, new Message(trajectoryXMsg, x, 8*size, id));
+        send(time.plus(Duration.ofMillis(10)), new Message(trajectoryYMsg, y, 8*size, id));
+    }
+
+    private int getNearestSegment(Vec2 pos) {
+        double currentNearestDistance = Double.POSITIVE_INFINITY;
+        int closestIndex = -1;
+        Path p = currentPath.get();
+        int count = p.getLength();
+        Vec2 normal = new Vec2();
+        Vec2 dir = new Vec2();
+        Vec2 delta = new Vec2();
+        Vec2 point = new Vec2();
+        double dist;
+        Vec2 lastPoint = new Vec2();
+        boolean hasLastPoint = false;
+        
+        for (int i = 0; i < count; i++){
+            p.get(i, point);
+
+            if (hasLastPoint){
+                // Check segment
+
+                // 1) Get segment "normal"
+                IPM.subtractToVec(point, lastPoint, dir);
+                // Manual normalization to keep the length
+                double length = dir.magnitude();
+                if (length > 0.001){
+                    IPM.multiply(dir, 1/length);
+                } else {
+                    dir.set(Double.NaN,Double.NaN);
+                }
+
+                // 2) check if in segment bounds
+                IPM.subtractToVec(pos, lastPoint, delta);
+                double projPos = IPM.dot(dir, delta);
+                if (projPos > 0 && projPos < length) {
+                
+                    // 3) get distance
+                    normal.set(-dir.y, dir.x);
+                    dist = Math.abs(IPM.dot(normal, delta));
+                    if (dist < currentNearestDistance){
+                        currentNearestDistance = dist;
+                        closestIndex = i-1;
+                    }
+                }
+            }
+
+            //Check point
+            dist = point.distance(pos);
+            if (dist < currentNearestDistance){
+                currentNearestDistance = dist;
+                closestIndex = i;
+            }
+
+            lastPoint.set(point);
+            hasLastPoint = true;
+        }
+        return closestIndex;
+    }
+
+
     @Override
     public EEComponentType getComponentType() {
         return EEComponentType.SERVICE;
     }
+
     
 }
\ No newline at end of file

eecomponents/src/main/java/de/rwth/montisim/simulation/eecomponents/navigation/NavigationProperties.java

+/**
+ * (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.montisim.simulation.eecomponents.navigation;
+
+import de.rwth.montisim.simulation.environment.pathfinding.Pathfinding;
+import de.rwth.montisim.simulation.vehicle.Vehicle;
+import de.rwth.montisim.simulation.vehicle.componentbuilders.ServiceComponentBuilder;
+import de.rwth.montisim.simulation.vehicle.componentbuilders.ServiceComponentProperties;
+
+public class NavigationProperties extends ServiceComponentProperties {
+    public static final String SERVICE_TYPE = "Navigation";
+    static {
+        ServiceComponentBuilder.registerServiceBuilder(SERVICE_TYPE, 
+            (ServiceComponentProperties properties, Pathfinding pathfinding, Vehicle vehicle) -> 
+                new Navigation((NavigationProperties) properties, pathfinding)
+        );
+    }
+
+    public NavigationProperties() {
+        super(SERVICE_TYPE);
+        this.name = "UnnamedNavigation";
+    }
+    
+}
\ No newline at end of file

eesimulator/src/main/java/de/rwth/montisim/simulation/eesimulator/EESimulator.java

@@ -7,8 +7,12 @@
 package de.rwth.montisim.simulation.eesimulator;
 
 import de.rwth.montisim.commons.eventsimulation.DiscreteEventSimulator;
+import de.rwth.montisim.simulation.eesimulator.components.ComponentManager;
+import de.rwth.montisim.simulation.eesimulator.events.EEDiscreteEvent;
+import de.rwth.montisim.simulation.eesimulator.events.EEEventType;
 import de.rwth.montisim.simulation.eesimulator.exceptions.EESetupErrors;
 import de.rwth.montisim.simulation.eesimulator.exceptions.EESetupException;
+import de.rwth.montisim.simulation.eesimulator.message.MessagePriorityComparator;
 import de.rwth.montisim.simulation.eesimulator.message.MessageTypeManager;
 
 /**
@@ -25,15 +29,17 @@ public class EESimulator extends DiscreteEventSimulator<EEEventType, EEDiscreteE
 	// Vector<DataType> types = new Vector<>();
 	// HashMap<DataType, Integer> typeIds = new HashMap<>();
 
+    protected final MessagePriorityComparator msgPrioComp;
 	protected final ComponentManager componentManager;
 	protected final MessageTypeManager messageTypeManager;
 	protected final EESetupErrors errors;
 	private boolean finalized = false;
 
-	public EESimulator() {
+	public EESimulator(MessageTypeManager messageTypeManager) {
 		this.errors = new EESetupErrors();
 		this.componentManager = new ComponentManager(this.errors);
-		this.messageTypeManager = new MessageTypeManager(this.componentManager, this.errors);
+		this.messageTypeManager = messageTypeManager;
+        this.msgPrioComp = new MessagePriorityComparator(messageTypeManager, componentManager);
 	}
 
 	public void processEvent(EEDiscreteEvent event) {
@@ -48,6 +54,10 @@ public class EESimulator extends DiscreteEventSimulator<EEEventType, EEDiscreteE
 		return componentManager;
 	}
 
+	public MessagePriorityComparator getMsgPrioComp() {
+		return msgPrioComp;
+	}
+
 	public void finalizeSetup() throws EESetupException {
 		componentManager.finalizeSetup();
 		errors.throwExceptions();

eesimulator/src/main/java/de/rwth/montisim/simulation/eesimulator/actuator/Actuator.java

@@ -6,58 +6,51 @@
  */
 package de.rwth.montisim.simulation.eesimulator.actuator;
 
-import java.time.Duration;
 import java.util.logging.Logger;
 
 import de.rwth.montisim.commons.dynamicinterface.DataType;
+import de.rwth.montisim.commons.dynamicinterface.DataType.Type;
 import de.rwth.montisim.commons.simulation.PhysicalValue;
 import de.rwth.montisim.commons.simulation.TimeUpdate;
 import de.rwth.montisim.commons.simulation.Updatable;
 import de.rwth.montisim.commons.simulation.Updater;
-import de.rwth.montisim.simulation.eesimulator.EEComponent;
-import de.rwth.montisim.simulation.eesimulator.EEComponentType;
-import de.rwth.montisim.simulation.eesimulator.EESimulator;
+import de.rwth.montisim.simulation.eesimulator.components.EEComponent;
+import de.rwth.montisim.simulation.eesimulator.components.EEComponentType;
 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.eesimulator.sensor.SensorLogic;
 
 public class Actuator extends EEComponent implements Updatable {
-    final SensorLogic sensor;
+    public static final String SETTER_PREFIX = "set_";
+
+    public final ActuatorProperties properties;
+    final PhysicalValue actuatedValue;
+    SensorLogic sensor;
     public double targetValue;
-    final PhysicalValue currentValue;
-    final double minValue;
-    final double maxValue;
-    double changeRate; // Max "value per second" this actuator can actuate.
-    final boolean sendFeedback; // Whether the actuator senses itself and sends its current value on the bus. ~> "Sensor enabled"
-    
-    final MessageInformation msgInfo;
+    final boolean sendFeedback; // Whether the actuator senses itself and sends its current value on the bus. ~>
+                                // "Sensor enabled"
+
+    MessageInformation msgInfo;
 
-    public Actuator(
-        EESimulator simulator, String name, int priority, 
-        PhysicalValue actuatedValue, double minValue, double maxValue, double changeRate,
-        boolean sendFeedback, Duration updateInterval, Duration readTime, boolean sendOnlyChanged,
-        Updater updater
-    ){
-        super(simulator, name, priority);
-        sensor = new SensorLogic(actuatedValue, updateInterval, readTime, sendOnlyChanged, this);
-        this.currentValue = actuatedValue;
-        this.minValue = minValue;
-        this.maxValue = maxValue;
-        this.targetValue = actuatedValue.get();
-        this.changeRate = changeRate;
-        this.sendFeedback = sendFeedback;
-        this.msgInfo = addOptionalInput("set_"+actuatedValue.name, DataType.DOUBLE, true);
+    public Actuator(ActuatorProperties properties, PhysicalValue actuatedValue, Updater updater) {
+        super(properties);
+        if (actuatedValue.type.type != Type.DOUBLE && actuatedValue.type.type != Type.FLOAT) throw new IllegalArgumentException("Actuator can only actuate on float or double values (but here type "+actuatedValue.type+ " for value "+ actuatedValue.name +")");
+        this.properties = properties;
+        this.actuatedValue = actuatedValue;
+        this.targetValue = actuatedValue.type.type == Type.DOUBLE ? (Double) actuatedValue.get() : (Float) actuatedValue.get();
         updater.addUpdatable(this);
+        this.sendFeedback = properties.sensorProperties.isPresent();
+        if (sendFeedback)
+            this.sensor = new SensorLogic(properties.sensorProperties.get(), actuatedValue);
     }
 
-    /** Returns an actuator which doesn't send its value back. */
-    public static Actuator newSimpleActuator(
-        EESimulator simulator, String name, int priority,
-        PhysicalValue actuatedValue, double minValue, double maxValue, double changeRate,
-        Updater updater
-    ){
-        return new Actuator(simulator, name, priority, actuatedValue, minValue, maxValue, changeRate, false, Duration.ZERO, Duration.ZERO, true, updater);
+    @Override
+    protected void init() throws EEMessageTypeException {
+        this.msgInfo = addOptionalInput(SETTER_PREFIX+actuatedValue.name, DataType.DOUBLE, true);
+        if (sendFeedback)
+            sensor.init(this);
     }
 
     @Override
@@ -69,18 +62,18 @@ public class Actuator extends EEComponent implements Updatable {
 
     // Can be overwritten for more complex actuation behavior
     protected void actuate(TimeUpdate newTime){
-        double val = currentValue.get();
-        double maxChange = changeRate*newTime.deltaSeconds;
+        double val = actuatedValue.type.type == Type.DOUBLE ? (Double) actuatedValue.get() : (Float) actuatedValue.get();
+        double maxChange = properties.changeRate*newTime.deltaSeconds;
         double delta = targetValue - val;
         if (Math.abs(delta) > maxChange){
             delta = Math.signum(delta) * maxChange;
         }
         val += delta;
-        if (val > maxValue)
-            val = maxValue;
-        if (val < minValue)
-            val = minValue;
-        currentValue.set(val);
+        if (val > properties.maxValue)
+            val = properties.maxValue;
+        if (val < properties.minValue)
+            val = properties.minValue;
+        actuatedValue.set(actuatedValue.type.type == Type.DOUBLE ? new Double(val) : new Float(val));
     }
 
     @Override
@@ -98,4 +91,5 @@ public class Actuator extends EEComponent implements Updatable {
         return EEComponentType.ACTUATOR;
     }
 
+
 }
\ No newline at end of file

eesimulator/src/main/java/de/rwth/montisim/simulation/eesimulator/actuator/ActuatorProperties.java

+/**
+ * (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.montisim.simulation.eesimulator.actuator;
+
+import java.util.Optional;
+
+import de.rwth.montisim.simulation.eesimulator.components.EEComponentProperties;
+import de.rwth.montisim.simulation.eesimulator.components.EEComponentType;
+import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
+
+public class ActuatorProperties extends EEComponentProperties {
+    public ActuatorProperties() {
+        super(EEComponentType.ACTUATOR);
+    }
+
+    public ActuatorProperties(double minValue, double maxValue, double changeRate) {
+        super(EEComponentType.ACTUATOR);
+        this.maxValue = maxValue;
+        this.minValue = minValue;
+        this.changeRate = changeRate;
+    }
+    
+    public ActuatorProperties setName(String name) {
+        this.name = name;
+        return this;
+    }
+
+    public ActuatorProperties setPhysicalValueName(String physicalValueName) {
+        this.physicalValueName = physicalValueName;
+        return this;
+    }
+
+    public ActuatorProperties setRange(double minValue, double maxValue) {
+        this.minValue = minValue;
+        this.maxValue = maxValue;
+        return this;
+    }
+
+    public ActuatorProperties setChangeRate(double changeRate) {
+        this.changeRate = changeRate;
+        return this;
+    }
+    
+    public ActuatorProperties setSensorProperties(SensorProperties properties) {
+        this.sensorProperties = Optional.of(properties);
+        return this;
+    }
+
+
+    public String physicalValueName;
+
+    public double minValue;
+    public double maxValue; 
+    public double changeRate;
+    public Optional<SensorProperties> sensorProperties;
+}
\ No newline at end of file

eesimulator/src/main/java/de/rwth/montisim/simulation/eesimulator/bridge/Bridge.java

@@ -6,30 +6,20 @@
  */
 package de.rwth.montisim.simulation.eesimulator.bridge;
 
-import java.time.Duration;
-
 import de.rwth.montisim.commons.eventsimulation.exceptions.UnexpectedEventException;
-import de.rwth.montisim.simulation.eesimulator.BusComponent;
-import de.rwth.montisim.simulation.eesimulator.EEComponentType;
-import de.rwth.montisim.simulation.eesimulator.EEDiscreteEvent;
-import de.rwth.montisim.simulation.eesimulator.EESimulator;
+import de.rwth.montisim.simulation.eesimulator.components.BusComponent;
+import de.rwth.montisim.simulation.eesimulator.components.EEComponentType;
+import de.rwth.montisim.simulation.eesimulator.events.EEDiscreteEvent;
 import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent;
 import de.rwth.montisim.simulation.eesimulator.events.MessageSendEvent;
 
 public class Bridge extends BusComponent {
 
-    /**