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 10a3d15d authored by Anja Veleva's avatar Anja Veleva
Browse files

added all 8 lidar sensors

parent a7170131
Pipeline #396112 passed with stage
in 1 minute and 1 second
......@@ -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