Commit 2278c239 authored by Evgeny Kusmenko's avatar Evgeny Kusmenko
Browse files

Merge branch 'dev_ema' into 'master'

Ema group contributions to the simulation

See merge request !39
parents bf66c6c2 30f0c11d
Pipeline #428837 failed with stage
in 55 seconds
......@@ -7,6 +7,7 @@ import java.util.*;
import de.rwth.montisim.commons.dynamicinterface.BasicType;
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.utils.Geometry;
import de.rwth.montisim.commons.utils.IPM;
......@@ -17,7 +18,10 @@ 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.vehicle.lidar.Lidar;
import de.rwth.montisim.simulation.vehicle.navigation.Navigation;
import de.rwth.montisim.simulation.vehicle.navigation.SpeedLimitService;
import de.rwth.montisim.simulation.vehicle.physicalvalues.BatteryLevel;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueVelocity;
......@@ -42,9 +46,17 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
transient int accelMsg;
transient int brakeMsg;
transient int batteryMsg;
transient List<Integer> sensorMsg = Arrays.asList(0,0,0,0,0,0,0,0);
transient int upperSpeedLimitMsg;
public double currentVelocity = 0;
public Vec2 currentPosition = null;
public double currentCompass = Double.NaN;
public double batteryLevel = 0;
public List<Double> lidarSensors = Arrays.asList(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
double newTrajX[] = null;
public int newTrajLength = 0;
......@@ -52,6 +64,8 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
public double trajX[] = null;
public double trajY[] = null;
public double upperSpeedLimitArr[] = null;
public double currentGas = 0;
public double currentSteering = 0;
public double currentBrakes = 0;
......@@ -78,6 +92,15 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
BasicType.DOUBLE));
this.accelMsg = addPort(PortInformation.newRequiredOutputDataPort(Actuator.SETTER_PREFIX + PowerTrainProperties.GAS_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));
for (int i=0; i<sensorMsg.size(); i++){
this.sensorMsg.set(i, addPort(PortInformation.newOptionalInputDataPort(Lidar.LIDAR_MSG.get(i), PhysicalValueDouble.TYPE, false)));
}
this.upperSpeedLimitMsg = addPort(PortInformation.newOptionalInputDataPort(SpeedLimitService.UPPER_SPEED_LIMIT_MSG, SpeedLimitService.SPEED_LIMIT_TYPE, false));
}
@Override
......@@ -100,6 +123,17 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
trajY = (double[]) msg.message;
trajLength = newTrajLength;
trajX = newTrajX;
} else if (msg.isMsg(batteryMsg)) {
batteryLevel = (double) msg.message;
} else if (msg.isMsg(upperSpeedLimitMsg)) {
upperSpeedLimitArr = (double[]) msg.message;
}
else {
for (int i=0; i<sensorMsg.size(); i++){
if(msg.isMsg(sensorMsg.get(i))){
this.lidarSensors.set(i, (double) msg.message);
}
}
}
}
......@@ -396,6 +430,16 @@ public class JavaAutopilot extends EEComponent implements Inspectable {
addEntry(entries, true, ports.elementAt(6), currentSteering);
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), lidarSensors.get(0)); // frontRightSensor
addEntry(entries, false, ports.elementAt(11), lidarSensors.get(1)); // frontLeftSensor
addEntry(entries, false, ports.elementAt(12), lidarSensors.get(2)); // rightFrontSensor
addEntry(entries, false, ports.elementAt(13), lidarSensors.get(3)); // rightBackSensor
addEntry(entries, false, ports.elementAt(14), lidarSensors.get(4)); // leftFrontSensor
addEntry(entries, false, ports.elementAt(15), lidarSensors.get(5)); // leftBackSensor
addEntry(entries, false, ports.elementAt(16), lidarSensors.get(6)); // backRightSensor
addEntry(entries, false, ports.elementAt(17), lidarSensors.get(7)); // backLeftSensor
addEntry(entries, false, ports.elementAt(18), upperSpeedLimitArr);
return entries;
}
......
......@@ -5,7 +5,9 @@ import de.rwth.montisim.simulation.eecomponents.autopilots.JavaAutopilotProperti
import de.rwth.montisim.simulation.eesimulator.bus.constant.ConstantBusProperties;
import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
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.SpeedLimitServiceProperties;
import de.rwth.montisim.simulation.vehicle.physicalvalues.*;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
......@@ -53,11 +55,28 @@ public class DefaultVehicleConfig {
.connectTo("DefaultBus")
);
properties.addComponent(
new SensorProperties()
.setPhysicalValueName(BatteryLevel.VALUE_NAME)
.setName("BatterySensor")
.connectTo("DefaultBus")
);
properties.addComponent(
new NavigationProperties()
.connectTo("DefaultBus")
);
properties.addComponent(
new LidarProperties()
.connectTo("DefaultBus")
);
properties.addComponent(
new SpeedLimitServiceProperties()
.connectTo("DefaultBus")
);
properties.addComponent(
ConstantBusProperties.instantBus().setName("DefaultBus")
);
......
......@@ -25,7 +25,7 @@
<properties>
<!-- USE THIS VARIABLE TO SET THE VERSION OF THIS PROJECT AND ALL SUB PROJECTS -->
<revision>3.1.6</revision>
<revision>3.1.7</revision>
<!-- USE THESE TO SET THE VERSIONS OF THE DEPENDENCIES FOR ALL THE SUB-PROJECTS -->
<commons.version>2.0.15</commons.version>
......
......@@ -15,7 +15,9 @@ import de.rwth.montisim.simulation.eesimulator.events.MessageReceiveEvent.Messag
import de.rwth.montisim.simulation.eesimulator.events.MessageSendEvent.MessageSendEventData;
import de.rwth.montisim.simulation.eesimulator.sensor.SensorProperties;
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.SpeedLimitServiceProperties;
import de.rwth.montisim.simulation.vehicle.physicsmodel.rigidbody.RigidbodyPhysicsProperties;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPTProperties;
import de.rwth.montisim.simulation.vehicle.task.metric.MetricGoalProperties;
......@@ -37,6 +39,8 @@ public class TypedSimulation {
Json.registerType(SensorProperties.class);
Json.registerType(TestCompProperties.class);
Json.registerType(NavigationProperties.class);
Json.registerType(LidarProperties.class);
Json.registerType(SpeedLimitServiceProperties.class);
Json.registerType(RigidbodyPhysicsProperties.class);
Json.registerType(ElectricalPTProperties.class);
Json.registerType(MetricGoalProperties.class);
......
......@@ -54,6 +54,12 @@
</dependency> -->
<!-- Testing -->
<dependency>
<groupId>org.mockito</groupId>
<artifactId>mockito-core</artifactId>
<version>3.7.0</version>
<scope>test</scope>
</dependency>
<!-- <dependency>
<groupId>org.powermock</groupId>
<artifactId>powermock-module-junit4</artifactId>
......
......@@ -11,6 +11,7 @@ import de.rwth.montisim.simulation.eesimulator.EESystem;
import de.rwth.montisim.simulation.vehicle.physicalvalues.*;
import de.rwth.montisim.simulation.vehicle.physicsmodel.PhysicsModel;
import de.rwth.montisim.simulation.vehicle.powertrain.PowerTrain;
import de.rwth.montisim.simulation.vehicle.powertrain.electrical.ElectricalPowerTrain;
import de.rwth.montisim.simulation.vehicle.task.Task;
public class Vehicle extends SimulationObject implements Updatable, Destroyable, TaskRunner, BuildObject {
......@@ -64,6 +65,9 @@ public class Vehicle extends SimulationObject implements Updatable, Destroyable,
}
protected void addPhysicalValues() {
if ( powerTrain instanceof ElectricalPowerTrain ) {
physicalValues.addPhysicalValue(new BatteryLevel( ((ElectricalPowerTrain) powerTrain).battery));
}
physicalValues.addPhysicalValue(new TrueCompass(physicalObject));
physicalValues.addPhysicalValue(new TrueVelocity(physicalObject));
physicalValues.addPhysicalValue(new TruePosition(physicalObject));
......
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.Vehicle;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueCompass;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TruePosition;
import java.time.Instant;
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;
private final Vector<Edge> edges = new Vector<>();
private transient int truePositionMsg;
private transient int trueCompassMsg;
private transient List<Integer> lidarMsg = Arrays.asList(0,0,0,0,0,0,0,0);
private double angleDeg;
public static final List<String> LIDAR_MSG = Arrays.asList("front_right_lidar", "front_left_lidar",
"right_front_lidar", "right_back_lidar", "left_front_lidar", "left_back_lidar",
"back_right_lidar", "back_left_lidar");
private Vec2 truePosition = new Vec2(0,0);
private Vec2 orientation = new Vec2(0,0);
public static final double DOUBLE_TOLERANCE = 0.000001d;
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++) {
//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));
for (int i=0; i<lidarMsg.size(); i++ ){
this.lidarMsg.set(i, addPort(PortInformation.newOptionalOutputDataPort(LIDAR_MSG.get(i), PhysicalValueDouble.TYPE)));
}
}
@Override
protected void receive(MessageReceiveEvent msgRecvEvent) {
Message msg = msgRecvEvent.getMessage();
if (msg.isMsg(truePositionMsg)){
Instant time = msgRecvEvent.getEventTime();
truePosition = (Vec2) msg.message;
compute(time);
} else if (msg.isMsg(trueCompassMsg)){
this.angleDeg = (double) msg.message;
double angleRad = angleDeg * Geometry.DEG_TO_RAD;
orientation = new Vec2(Math.cos(angleRad), Math.sin(angleRad));
}
}
/**
* 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 = 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));
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));
// The lidars in the lists always follow the following order: front right, front left,
// right front, right back, left front, left back, back right, back left
List<Vec2> positionOffset = Arrays.asList(new Vec2(length/2,width/4), new Vec2(length/2,width/-4),
new Vec2(length/4,width/2), new Vec2(length/-4,width/2),
new Vec2(length/4,width/-2), new Vec2(length/-4,width/-2),
new Vec2(length/-2,width/4), new Vec2(length/-2,width/-4));
List<Vec2> orientationOffset = Arrays.asList(orientation, orientation,
rightOrientation, rightOrientation,
leftOrientation, leftOrientation,
backOrientation, backOrientation);
for (int i=0; i<lidarMsg.size(); i++){
double distance = computeShortestDistance(truePosition.add(positionOffset.get(i)), orientationOffset.get(i));
sendMessage(time, lidarMsg.get(i), distance);
}
}
/**
* 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;
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;
}
/**
* 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;
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;
import de.rwth.montisim.simulation.vehicle.Vehicle;
@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), context.getObject( Vehicle.CONTEXT_KEY ));
}
}
package de.rwth.montisim.simulation.vehicle.navigation;
import de.rwth.montisim.commons.dynamicinterface.BasicType;
import de.rwth.montisim.commons.dynamicinterface.PortInformation;
import de.rwth.montisim.commons.dynamicinterface.VectorType;
import de.rwth.montisim.commons.utils.UMath;
import de.rwth.montisim.commons.utils.Vec3;
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.Node;
import de.rwth.montisim.simulation.environment.world.elements.Way;
import de.rwth.montisim.simulation.vehicle.lidar.Lidar;
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 {
/**
* The simulated world. Used for fetching speed limits
*/
private final World world;
private double[] trajectoryX = null;
private double[] trajectoryY = null;
private int trajectoryLength = 0;
//input ports
transient int trajectoryXMsg,
trajectoryYMsg,
trajectoryLengthMsg;
//output ports
transient int upperSpeedLimitMsg;
public static final String UPPER_SPEED_LIMIT_MSG = "upper_speed_limit";
public static final VectorType SPEED_LIMIT_TYPE = new VectorType(BasicType.DOUBLE, Navigation.TRAJ_ARRAY_LENGTH - 1);
public SpeedLimitService(SpeedLimitServiceProperties properties, EESystem eeSystem, World world) {
super(properties, eeSystem);
this.world = world;
this.trajectoryLengthMsg = addPort(PortInformation.newRequiredInputDataPort(Navigation.TRAJECTORY_LENGTH_MSG, BasicType.N, false));
this.trajectoryXMsg = addPort(PortInformation.newRequiredInputDataPort(Navigation.TRAJECTORY_X_MSG, Navigation.TRAJECTORY_X_TYPE, false));
this.trajectoryYMsg = addPort(PortInformation.newRequiredInputDataPort(Navigation.TRAJECTORY_Y_MSG, Navigation.TRAJECTORY_Y_TYPE, false));
this.upperSpeedLimitMsg = addPort(PortInformation.newRequiredOutputDataPort(UPPER_SPEED_LIMIT_MSG, SPEED_LIMIT_TYPE));
}
@Override
protected void receive(MessageReceiveEvent msgRecvEvent) {
Message msg = msgRecvEvent.getMessage();
Instant time = msgRecvEvent.getEventTime();
if (msg.isMsg(trajectoryYMsg)) {
trajectoryY = (double[]) msg.message;
compute(time);
} else if(msg.isMsg(trajectoryXMsg)) {
trajectoryX = (double[]) msg.message;
} else if(msg.isMsg(trajectoryLengthMsg)) {
trajectoryLength = (int) msg.message;
}
}
/**
* 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) {
double[] maxSpeedArr = fetchUpperSpeedLimits();
sendMessage(time, upperSpeedLimitMsg, maxSpeedArr, 8*(trajectoryLength - 1));
}
/**
* 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));
for(int i = 1; i < trajectoryLength; i++) {
Vec3 nextTrajPoint = new Vec3(trajectoryX[i], trajectoryY[i], 0);
if (previousPointInfo.ways.size() == 1){
//previous trajectory point is a point but not an intersection
maxSpeedArr[i-1] = previousPointInfo.ways.get(0).maxSpeed;
previousPointInfo = getPointInformation(previousPointInfo.ways, nextTrajPoint);
} else if (previousPointInfo.ways.size() == 0) {
//previous trajectory point cannot be found on any way
previousPointInfo = getPointInformation(world.ways, nextTrajPoint);
} 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<>();
tmpWays.add(mutualWay.get());
maxSpeedArr[i-1] = mutualWay.get().maxSpeed;
previousPointInfo = getPointInformation(tmpWays,nextTrajPoint);
} else {
previousPointInfo = getPointInformation(world.ways, nextTrajPoint);
}
}
}
}
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++) {
Vec3 currentPoint = way.points.get(i);
Vec3 nextPoint = way.points.get(i+1);
if (currentPoint.equals(pointAInfo.coordinates) && nextPoint.equals(pointB)
|| currentPoint.equals(pointB) && nextPoint.equals(pointAInfo.coordinates)) {
return Optional.of(way);
}