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
//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