Aufgrund einer Störung des s3 Storage, könnten in nächster Zeit folgende GitLab Funktionen nicht zur Verfügung stehen: Container Registry, Job Artifacs,. Wir bitten um Verständnis. Es wird mit Hochdruck an der Behebung des Problems gearbeitet. Weitere Informationen zur Störung des Object Storage finden Sie hier: https://maintenance.itc.rwth-aachen.de/ticket/status/messages/59-object-storage-pilot

Commit 9edabc37 authored by Lukas Bram's avatar Lukas Bram
Browse files

Merge branch 'dev_ema' of...

Merge branch 'dev_ema' of git.rwth-aachen.de:monticore/EmbeddedMontiArc/simulators/simulation into dev_ema
parents b5d7a5fa 10a3d15d
Pipeline #396343 passed with stage
in 58 seconds
......@@ -47,13 +47,27 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
transient int batteryMsg;
transient int frontSensorMsg;
transient int frontRightSensorMsg;
transient int frontLeftSensorMsg;
transient int rightFrontSensorMsg;
transient int rightBackSensorMsg;
transient int leftFrontSensorMsg;
transient int leftBackSensorMsg;
transient int backRightSensorMsg;
transient int backLeftSensorMsg;
public double currentVelocity = 0;
public Vec2 currentPosition = null;
public double currentCompass = Double.NaN;
public double batteryLevel = 0;
public double frontSensor = 0;
public double frontRightSensor = 0;
public double frontLeftSensor = 0;
public double rightFrontSensor = 0;
public double rightBackSensor = 0;
public double leftFrontSensor = 0;
public double leftBackSensor = 0;
public double backRightSensor = 0;
public double backLeftSensor = 0;
double newTrajX[] = null;
public int newTrajLength = 0;
......@@ -90,7 +104,14 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
this.batteryMsg = addPort(PortInformation.newOptionalInputDataPort(BatteryLevel.VALUE_NAME, BatteryLevel.TYPE, false));
this.frontSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.FRONT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.frontRightSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.FRONT_RIGHT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.frontLeftSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.FRONT_LEFT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.rightFrontSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.RIGHT_FRONT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.rightBackSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.RIGHT_BACK_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
this.leftFrontSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.LEFT_FRONT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
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));
}
@Override
......@@ -115,8 +136,22 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
trajX = newTrajX;
} else if (msg.isMsg(batteryMsg)) {
batteryLevel = (double) msg.message;
} else if (msg.isMsg(frontSensorMsg)) {
this.frontSensor = (double) msg.message;
} else if (msg.isMsg(frontRightSensorMsg)) {
this.frontRightSensor = (double) msg.message;
} else if (msg.isMsg(frontLeftSensorMsg)) {
this.frontLeftSensor = (double) msg.message;
} else if (msg.isMsg(rightFrontSensorMsg)) {
this.rightFrontSensor = (double) msg.message;
}else if (msg.isMsg(rightBackSensorMsg)) {
this.rightBackSensor = (double) msg.message;
}else if (msg.isMsg(leftFrontSensorMsg)) {
this.leftFrontSensor = (double) msg.message;
}else if (msg.isMsg(leftBackSensorMsg)) {
this.leftBackSensor = (double) msg.message;
}else if (msg.isMsg(backRightSensorMsg)) {
this.backRightSensor = (double) msg.message;
}else if (msg.isMsg(backLeftSensorMsg)) {
this.backLeftSensor = (double) msg.message;
}
}
......@@ -414,7 +449,14 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
addEntry(entries, true, ports.elementAt(7), currentGas);
addEntry(entries, true, ports.elementAt(8), currentBrakes);
addEntry(entries, false, ports.elementAt(9), batteryLevel);
addEntry(entries, false, ports.elementAt(10), frontSensor);
addEntry(entries, false, ports.elementAt(10), frontRightSensor);
addEntry(entries, false, ports.elementAt(11), frontLeftSensor);
addEntry(entries, false, ports.elementAt(12), rightFrontSensor);
addEntry(entries, false, ports.elementAt(13), rightBackSensor);
addEntry(entries, false, ports.elementAt(14), leftFrontSensor);
addEntry(entries, false, ports.elementAt(15), leftBackSensor);
addEntry(entries, false, ports.elementAt(16), backRightSensor);
addEntry(entries, false, ports.elementAt(17), backLeftSensor);
return entries;
}
......
......@@ -9,6 +9,7 @@ import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent;
import de.rwth.montisim.simulation.eesimulator.message.Message;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.environment.world.elements.Building;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
......@@ -24,9 +25,28 @@ public class Lidar extends EEComponent {
private transient int truePositionMsg;
private transient int trueCompassMsg;
private transient int frontSensorMsg;
// define sensors massages variables
private transient int frontRightSensorMsg;
private transient int frontLeftSensorMsg;
private transient int rightFrontSensorMsg;
private transient int rightBackSensorMsg;
private transient int leftFrontSensorMsg;
private transient int leftBackSensorMsg;
private transient int backRightSensorMsg;
private transient int backLeftSensorMsg;
public static final String FRONT_SENSOR_MSG = "front_sensor";
private double angleDeg;
public static final String FRONT_RIGHT_SENSOR_MSG = "front_right_lidar";
public static final String FRONT_LEFT_SENSOR_MSG = "front_left_lidar";
public static final String RIGHT_FRONT_SENSOR_MSG = "right_front_lidar";
public static final String RIGHT_BACK_SENSOR_MSG = "right_back_lidar";
public static final String LEFT_FRONT_SENSOR_MSG = "left_front_lidar";
public static final String LEFT_BACK_SENSOR_MSG = "left_back_lidar";
public static final String BACK_RIGHT_SENSOR_MSG = "back_right_lidar";
public static final String BACK_LEFT_SENSOR_MSG = "back_left_lidar";
private Vec2 truePosition = new Vec2(0,0);
private Vec2 orientation = new Vec2(0,0);
......@@ -46,7 +66,15 @@ public class Lidar extends EEComponent {
this.truePositionMsg = addPort(PortInformation.newOptionalInputDataPort(TruePosition.VALUE_NAME, TruePosition.TYPE, false));
this.trueCompassMsg = addPort(PortInformation.newOptionalInputDataPort(TrueCompass.VALUE_NAME, TrueCompass.TYPE, false));
this.frontSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(FRONT_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.frontRightSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(FRONT_RIGHT_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.frontLeftSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(FRONT_LEFT_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.rightFrontSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(RIGHT_FRONT_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.rightBackSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(RIGHT_BACK_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.leftFrontSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(LEFT_FRONT_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.leftBackSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(LEFT_BACK_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.backRightSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(BACK_RIGHT_SENSOR_MSG, PhysicalValueDouble.TYPE));
this.backLeftSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(BACK_LEFT_SENSOR_MSG, PhysicalValueDouble.TYPE));
}
@Override
protected void receive(MessageReceiveEvent msgRecvEvent) {
......@@ -56,23 +84,60 @@ public class Lidar extends EEComponent {
truePosition = (Vec2) msg.message;
compute(time);
} else if (msg.isMsg(trueCompassMsg)){
Double angleDeg = (Double) msg.message;
Double angleRad = angleDeg * Geometry.DEG_TO_RAD;
this.angleDeg = (double) msg.message;
double angleRad = angleDeg * Geometry.DEG_TO_RAD;
orientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
}
}
private void compute(Instant time) {
double distance = computeShortestDistance(truePosition, orientation);
sendMessage(time, frontSensorMsg, distance);
double length = 4.971;
double width = 1.87;
double height = 1.383;
Vec2 frontRightPosition = truePosition.add(length/2,width/4); //in front
Vec2 frontLeftPosition = truePosition.add(length/2,width/-4); //in front
Vec2 rightFrontPosition = truePosition.add(length/4,width/2); //on the right
Vec2 rightBackPosition = truePosition.add(length/-4,width/2); //on the right
Vec2 leftFrontPosition = truePosition.add(length/4,width/-2); //on the left
Vec2 leftBackPosition = truePosition.add(length/-4,width/-2); //on the left
Vec2 backRightPosition = truePosition.add(length/-2,width/4); //in the back
Vec2 backLeftPosition = truePosition.add(length/-2,width/-4); //in the back
double angleRad = (angleDeg + 90) * Geometry.DEG_TO_RAD;
Vec2 rightOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
angleRad = (angleDeg - 90) * Geometry.DEG_TO_RAD;
Vec2 leftOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
angleRad = (angleDeg + 180) * Geometry.DEG_TO_RAD;
Vec2 backOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
double distanceFrontRight = computeShortestDistance(frontRightPosition, orientation);
double distanceFrontLeft = computeShortestDistance(frontLeftPosition, orientation);
double distanceRightFront = computeShortestDistance(rightFrontPosition, rightOrientation);
double distanceRightBack = computeShortestDistance(rightBackPosition, rightOrientation);
double distanceLeftFront = computeShortestDistance(leftFrontPosition, leftOrientation);
double distanceLeftBack = computeShortestDistance(leftBackPosition, leftOrientation);
double distanceBackRight = computeShortestDistance(backRightPosition, backOrientation);
double distanceBackLeft = computeShortestDistance(backLeftPosition, backOrientation);
sendMessage(time, frontRightSensorMsg, distanceFrontRight);
sendMessage(time, frontLeftSensorMsg, distanceFrontLeft);
sendMessage(time, rightFrontSensorMsg, distanceRightFront);
sendMessage(time, rightBackSensorMsg, distanceRightBack);
sendMessage(time, leftFrontSensorMsg, distanceLeftFront);
sendMessage(time, leftBackSensorMsg, distanceLeftBack);
sendMessage(time, backRightSensorMsg, distanceBackRight);
sendMessage(time, backLeftSensorMsg, distanceBackLeft);
}
public double computeShortestDistance(Vec2 rayStart, Vec2 rayDirection) {
// Vec2 rayStart = position.asVec2();
// Vec2 rayDirection = direction.asVec2();
double shortestDistance = Double.MAX_VALUE, currentDistance = 0;
Vec2 currentIntersection = null, nearestIntersection = null;
double edgeScalar;
......
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