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

Add test for SpeedLimitService and do some general code cleanup

parent 2574e8cf
Pipeline #413062 passed with stage
in 1 minute and 4 seconds
......@@ -71,7 +71,6 @@ public class Vehicle extends SimulationObject implements Updatable, Destroyable,
physicalValues.addPhysicalValue(new TrueCompass(physicalObject));
physicalValues.addPhysicalValue(new TrueVelocity(physicalObject));
physicalValues.addPhysicalValue(new TruePosition(physicalObject));
physicalValues.addPhysicalValue(new UpperSpeedLimit(physicalObject));
}
public static final String K_CONFIG = "config";
......
......@@ -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.Vehicle;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
......@@ -18,6 +19,10 @@ import java.util.Arrays;
import java.util.List;
import java.util.Vector;
/**
* This component is responsible for computing distances from the vehicle to the surrounding buildings.
* It currently uses 8 distance sensors placed on the chassis of the vehicle
*/
public class Lidar extends EEComponent {
public World world;
......@@ -38,9 +43,13 @@ public class Lidar extends EEComponent {
private Vec2 orientation = new Vec2(0,0);
public static final double DOUBLE_TOLERANCE = 0.000001d;
public Lidar(LidarProperties properties, EESystem eeSystem, World world){
private final Vehicle vehicle;
public Lidar(LidarProperties properties, EESystem eeSystem, World world, Vehicle vehicle){
super(properties,eeSystem);
this.vehicle = vehicle;
for (Building building : world.buildings) {
int corners = building.boundary.size();
for (int i=0; i < corners; i++) {
......@@ -59,8 +68,8 @@ public class Lidar extends EEComponent {
@Override
protected void receive(MessageReceiveEvent msgRecvEvent) {
Message msg = msgRecvEvent.getMessage();
Instant time = msgRecvEvent.getEventTime();
if (msg.isMsg(truePositionMsg)){
Instant time = msgRecvEvent.getEventTime();
truePosition = (Vec2) msg.message;
compute(time);
} else if (msg.isMsg(trueCompassMsg)){
......@@ -70,10 +79,14 @@ public class Lidar extends EEComponent {
}
}
/**
* The main computing part of the component.
* Here the distances are computed and sent to other components
* @param time the time of the last received truePosition message
*/
private void compute(Instant time) {
double length = 4.971;
double width = 1.87;
double length = vehicle.properties.body.length;
double width = vehicle.properties.body.width;
double angleRad = (angleDeg + 90) * Geometry.DEG_TO_RAD;
Vec2 rightOrientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
......@@ -103,8 +116,13 @@ public class Lidar extends EEComponent {
}
/**
* Sends out a ray from a given point (rayStart) in a given direction (rayDirection)
* and returns the distance traveled until it hits the first building in the world.
* @param rayStart the starting point from where to shoot the ray
* @param rayDirection the direction of the ray
* @return the closest distance to a building
*/
public double computeShortestDistance(Vec2 rayStart, Vec2 rayDirection) {
double shortestDistance = Double.MAX_VALUE, currentDistance = 0;
Vec2 currentIntersection = null, nearestIntersection = null;
......@@ -131,10 +149,19 @@ public class Lidar extends EEComponent {
return shortestDistance;
}
/**
* Calculates the determinant for a 2x2 matrix
* @param a the first column of the matrix
* @param b the second column of the matrix
* @return the determinant of the matrix
*/
private double determinant2x2(Vec2 a, Vec2 b){
return a.x*b.y - a.y*b.x;
}
/**
* Simple representation class for an edge, which consists of two points.
*/
private static class Edge {
public Vec3 start;
public Vec3 end;
......
......@@ -8,6 +8,7 @@ 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;
import de.rwth.montisim.simulation.vehicle.Vehicle;
@Typed(LidarProperties.TYPE)
public class LidarProperties extends EEComponentProperties {
......@@ -35,6 +36,6 @@ public class LidarProperties extends EEComponentProperties {
@Override
public EEComponent build(EESystem eesystem, BuildContext context) throws EEMessageTypeException {
return new Lidar(this, eesystem, context.getObject(World.CONTEXT_KEY));
return new Lidar(this, eesystem, context.getObject(World.CONTEXT_KEY), context.getObject( Vehicle.CONTEXT_KEY ));
}
}
......@@ -18,25 +18,27 @@ import java.time.Instant;
import java.util.Optional;
import java.util.Vector;
/**
* This component supplies the vehicle with the upper speed limits for the current trajectory.
*/
public class SpeedLimitService extends EEComponent {
private World world;
private SpeedLimitServiceProperties properties;
/**
* The simulated world. Used for fetching speed limits
*/
private final World world;
private double[] trajectoryX = null;
private double[] trajectoryY = null;
private int trajectoryLength = 0;
//inputs
transient int trajectoryXMsg,
trajectoryYMsg,
trajectoryLengthMsg;
//input ports
transient int trajectoryXMsg,
trajectoryYMsg,
trajectoryLengthMsg;
//outputs
transient int upperSpeedLimitMsg,
lowerSpeedLimitMsg;
//output ports
transient int upperSpeedLimitMsg;
public static final String UPPER_SPEED_LIMIT_MSG = "upper_speed_limit";
......@@ -67,22 +69,30 @@ public class SpeedLimitService extends EEComponent {
}
}
/**
* Main compute method of this component. It is executed each time a trajetory is received.
* @param time the event time of the last trajectoryY event
*/
private void compute(Instant time) {
//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 - 1));
}
private double[] fetchUpperSpeedLimits() {
Vector<Way> ways = world.ways;
/**
* Tries to fetch the upper speed limits for all the trajectory segments. Each segment is mapped
* to its speed limit and if it doesn't have one, -1 is returned.
* @return array of speed limits (with a length of trajectoryLength - 1)
*/
public double[] fetchUpperSpeedLimits() {
double[] maxSpeedArr = new double[trajectoryLength - 1];
for (int i=0; i < trajectoryLength - 1; i++){
maxSpeedArr[i] = -1;
}
if ( trajectoryX != null && trajectoryLength > 1){
PointInformation previousPointInfo = getPointInformation(world.ways, new Vec3(trajectoryX[0], trajectoryY[0], 0));
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);
......@@ -96,6 +106,7 @@ public class SpeedLimitService extends EEComponent {
} 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<>();
......@@ -111,6 +122,14 @@ public class SpeedLimitService extends EEComponent {
return maxSpeedArr;
}
/**
* Checks if two points lie on the same way directly behind each other. If they do, the way is returned, otherwise
* an Optional.Empty() is returned. Only the ways in the pointAInfo object are considered.
* If two ways which fulfill the condition exist, only the first one is returned.
* @param pointAInfo point information for a point A
* @param pointB point B
* @return optional of the way, where the two points are contained.
*/
private Optional<Way> getMutualWay(PointInformation pointAInfo, Vec3 pointB) {
for (Way way : pointAInfo.ways){
for (int i=0; i < way.points.size() - 1; i++) {
......@@ -127,8 +146,12 @@ public class SpeedLimitService extends EEComponent {
/** @function: getNodeIdOfPoint
* @return: the nodeId if the given @code{point} is an intersection, otherwise -1
/**
* Collects information about a point from a given vector of ways. The information
* is returned as a PointInformation object
* @param point the point to collect information about
* @param ways Some ways that can, but don't have to contain the point
* @return a PointInformation object with the corresponding coordinates, ways and node
* */
private PointInformation getPointInformation(Vector<Way> ways, Vec3 point){
PointInformation pointInformation = new PointInformation();
......@@ -148,11 +171,24 @@ public class SpeedLimitService extends EEComponent {
}
/**
* @Class: PointInformation
* It provides information about a specific point from the world.*/
* PointInformation
* It provides information about a specific point from the world.
*/
private static class PointInformation{
/**
* The coordinates of the point
*/
public Vec3 coordinates;
/**
* A node corresponding to the point. Not every point is correlated
* to a node, so this can be null.
*/
public Node node;
/**
* A vector of ways that contain the point.
*/
public Vector<Way> ways = new Vector<>();
}
}
......@@ -3,6 +3,9 @@ package de.rwth.montisim.simulation.vehicle.physicalvalues;
import de.rwth.montisim.commons.physicalvalue.PhysicalValueDouble;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.battery.Battery;
/**
* Current battery level in percent
*/
public class BatteryLevel extends PhysicalValueDouble {
public static final String VALUE_NAME = "battery_level";
transient final Battery battery;
......@@ -12,8 +15,6 @@ public class BatteryLevel extends PhysicalValueDouble {
this.battery = battery;
}
/**
* @return battery level in percentage */
@Override
public Object get(){
return battery.percentage();
......
package de.rwth.montisim.simulation.vehicle.physicalvalues;
import de.rwth.montisim.commons.physicalvalue.PhysicalValueDouble;
import de.rwth.montisim.commons.simulation.DynamicObject;
public class UpperSpeedLimit extends PhysicalValueDouble {
public static final String VALUE_NAME = "upper_speed_limit";
//transient final DynamicObject object;
public UpperSpeedLimit(DynamicObject object) {
super(VALUE_NAME);
}
/**
* @return [x,y, upper_speed_limit] */
@Override
public Object get(){
return 10;
}
@Override
public void set(Object value){
// Cannot change the velocity this way
}
}
......@@ -5,6 +5,8 @@ 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 de.rwth.montisim.simulation.vehicle.Vehicle;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import org.junit.Assert;
import org.junit.Test;
......@@ -27,7 +29,8 @@ public class LidarTest {
LidarProperties lidarProperties = mock(LidarProperties.class);
EESystem eeSystem = mock(EESystem.class);
Lidar lidar = new Lidar(lidarProperties,eeSystem,world);
Vehicle vehicle = mock(Vehicle.class);
Lidar lidar = new Lidar(lidarProperties,eeSystem,world,vehicle);
Assert.assertEquals(50d, lidar.computeShortestDistance(vehiclePos, ray), Lidar.DOUBLE_TOLERANCE);
......@@ -43,7 +46,8 @@ public class LidarTest {
LidarProperties lidarProperties = mock(LidarProperties.class);
EESystem eeSystem = mock(EESystem.class);
Lidar lidar = new Lidar(lidarProperties,eeSystem,world);
Vehicle vehicle = mock(Vehicle.class);
Lidar lidar = new Lidar(lidarProperties,eeSystem,world,vehicle);
Assert.assertEquals(Double.MAX_VALUE, lidar.computeShortestDistance(vehiclePos, ray), Lidar.DOUBLE_TOLERANCE);
......
package de.rwth.montisim.simulation.vehicle.navigation;
import de.rwth.montisim.commons.dynamicinterface.BasicType;
import de.rwth.montisim.commons.dynamicinterface.DataType;
import de.rwth.montisim.commons.dynamicinterface.VectorType;
import de.rwth.montisim.commons.eventsimulation.DiscreteEventSimulator;
import de.rwth.montisim.commons.utils.BuildContext;
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.eesimulator.events.MessageReceiveEvent;
import de.rwth.montisim.simulation.eesimulator.exceptions.EEMessageTypeException;
import de.rwth.montisim.simulation.eesimulator.message.Message;
import de.rwth.montisim.simulation.eesimulator.message.MessageInformation;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.environment.world.elements.Building;
import de.rwth.montisim.simulation.environment.world.elements.Node;
import de.rwth.montisim.simulation.environment.world.elements.Way;
import de.rwth.montisim.simulation.vehicle.lidar.Lidar;
import de.rwth.montisim.simulation.vehicle.lidar.LidarProperties;
import org.junit.Assert;
import org.junit.Test;
import java.time.Instant;
import java.util.HashMap;
import java.util.Map;
import java.util.Vector;
import static org.mockito.ArgumentMatchers.*;
import static org.mockito.Mockito.*;
public class SpeedLimitServiceTest {
@Test
public void test_fetchUpperSpeedLimits() throws EEMessageTypeException {
World world = new World("Stub");
Way way1 = new Way("way_1", true,1, false, 30 );
Way way2 = new Way("way_2", true,1, false, 50 );
Way way3 = new Way("way_3", true,1, false, 30 );
world.addWay(way1);
world.addWay(way2);
world.addWay(way3);
//first intersection between way1 and way2
Node node1 = new Node(new Vec3(20,0,0));
node1.ways.add(way1);
node1.ways.add(way2);
int node1Id = world.addNode(node1);
//second intersection between way2 and way3
Node node2 = new Node(new Vec3(50,0,0));
node2.ways.add(way2);
node2.ways.add(way3);
int node2Id = world.addNode(node2);
//waypoints of way1
way1.addPoint(new Vec3(0,0,0), -1);
way1.addPoint(new Vec3(10,0,0), -1);
way1.addPoint(node1.point, node1Id);
//waypoints of way2
way2.addPoint(node1.point, node1Id);
way2.addPoint(new Vec3(30,0,0), -1);
way2.addPoint(new Vec3(40,0,0), -1);
way2.addPoint(node2.point, node2Id);
//waypoints of way3
way3.addPoint(node2.point, node2Id);
way3.addPoint(new Vec3(60,0,0), -1);
way3.addPoint(new Vec3(70,0,0), -1);
way3.addPoint(new Vec3(80,0,0), -1);
way3.addPoint(new Vec3(90,0,0), -1);
//construct SpeedlimitService with the world
BuildContext buildContext = new BuildContext();
buildContext.addObject(world, World.CONTEXT_KEY);
DiscreteEventSimulator simulator = mock(DiscreteEventSimulator.class);
EESystem eeSystem = new EESystem(simulator);
SpeedLimitService speedLimitService = (SpeedLimitService) new SpeedLimitServiceProperties().build(eeSystem, buildContext);
//the test trajectory with same points as on the individual ways
double[] trajectoryX = new double[]{ 0 , 10 , 20 , 30 , 40 , 50 , 60 , 70 , 80 , 90};
double[] trajectoryY = new double[]{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0};
int trajectoryLength = 10;
VectorType trajectoryDatatype = new VectorType(BasicType.DOUBLE, trajectoryLength);
//send the test trajectory to the SpeedLimitService
MessageInformation trajectoryLengthMessageInformation =
new MessageInformation(speedLimitService.trajectoryLengthMsg, null, BasicType.N, null);
MessageInformation trajectoryXMessageInformation =
new MessageInformation(
speedLimitService.trajectoryXMsg,
null,
trajectoryDatatype,
null
);
MessageInformation trajectoryYMessageInformation =
new MessageInformation(
speedLimitService.trajectoryYMsg,
null,
trajectoryDatatype,
null
);
Message trajectoryLengthMessage = new Message(trajectoryLengthMessageInformation, trajectoryLength);
Message trajectoryXMessage = new Message(trajectoryXMessageInformation, trajectoryX, trajectoryLength);
Message trajectoryYMessage = new Message(trajectoryYMessageInformation, trajectoryY, trajectoryLength);
speedLimitService.receive(new MessageReceiveEvent(speedLimitService, Instant.now(), trajectoryLengthMessage));
speedLimitService.receive(new MessageReceiveEvent(speedLimitService, Instant.now(), trajectoryXMessage));
speedLimitService.receive(new MessageReceiveEvent(speedLimitService, Instant.now(), trajectoryYMessage));
//assertion
double[] expectedSpeedLimits = new double[]{30,30,50,50,50,30,30,30,30};
double[] actualSpeedLimits = speedLimitService.fetchUpperSpeedLimits();
Assert.assertArrayEquals(expectedSpeedLimits, actualSpeedLimits, Lidar.DOUBLE_TOLERANCE);
}
}
This diff is collapsed.
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