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 5212b671 authored by Lukas Bram's avatar Lukas Bram
Browse files

first working upper speed limit sensor

parent 9edabc37
Pipeline #397529 passed with stage
in 58 seconds
......@@ -20,6 +20,7 @@ import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent;
import de.rwth.montisim.simulation.eesimulator.message.Message;
import de.rwth.montisim.simulation.vehicle.lidar.Lidar;
import de.rwth.montisim.simulation.vehicle.navigation.Navigation;
import de.rwth.montisim.simulation.vehicle.navigation.SpeedLimitService;
import de.rwth.montisim.simulation.vehicle.physicalvalues.BatteryLevel;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
......@@ -56,6 +57,8 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
transient int backRightSensorMsg;
transient int backLeftSensorMsg;
transient int upperSpeedLimitMsg;
public double currentVelocity = 0;
public Vec2 currentPosition = null;
public double currentCompass = Double.NaN;
......@@ -75,6 +78,8 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
public double trajX[] = null;
public double trajY[] = null;
public double upperSpeedLimitArr[] = null;
public double currentGas = 0;
public double currentSteering = 0;
public double currentBrakes = 0;
......@@ -112,6 +117,9 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
this.leftBackSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.LEFT_BACK_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.backRightSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.BACK_RIGHT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.backLeftSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.BACK_LEFT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.upperSpeedLimitMsg = addPort(PortInformation.newOptionalInputDataPort(SpeedLimitService.UPPER_SPEED_LIMIT_MSG, SpeedLimitService.SPEED_LIMIT_TYPE, false));
}
@Override
......@@ -152,6 +160,8 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
this.backRightSensor = (double) msg.message;
}else if (msg.isMsg(backLeftSensorMsg)) {
this.backLeftSensor = (double) msg.message;
}else if (msg.isMsg(upperSpeedLimitMsg)) {
this.upperSpeedLimitArr = (double[]) msg.message;
}
}
......@@ -457,6 +467,7 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
addEntry(entries, false, ports.elementAt(15), leftBackSensor);
addEntry(entries, false, ports.elementAt(16), backRightSensor);
addEntry(entries, false, ports.elementAt(17), backLeftSensor);
addEntry(entries, false, ports.elementAt(18), upperSpeedLimitArr);
return entries;
}
......
......@@ -7,6 +7,7 @@ import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.lidar.LidarProperties;
import de.rwth.montisim.simulation.vehicle.navigation.NavigationProperties;
import de.rwth.montisim.simulation.vehicle.navigation.SpeedLimitServiceProperties;
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;
......@@ -70,7 +71,12 @@ public class DefaultVehicleConfig {
new LidarProperties()
.connectTo("DefaultBus")
);
properties.addComponent(
new SpeedLimitServiceProperties()
.connectTo("DefaultBus")
);
properties.addComponent(
ConstantBusProperties.instantBus().setName("DefaultBus")
);
......
......@@ -17,6 +17,7 @@ import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
import de.rwth.montisim.simulation.eesimulator.testcomponents.TestCompProperties;
import de.rwth.montisim.simulation.vehicle.lidar.LidarProperties;
import de.rwth.montisim.simulation.vehicle.navigation.NavigationProperties;
import de.rwth.montisim.simulation.vehicle.navigation.SpeedLimitService;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
import de.rwth.montisim.simulation.vehicle.task.metric.MetricGoalProperties;
......@@ -39,6 +40,7 @@ public class TypedSimulation {
Json.registerType(TestCompProperties.class);
Json.registerType(NavigationProperties.class);
Json.registerType(LidarProperties.class);
Json.registerType(SpeedLimitService.class);
Json.registerType(RigidbodyPhysicsProperties.class);
Json.registerType(ElectricalPTProperties.class);
Json.registerType(MetricGoalProperties.class);
......
......@@ -2,6 +2,7 @@ package de.rwth.montisim.simulation.vehicle.navigation;
import de.rwth.montisim.commons.dynamicinterface.BasicType;
import de.rwth.montisim.commons.dynamicinterface.PortInformation;
import de.rwth.montisim.commons.dynamicinterface.VectorType;
import de.rwth.montisim.commons.utils.UMath;
import de.rwth.montisim.commons.utils.Vec3;
import de.rwth.montisim.simulation.eesimulator.EEComponent;
......@@ -24,8 +25,8 @@ public class SpeedLimitService extends EEComponent {
private SpeedLimitServiceProperties properties;
private Double[] trajectoryX = null;
private Double[] trajectoryY = null;
private double[] trajectoryX = null;
private double[] trajectoryY = null;
private int trajectoryLength = 0;
//inputs
......@@ -37,6 +38,10 @@ public class SpeedLimitService extends EEComponent {
transient int upperSpeedLimitMsg,
lowerSpeedLimitMsg;
public static final String UPPER_SPEED_LIMIT_MSG = "upper_speed_limit";
public static final VectorType SPEED_LIMIT_TYPE = new VectorType(BasicType.DOUBLE, Navigation.TRAJ_ARRAY_LENGTH - 1);
public SpeedLimitService(SpeedLimitServiceProperties properties, EESystem eeSystem, World world) {
super(properties, eeSystem);
this.world = world;
......@@ -44,6 +49,8 @@ public class SpeedLimitService extends EEComponent {
this.trajectoryLengthMsg = addPort(PortInformation.newRequiredInputDataPort(Navigation.TRAJECTORY_LENGTH_MSG, BasicType.N, false));
this.trajectoryXMsg = addPort(PortInformation.newRequiredInputDataPort(Navigation.TRAJECTORY_X_MSG, Navigation.TRAJECTORY_X_TYPE, false));
this.trajectoryYMsg = addPort(PortInformation.newRequiredInputDataPort(Navigation.TRAJECTORY_Y_MSG, Navigation.TRAJECTORY_Y_TYPE, false));
this.upperSpeedLimitMsg = addPort(PortInformation.newRequiredOutputDataPort(UPPER_SPEED_LIMIT_MSG, SPEED_LIMIT_TYPE));
}
@Override
......@@ -51,10 +58,10 @@ public class SpeedLimitService extends EEComponent {
Message msg = msgRecvEvent.getMessage();
Instant time = msgRecvEvent.getEventTime();
if (msg.isMsg(trajectoryYMsg)) {
trajectoryY = (Double[]) msg.message;
trajectoryY = (double[]) msg.message;
compute(time);
} else if(msg.isMsg(trajectoryXMsg)) {
trajectoryX = (Double[]) msg.message;
trajectoryX = (double[]) msg.message;
} else if(msg.isMsg(trajectoryLengthMsg)) {
trajectoryLength = (int) msg.message;
}
......@@ -62,31 +69,41 @@ public class SpeedLimitService extends EEComponent {
private void compute(Instant time) {
Double[] maxSpeedArr = fetchUpperSpeedLimits();
sendMessage(time, upperSpeedLimitMsg, maxSpeedArr);
//System.out.println("testest");
double[] maxSpeedArr = fetchUpperSpeedLimits();
//sendMessage(time, upperSpeedLimitMsg, new double[]{0,0,0,0,0,0,0,0,0,0}, 8*trajectoryLength);
sendMessage(time, upperSpeedLimitMsg, maxSpeedArr, 8*trajectoryLength);
}
private Double[] fetchUpperSpeedLimits() {
private double[] fetchUpperSpeedLimits() {
Vector<Way> ways = world.ways;
Double[] maxSpeedArr = new Double[trajectoryLength - 1];
for (int i=0; i < trajectoryLength - 1; i++){
maxSpeedArr[i] = Double.MAX_VALUE;
double[] maxSpeedArr = new double[trajectoryLength];
for (int i=0; i < trajectoryLength; i++){
maxSpeedArr[i] = -1;
}
if ( trajectoryX != null && trajectoryLength > 1){
Vector<Node> wayNodes = new Vector<Node>();
for(Way way : ways){
for (int nodeId : way.nodeID){
wayNodes.add(world.nodes.get(nodeId));
}
}
for(int i = 0; i<trajectoryLength; i++){
Optional<Node> node = getNodeForCoordinates(wayNodes, trajectoryX[i], trajectoryY[i]);
if (node.isPresent()) {
Optional<Way> way = getNextWay(node.get(), trajectoryX[i+1], trajectoryY[i+1]);
if (way.isPresent()) {
maxSpeedArr[i] = way.get().maxSpeed;
PointInformation previousPointInfo = getPointInformation(world.ways, new Vec3(trajectoryX[0], trajectoryY[0], 0));
for(int i = 1; i < trajectoryLength; i++) {
Vec3 nextTrajPoint = new Vec3(trajectoryX[i], trajectoryY[i], 0);
if (previousPointInfo.ways.size() == 1){
//previous trajectory point is a point but not an intersection
maxSpeedArr[i-1] = previousPointInfo.ways.get(0).maxSpeed;
previousPointInfo = getPointInformation(previousPointInfo.ways, nextTrajPoint);
} else if (previousPointInfo.ways.size() == 0) {
//previous trajectory point cannot be found on any way
previousPointInfo = getPointInformation(world.ways, nextTrajPoint);
} else {
// initial/previous trajectory point is an intersection
Optional<Way> mutualWay = getMutualWay(previousPointInfo, nextTrajPoint);
if (mutualWay.isPresent()) {
// current - and next trajectory points are on the same way
Vector<Way> tmpWays = new Vector<>();
tmpWays.add(mutualWay.get());
maxSpeedArr[i-1] = mutualWay.get().maxSpeed;
previousPointInfo = getPointInformation(tmpWays,nextTrajPoint);
} else {
break;
previousPointInfo = getPointInformation(world.ways, nextTrajPoint);
}
}
}
......@@ -94,33 +111,67 @@ public class SpeedLimitService extends EEComponent {
return maxSpeedArr;
}
private Optional<Node> getNodeForCoordinates(Vector<Node> nodes, double x, double y){
for (Node node : nodes) {
double currentDistance = node.point.distance(new Vec3(x,y,0));
if (UMath.equalsThreshold(currentDistance, 0, Lidar.DOUBLE_TOLERANCE)){
return Optional.of(node);
private Optional<Way> getMutualWay(PointInformation pointAInfo, Vec3 pointB) {
for (Way way : pointAInfo.ways){
for (int i=0; i < way.points.size() - 1; i++) {
Vec3 currentPoint = way.points.get(i);
Vec3 nextPoint = way.points.get(i+1);
if (currentPoint.equals(pointAInfo.coordinates) && nextPoint.equals(pointB)
|| currentPoint.equals(pointB) && nextPoint.equals(pointAInfo.coordinates)) {
return Optional.of(way);
}
}
}
return Optional.empty();
}
private Vector<Node> getWayNodes(World world, Way way) {
Vector<Node> wayNodes = new Vector<Node>();
for (int nodeId : way.nodeID){
wayNodes.add(world.nodes.get(nodeId));
}
return wayNodes;
}
private Optional<Way> getNextWay(Node node, double x, double y){
if (node.ways.size() != 0) {
for (Way way : node.ways){
if (getNodeForCoordinates(getWayNodes(world, way),x,y).isPresent()) {
return Optional.of(way);
/** @function: getNodeIdOfPoint
* @return: the nodeId if the given @code{point} is an intersection, otherwise -1
* */
private PointInformation getPointInformation(Vector<Way> ways, Vec3 point){
PointInformation pointInformation = new PointInformation();
for (Way way : ways) {
for (int i=0;i<way.points.size(); i++) {
if (way.points.get(i).equals(point)) {
pointInformation.coordinates = point;
pointInformation.ways.add(way);
if (way.nodeID.get(i) != -1) {
pointInformation.node = world.nodes.get(way.nodeID.get(i));
}
}
}
}
return pointInformation;
}
return Optional.empty();
// private Vector<Node> getWayNodes(World world, Way way) {
// Vector<Node> wayNodes = new Vector<Node>();
// for (int nodeId : way.nodeID){
// wayNodes.add(world.nodes.get(nodeId));
// }
// return wayNodes;
// }
// private Optional<Way> getNextWay(Node node, double x, double y){
// if (node.ways.size() != 0) {
// for (Way way : node.ways){
// if (getNodeIdOfPoint(getWayNodes(world, way),x,y).isPresent()) {
// return Optional.of(way);
// }
// }
// }
//
// return Optional.empty();
// }
/**
* @Class: PointInformation
* It provides information about a specific point from the world.*/
private static class PointInformation{
public Vec3 coordinates;
public Node node;
public Vector<Way> ways = new Vector<>();
}
}
package de.rwth.montisim.simulation.vehicle.navigation;
import de.rwth.montisim.commons.utils.BuildContext;
import de.rwth.montisim.commons.utils.json.Typed;
import de.rwth.montisim.simulation.eesimulator.EEComponent;
import de.rwth.montisim.simulation.eesimulator.EEComponentProperties;
import de.rwth.montisim.simulation.eesimulator.EEComponentType;
......@@ -8,6 +9,7 @@ import de.rwth.montisim.simulation.eesimulator.EESystem;
import de.rwth.montisim.simulation.eesimulator.exceptions.EEMessageTypeException;
import de.rwth.montisim.simulation.environment.world.World;
@Typed(SpeedLimitServiceProperties.TYPE)
public class SpeedLimitServiceProperties extends EEComponentProperties {
public static final String TYPE = "speed_limit";
public static final String NAME = "SpeedLimit";
......
Markdown is supported
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