Commit a7170131 authored by Lukas Bram's avatar Lukas Bram
Browse files

Add basic Lidar with one front sensor for now

parent 08c9bd7c
Pipeline #388352 passed with stage
in 1 minute and 16 seconds
...@@ -7,6 +7,7 @@ import java.util.*; ...@@ -7,6 +7,7 @@ import java.util.*;
import de.rwth.montisim.commons.dynamicinterface.BasicType; import de.rwth.montisim.commons.dynamicinterface.BasicType;
import de.rwth.montisim.commons.dynamicinterface.PortInformation; import de.rwth.montisim.commons.dynamicinterface.PortInformation;
import de.rwth.montisim.commons.physicalvalue.PhysicalValueDouble;
import de.rwth.montisim.commons.simulation.Inspectable; import de.rwth.montisim.commons.simulation.Inspectable;
import de.rwth.montisim.commons.utils.Geometry; import de.rwth.montisim.commons.utils.Geometry;
import de.rwth.montisim.commons.utils.IPM; import de.rwth.montisim.commons.utils.IPM;
...@@ -17,6 +18,7 @@ import de.rwth.montisim.simulation.eesimulator.EEComponent; ...@@ -17,6 +18,7 @@ import de.rwth.montisim.simulation.eesimulator.EEComponent;
import de.rwth.montisim.simulation.eesimulator.EESystem; import de.rwth.montisim.simulation.eesimulator.EESystem;
import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent; import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent;
import de.rwth.montisim.simulation.eesimulator.message.Message; 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.Navigation;
import de.rwth.montisim.simulation.vehicle.physicalvalues.BatteryLevel; import de.rwth.montisim.simulation.vehicle.physicalvalues.BatteryLevel;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass; import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
...@@ -45,10 +47,13 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -45,10 +47,13 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
transient int batteryMsg; transient int batteryMsg;
transient int frontSensorMsg;
public double currentVelocity = 0; public double currentVelocity = 0;
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 frontSensor = 0;
double newTrajX[] = null; double newTrajX[] = null;
public int newTrajLength = 0; public int newTrajLength = 0;
...@@ -84,6 +89,8 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -84,6 +89,8 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
this.brakeMsg = addPort(PortInformation.newRequiredOutputDataPort(Actuator.SETTER_PREFIX + PowerTrainProperties.BRAKING_VALUE_NAME, BasicType.DOUBLE)); this.brakeMsg = addPort(PortInformation.newRequiredOutputDataPort(Actuator.SETTER_PREFIX + PowerTrainProperties.BRAKING_VALUE_NAME, BasicType.DOUBLE));
this.batteryMsg = addPort(PortInformation.newOptionalInputDataPort(BatteryLevel.VALUE_NAME, BatteryLevel.TYPE, false)); this.batteryMsg = addPort(PortInformation.newOptionalInputDataPort(BatteryLevel.VALUE_NAME, BatteryLevel.TYPE, false));
this.frontSensorMsg = addPort(PortInformation.newOptionalInputDataPort(Lidar.FRONT_SENSOR_MSG, PhysicalValueDouble.TYPE, false));
} }
@Override @Override
...@@ -108,6 +115,8 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -108,6 +115,8 @@ 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(frontSensorMsg)) {
this.frontSensor = (double) msg.message;
} }
} }
...@@ -405,6 +414,7 @@ public class JavaAutopilot extends EEComponent implements Inspectable { ...@@ -405,6 +414,7 @@ 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), frontSensor);
return entries; return entries;
} }
......
...@@ -5,6 +5,7 @@ import de.rwth.montisim.simulation.eecomponents.autopilots.JavaAutopilotProperti ...@@ -5,6 +5,7 @@ import de.rwth.montisim.simulation.eecomponents.autopilots.JavaAutopilotProperti
import de.rwth.montisim.simulation.eesimulator.bus.constant.ConstantBusProperties; import de.rwth.montisim.simulation.eesimulator.bus.constant.ConstantBusProperties;
import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties; import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
import de.rwth.montisim.simulation.vehicle.VehicleProperties; 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.NavigationProperties;
import de.rwth.montisim.simulation.vehicle.physicalvalues.*; import de.rwth.montisim.simulation.vehicle.physicalvalues.*;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties; import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
...@@ -64,6 +65,11 @@ public class DefaultVehicleConfig { ...@@ -64,6 +65,11 @@ public class DefaultVehicleConfig {
new NavigationProperties() new NavigationProperties()
.connectTo("DefaultBus") .connectTo("DefaultBus")
); );
properties.addComponent(
new LidarProperties()
.connectTo("DefaultBus")
);
properties.addComponent( properties.addComponent(
ConstantBusProperties.instantBus().setName("DefaultBus") ConstantBusProperties.instantBus().setName("DefaultBus")
......
...@@ -15,6 +15,7 @@ import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent.Messag ...@@ -15,6 +15,7 @@ import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent.Messag
import de.rwth.montisim.simulation.eesimulator.events.MessageSendEvent.MessageSendEventData; import de.rwth.montisim.simulation.eesimulator.events.MessageSendEvent.MessageSendEventData;
import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties; import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
import de.rwth.montisim.simulation.eesimulator.testcomponents.TestCompProperties; 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.NavigationProperties;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties; import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties; import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
...@@ -37,6 +38,7 @@ public class TypedSimulation { ...@@ -37,6 +38,7 @@ public class TypedSimulation {
Json.registerType(SensorProperties.class); Json.registerType(SensorProperties.class);
Json.registerType(TestCompProperties.class); Json.registerType(TestCompProperties.class);
Json.registerType(NavigationProperties.class); Json.registerType(NavigationProperties.class);
Json.registerType(LidarProperties.class);
Json.registerType(RigidbodyPhysicsProperties.class); Json.registerType(RigidbodyPhysicsProperties.class);
Json.registerType(ElectricalPTProperties.class); Json.registerType(ElectricalPTProperties.class);
Json.registerType(MetricGoalProperties.class); Json.registerType(MetricGoalProperties.class);
......
...@@ -54,6 +54,12 @@ ...@@ -54,6 +54,12 @@
</dependency> --> </dependency> -->
<!-- Testing --> <!-- Testing -->
<dependency>
<groupId>org.mockito</groupId>
<artifactId>mockito-core</artifactId>
<version>3.7.0</version>
<scope>test</scope>
</dependency>
<!-- <dependency> <!-- <dependency>
<groupId>org.powermock</groupId> <groupId>org.powermock</groupId>
<artifactId>powermock-module-junit4</artifactId> <artifactId>powermock-module-junit4</artifactId>
......
package de.rwth.montisim.simulation.vehicle.lidar;
import de.rwth.montisim.commons.dynamicinterface.PortInformation;
import de.rwth.montisim.commons.physicalvalue.PhysicalValueDouble;
import de.rwth.montisim.commons.utils.*;
import de.rwth.montisim.simulation.eesimulator.EEComponent;
import de.rwth.montisim.simulation.eesimulator.EESystem;
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.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
import java.time.Instant;
import java.util.Vector;
public class Lidar extends EEComponent {
public World world;
private final Vector<Edge> edges = new Vector<>();
private transient int truePositionMsg;
private transient int trueCompassMsg;
private transient int frontSensorMsg;
public static final String FRONT_SENSOR_MSG = "front_sensor";
private Vec2 truePosition = new Vec2(0,0);
private Vec2 orientation = new Vec2(0,0);
public static final double DOUBLE_TOLERANCE = 0.000001d;
public Lidar(LidarProperties properties, EESystem eeSystem, World world){
super(properties,eeSystem);
for (Building building : world.buildings) {
int corners = building.boundary.size();
for (int i=0; i < corners; i++) {
//Line line = getLineBetweenPoints(building.boundary.elementAt(i), building.boundary.elementAt(i%corners));
edges.add(new Edge(building.boundary.elementAt(i), building.boundary.elementAt((i+1)%corners)));
}
}
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));
}
@Override
protected void receive(MessageReceiveEvent msgRecvEvent) {
Message msg = msgRecvEvent.getMessage();
Instant time = msgRecvEvent.getEventTime();
if (msg.isMsg(truePositionMsg)){
truePosition = (Vec2) msg.message;
compute(time);
} else if (msg.isMsg(trueCompassMsg)){
Double 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);
}
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;
for (Edge edge : edges) {
Vec2 edgeStart = edge.start.asVec2();
Vec2 edgeDirection = edge.end.subtract(edge.start).asVec2();
double directionsDeterminant = determinant2x2(edgeDirection, rayDirection);
if (!UMath.equalsThreshold(directionsDeterminant, 0, DOUBLE_TOLERANCE)) {
edgeScalar = (determinant2x2(rayDirection, edgeStart) + determinant2x2(rayStart, rayDirection)) / directionsDeterminant;
if (edgeScalar >= 0 && edgeScalar <= 1) {
// ray hits the edge
currentIntersection = edgeStart.add(edgeDirection.multiply(edgeScalar));
currentDistance = currentIntersection.distance(rayStart);
if (currentDistance < shortestDistance) {
shortestDistance = currentDistance;
nearestIntersection = currentIntersection;
}
}
}
}
return shortestDistance;
}
private double determinant2x2(Vec2 a, Vec2 b){
return a.x*b.y - a.y*b.x;
}
private static class Edge {
public Vec3 start;
public Vec3 end;
public Edge(Vec3 start, Vec3 end) {
this.start = start;
this.end = end;
}
}
}
package de.rwth.montisim.simulation.vehicle.lidar;
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;
import de.rwth.montisim.simulation.eesimulator.EESystem;
import de.rwth.montisim.simulation.eesimulator.exceptions.EEMessageTypeException;
import de.rwth.montisim.simulation.environment.world.World;
@Typed(LidarProperties.TYPE)
public class LidarProperties extends EEComponentProperties {
public static final String TYPE = "lidar";
public static final String NAME = "Lidar";
public LidarProperties() {
this.name = NAME;
}
public LidarProperties setName(String name) {
this.name = name;
return this;
}
@Override
public EEComponentType getGeneralType() {
return EEComponentType.SERVICE;
}
@Override
public String getType() {
return TYPE;
}
@Override
public EEComponent build(EESystem eesystem, BuildContext context) throws EEMessageTypeException {
return new Lidar(this, eesystem, context.getObject(World.CONTEXT_KEY));
}
}
package de.rwth.montisim.simulation.vehicle.lidar;
import de.rwth.montisim.commons.utils.Vec2;
import de.rwth.montisim.commons.utils.Vec3;
import de.rwth.montisim.simulation.eesimulator.EESystem;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.environment.world.elements.Building;
import org.junit.Assert;
import org.junit.Test;
import static org.mockito.Mockito.mock;
public class LidarTest {
@Test
public void computeShortestDistanceTest() {
World world = new World("Stub");
Building building = new Building("Testbuilding","Testtype", 20d, 3);
building.boundary.add(new Vec3(-30,80,0));
building.boundary.add(new Vec3(-30,50,0));
building.boundary.add(new Vec3(10,50,0));
building.boundary.add(new Vec3(10,80,0));
world.buildings.add(building);
Vec2 vehiclePos = new Vec2(0,0);
Vec2 ray = new Vec2(0,1);
LidarProperties lidarProperties = mock(LidarProperties.class);
EESystem eeSystem = mock(EESystem.class);
Lidar lidar = new Lidar(lidarProperties,eeSystem,world);
Assert.assertEquals(50d, lidar.computeShortestDistance(vehiclePos, ray), Lidar.DOUBLE_TOLERANCE);
}
@Test
public void computeShortestDistanceInfinityTest() {
World world = new World("Stub");
Vec2 vehiclePos = new Vec2(0,0);
Vec2 ray = new Vec2(0,1);
LidarProperties lidarProperties = mock(LidarProperties.class);
EESystem eeSystem = mock(EESystem.class);
Lidar lidar = new Lidar(lidarProperties,eeSystem,world);
Assert.assertEquals(Double.MAX_VALUE, lidar.computeShortestDistance(vehiclePos, ray), Lidar.DOUBLE_TOLERANCE);
}
}
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