Commit e6f49c1d authored by Jean Meurice's avatar Jean Meurice
Browse files

Properties Structure, Vehicle Builder and Navigation

parent eb5b226e
Pipeline #285432 failed with stage
......@@ -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
/**
* (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
/**
* (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
......@@ -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();
......
......@@ -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
/**
* (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
......@@ -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 {
/**