Commit 514c6abd authored by Ana Yurgandzhieva's avatar Ana Yurgandzhieva
Browse files

reduced the number of lines for the lidar code

parent fa5f3eaf
Pipeline #400563 passed with stage
in 4 minutes and 28 seconds
...@@ -48,14 +48,7 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -48,14 +48,7 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
transient int batteryMsg; transient int batteryMsg;
transient int frontRightSensorMsg; transient List<Integer> sensorMsg = Arrays.asList(0,0,0,0,0,0,0,0);
transient int frontLeftSensorMsg;
transient int rightFrontSensorMsg;
transient int rightBackSensorMsg;
transient int leftFrontSensorMsg;
transient int leftBackSensorMsg;
transient int backRightSensorMsg;
transient int backLeftSensorMsg;
transient int upperSpeedLimitMsg; transient int upperSpeedLimitMsg;
...@@ -63,14 +56,7 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -63,14 +56,7 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
public Vec2 currentPosition = null; public Vec2 currentPosition = null;
public double currentCompass = Double.NaN; public double currentCompass = Double.NaN;
public double batteryLevel = 0; public double batteryLevel = 0;
public double frontRightSensor = 0; public List<Double> lidarSensors = Arrays.asList(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.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; double newTrajX[] = null;
public int newTrajLength = 0; public int newTrajLength = 0;
...@@ -109,14 +95,9 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -109,14 +95,9 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
this.batteryMsg = addPort(PortInformation.newOptionalInputDataPort(BatteryLevel.VALUE_NAME, BatteryLevel.TYPE, false)); this.batteryMsg = addPort(PortInformation.newOptionalInputDataPort(BatteryLevel.VALUE_NAME, BatteryLevel.TYPE, false));
this.frontRightSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.FRONT_RIGHT_SENSOR_MSG, PhysicalValueDouble.TYPE, false)); for (int i=0; i<sensorMsg.size(); i++){
this.frontLeftSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.FRONT_LEFT_SENSOR_MSG, PhysicalValueDouble.TYPE, false)); this.sensorMsg.set(i, addPort(PortInformation.newOptionalInputDataPort(Lidar.LIDAR_MSG.get(i), 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));
this.upperSpeedLimitMsg = addPort(PortInformation.newOptionalInputDataPort(SpeedLimitService.UPPER_SPEED_LIMIT_MSG, SpeedLimitService.SPEED_LIMIT_TYPE, false)); this.upperSpeedLimitMsg = addPort(PortInformation.newOptionalInputDataPort(SpeedLimitService.UPPER_SPEED_LIMIT_MSG, SpeedLimitService.SPEED_LIMIT_TYPE, false));
...@@ -144,24 +125,13 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -144,24 +125,13 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
trajX = newTrajX; trajX = newTrajX;
} else if (msg.isMsg(batteryMsg)) { } else if (msg.isMsg(batteryMsg)) {
batteryLevel = (double) msg.message; batteryLevel = (double) msg.message;
} else if (msg.isMsg(frontRightSensorMsg)) { }
this.frontRightSensor = (double) msg.message; else {
} else if (msg.isMsg(frontLeftSensorMsg)) { for (int i=0; i<sensorMsg.size(); i++){
this.frontLeftSensor = (double) msg.message; if(msg.isMsg(sensorMsg.get(i))){
} else if (msg.isMsg(rightFrontSensorMsg)) { this.lidarSensors.set(i, (double) msg.message);
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;
}else if (msg.isMsg(upperSpeedLimitMsg)) {
this.upperSpeedLimitArr = (double[]) msg.message;
} }
} }
...@@ -459,14 +429,14 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -459,14 +429,14 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
addEntry(entries, true, ports.elementAt(7), currentGas); addEntry(entries, true, ports.elementAt(7), currentGas);
addEntry(entries, true, ports.elementAt(8), currentBrakes); addEntry(entries, true, ports.elementAt(8), currentBrakes);
addEntry(entries, false, ports.elementAt(9), batteryLevel); addEntry(entries, false, ports.elementAt(9), batteryLevel);
addEntry(entries, false, ports.elementAt(10), frontRightSensor); addEntry(entries, false, ports.elementAt(10), lidarSensors.get(0)); // frontRightSensor
addEntry(entries, false, ports.elementAt(11), frontLeftSensor); addEntry(entries, false, ports.elementAt(11), lidarSensors.get(1)); // frontLeftSensor
addEntry(entries, false, ports.elementAt(12), rightFrontSensor); addEntry(entries, false, ports.elementAt(12), lidarSensors.get(2)); // rightFrontSensor
addEntry(entries, false, ports.elementAt(13), rightBackSensor); addEntry(entries, false, ports.elementAt(13), lidarSensors.get(3)); // rightBackSensor
addEntry(entries, false, ports.elementAt(14), leftFrontSensor); addEntry(entries, false, ports.elementAt(14), lidarSensors.get(4)); // leftFrontSensor
addEntry(entries, false, ports.elementAt(15), leftBackSensor); addEntry(entries, false, ports.elementAt(15), lidarSensors.get(5)); // leftBackSensor
addEntry(entries, false, ports.elementAt(16), backRightSensor); addEntry(entries, false, ports.elementAt(16), lidarSensors.get(6)); // backRightSensor
addEntry(entries, false, ports.elementAt(17), backLeftSensor); addEntry(entries, false, ports.elementAt(17), lidarSensors.get(7)); // backLeftSensor
addEntry(entries, false, ports.elementAt(18), upperSpeedLimitArr); addEntry(entries, false, ports.elementAt(18), upperSpeedLimitArr);
return entries; return entries;
} }
......
...@@ -14,6 +14,8 @@ import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass; ...@@ -14,6 +14,8 @@ import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition; import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
import java.time.Instant; import java.time.Instant;
import java.util.Arrays;
import java.util.List;
import java.util.Vector; import java.util.Vector;
public class Lidar extends EEComponent { public class Lidar extends EEComponent {
...@@ -25,28 +27,12 @@ public class Lidar extends EEComponent { ...@@ -25,28 +27,12 @@ public class Lidar extends EEComponent {
private transient int truePositionMsg; private transient int truePositionMsg;
private transient int trueCompassMsg; private transient int trueCompassMsg;
// define sensors massages variables private transient List<Integer> lidarMsg = Arrays.asList(0,0,0,0,0,0,0,0);
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;
private double angleDeg; private double angleDeg;
public static final List<String> LIDAR_MSG = Arrays.asList("front_right_lidar", "front_left_lidar",
public static final String FRONT_RIGHT_SENSOR_MSG = "front_right_lidar"; "right_front_lidar", "right_back_lidar", "left_front_lidar", "left_back_lidar",
public static final String FRONT_LEFT_SENSOR_MSG = "front_left_lidar"; "back_right_lidar", "back_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 truePosition = new Vec2(0,0);
private Vec2 orientation = new Vec2(0,0); private Vec2 orientation = new Vec2(0,0);
...@@ -66,15 +52,9 @@ public class Lidar extends EEComponent { ...@@ -66,15 +52,9 @@ public class Lidar extends EEComponent {
this.truePositionMsg = addPort(PortInformation.newOptionalInputDataPort(TruePosition.VALUE_NAME, TruePosition.TYPE, false)); this.truePositionMsg = addPort(PortInformation.newOptionalInputDataPort(TruePosition.VALUE_NAME, TruePosition.TYPE, false));
this.trueCompassMsg = addPort(PortInformation.newOptionalInputDataPort(TrueCompass.VALUE_NAME, TrueCompass.TYPE, false)); this.trueCompassMsg = addPort(PortInformation.newOptionalInputDataPort(TrueCompass.VALUE_NAME, TrueCompass.TYPE, false));
this.frontRightSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(FRONT_RIGHT_SENSOR_MSG, PhysicalValueDouble.TYPE)); for (int i=0; i<lidarMsg.size(); i++ ){
this.frontLeftSensorMsg = addPort(PortInformation.newOptionalOutputDataPort(FRONT_LEFT_SENSOR_MSG, PhysicalValueDouble.TYPE)); this.lidarMsg.set(i, addPort(PortInformation.newOptionalOutputDataPort(LIDAR_MSG.get(i), 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 @Override
protected void receive(MessageReceiveEvent msgRecvEvent) { protected void receive(MessageReceiveEvent msgRecvEvent) {
...@@ -94,17 +74,6 @@ public class Lidar extends EEComponent { ...@@ -94,17 +74,6 @@ public class Lidar extends EEComponent {
double length = 4.971; double length = 4.971;
double width = 1.87; 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; double angleRad = (angleDeg + 90) * Geometry.DEG_TO_RAD;
Vec2 rightOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad)); Vec2 rightOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
...@@ -115,23 +84,22 @@ public class Lidar extends EEComponent { ...@@ -115,23 +84,22 @@ public class Lidar extends EEComponent {
angleRad = (angleDeg + 180) * Geometry.DEG_TO_RAD; angleRad = (angleDeg + 180) * Geometry.DEG_TO_RAD;
Vec2 backOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad)); Vec2 backOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
double distanceFrontRight = computeShortestDistance(frontRightPosition, orientation); // The lidars in the lists always follow the following order: front right, front left,
double distanceFrontLeft = computeShortestDistance(frontLeftPosition, orientation); // right front, right back, left front, left back, back right, back left
double distanceRightFront = computeShortestDistance(rightFrontPosition, rightOrientation); List<Vec2> positionOffset = Arrays.asList(new Vec2(length/2,width/4), new Vec2(length/2,width/-4),
double distanceRightBack = computeShortestDistance(rightBackPosition, rightOrientation); new Vec2(length/4,width/2), new Vec2(length/-4,width/2),
double distanceLeftFront = computeShortestDistance(leftFrontPosition, leftOrientation); new Vec2(length/4,width/-2), new Vec2(length/-4,width/-2),
double distanceLeftBack = computeShortestDistance(leftBackPosition, leftOrientation); new Vec2(length/-2,width/4), new Vec2(length/-2,width/-4));
double distanceBackRight = computeShortestDistance(backRightPosition, backOrientation);
double distanceBackLeft = computeShortestDistance(backLeftPosition, backOrientation); List<Vec2> orientationOffset = Arrays.asList(orientation, orientation,
rightOrientation, rightOrientation,
sendMessage(time, frontRightSensorMsg, distanceFrontRight); leftOrientation, leftOrientation,
sendMessage(time, frontLeftSensorMsg, distanceFrontLeft); backOrientation, backOrientation);
sendMessage(time, rightFrontSensorMsg, distanceRightFront);
sendMessage(time, rightBackSensorMsg, distanceRightBack); for (int i=0; i<lidarMsg.size(); i++){
sendMessage(time, leftFrontSensorMsg, distanceLeftFront); double distance = computeShortestDistance(truePosition.add(positionOffset.get(i)), orientationOffset.get(i));
sendMessage(time, leftBackSensorMsg, distanceLeftBack); sendMessage(time, lidarMsg.get(i), distance);
sendMessage(time, backRightSensorMsg, distanceBackRight); }
sendMessage(time, backLeftSensorMsg, distanceBackLeft);
} }
......
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