diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java index af9248f4e..c431b037a 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java @@ -336,11 +336,13 @@ private void process(final VehicleTypesInitialization vehicleTypesInitialization } private void process(final VehicleRoutesInitialization vehicleRoutesInitialization) { - SimulationKernel.SimulationKernel.getRoutes().putAll(vehicleRoutesInitialization.getRoutes()); + for (var routeEntry : vehicleRoutesInitialization.getRoutes().entrySet()) { + SimulationKernel.SimulationKernel.registerRoute(routeEntry.getKey(), routeEntry.getValue()); + } } private void process(final VehicleRouteRegistration vehicleRouteRegistration) { - SimulationKernel.SimulationKernel.getRoutes().put(vehicleRouteRegistration.getRoute().getId(), vehicleRouteRegistration.getRoute()); + SimulationKernel.SimulationKernel.registerRoute(vehicleRouteRegistration.getRoute().getId(), vehicleRouteRegistration.getRoute()); } private void process(final RsuRegistration rsuRegistration) { diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/SimulationKernel.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/SimulationKernel.java index b6af7aed0..b74ae0394 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/SimulationKernel.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/SimulationKernel.java @@ -36,6 +36,7 @@ import ch.qos.logback.core.Appender; import ch.qos.logback.core.FileAppender; import edu.umd.cs.findbugs.annotations.SuppressWarnings; +import org.apache.commons.lang3.Validate; import org.slf4j.LoggerFactory; import java.io.File; @@ -270,17 +271,17 @@ public CentralPerceptionComponent getCentralPerceptionComponent() { */ @Nonnull public Map getRoutes() { - return routes; + return Collections.unmodifiableMap(routes); } /** - * Returns a view for the {@link #routes}. + * Registers a new route to the simulation kernel. * - * @return a view for the {@link #routes}. + * @param id the id of the route + * @param route the {@link VehicleRoute} to register */ - @Nonnull - public Map getRoutesView() { - return routesView; + public void registerRoute(String id, VehicleRoute route) { + routes.put(id, Validate.notNull(route, "The given route must not be null.")); } /** diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponent.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponent.java index 2bb20afcc..bd3779b9d 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponent.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponent.java @@ -52,8 +52,6 @@ import java.util.ArrayList; import java.util.Collection; -import java.util.Collections; -import java.util.HashMap; import java.util.List; import java.util.Map; import java.util.function.Predicate; @@ -85,11 +83,6 @@ public class CentralNavigationComponent { */ private Routing routing; - /** - * Map storing all known route IDs with the belonging route. - */ - private Map routeMap = new HashMap<>(); - /** * The configuration for routingAPI. */ @@ -134,7 +127,10 @@ public void initialize(RtiAmbassador rtiAmbassador) throws InternalFederateExcep this.log.info("CNC - Navigation-System initialized"); try { - routeMap = routing.getRoutesFromDatabaseForMessage(); + final Map routeMap = routing.getRoutesFromDatabaseForMessage(); + for (var routeEntry: routeMap.entrySet()) { + SimulationKernel.SimulationKernel.registerRoute(routeEntry.getKey(), routeEntry.getValue()); + } // generate VehicleRoutesInitialization to inform other simulators VehicleRoutesInitialization interaction = new VehicleRoutesInitialization(0, routeMap); @@ -151,6 +147,15 @@ public void initialize(RtiAmbassador rtiAmbassador) throws InternalFederateExcep } } + /** + * Returns an unmodifiable view of all routes known to the {@link SimulationKernel}. + * + * @return unmodifiable view of all routes known to the {@link SimulationKernel} + */ + public Map getAllRoutes() { + return SimulationKernel.SimulationKernel.getRoutes(); + } + Routing createFromType(String type) throws InternalFederateException { if (type == null || "database".equalsIgnoreCase(type) || "graphhopper".equalsIgnoreCase(type)) { return new DatabaseRouting(); @@ -202,8 +207,8 @@ public VehicleRoute switchRoute(VehicleData vehicleData, CandidateRoute rawRoute // — first check if already a route exists // — generate a complete route with an ID and propagate it VehicleRoute knownRoute = null; - for (VehicleRoute route : routeMap.values()) { - newRouteOnOriginalRoute = isNewRouteOnOriginalRoute(rawRoute.getConnectionIds(), routeMap.get(route.getId()).getConnectionIds()); + for (VehicleRoute route : getAllRoutes().values()) { + newRouteOnOriginalRoute = isNewRouteOnOriginalRoute(rawRoute.getConnectionIds(), getAllRoutes().get(route.getId()).getConnectionIds()); if (newRouteOnOriginalRoute) { knownRoute = route; break; @@ -261,9 +266,9 @@ private VehicleRoute requestStaticRouteChange(VehicleData vehicleData, VehicleRo * @return {@link GeoPoint}-target of the given route, {@code null} if route doesn't exist. */ GeoPoint getTargetPositionOfRoute(String routeId) { - if (routeMap.containsKey(routeId)) { - String lastNodeId = Iterables.getLast(routeMap.get(routeId).getNodeIds(), null); - return getPositionOfNode(lastNodeId); + if (getAllRoutes().containsKey(routeId)) { + String lastConnectionId = Iterables.getLast(getAllRoutes().get(routeId).getConnectionIds(), null); + return routing.getConnection(lastConnectionId).getEndNode().getPosition(); } else { return null; } @@ -276,9 +281,9 @@ GeoPoint getTargetPositionOfRoute(String routeId) { * @return {@link GeoPoint}-target of the given route, {@code null} if route doesn't exist. */ public GeoPoint getSourcePositionOfRoute(String routeId) { - if (routeMap.containsKey(routeId)) { - String firstNodeId = Iterables.getFirst(routeMap.get(routeId).getNodeIds(), null); - return getPositionOfNode(firstNodeId); + if (getAllRoutes().containsKey(routeId)) { + String firstConnectionId = Iterables.getFirst(getAllRoutes().get(routeId).getConnectionIds(), null); + return routing.getConnection(firstConnectionId).getStartNode().getPosition(); } else { return null; } @@ -321,7 +326,7 @@ private GeoPoint getPositionOfNode(String nodeId) { * @throws InternalFederateException If the {@link Interaction} could not be send. */ private void propagateRoute(VehicleRoute newRoute, long time) throws InternalFederateException { - if (routeMap.containsKey(newRoute.getId())) { + if (getAllRoutes().containsKey(newRoute.getId())) { throw new InternalFederateException( String.format("Route %s is already known but is tried to be propagated, which is not allowed.", newRoute.getId()) ); @@ -331,7 +336,7 @@ private void propagateRoute(VehicleRoute newRoute, long time) throws InternalFed try { this.rtiAmbassador.triggerInteraction(interaction); // store route in local map - routeMap.put(newRoute.getId(), newRoute); + SimulationKernel.SimulationKernel.registerRoute(newRoute.getId(), newRoute); } catch (IllegalValueException e) { throw new InternalFederateException(e); } @@ -385,7 +390,7 @@ public VehicleDeparture createRouteForOdInfo(long time, OriginDestinationPair od // check if best route, matches one of the existing routes and if so choose that existing route if (response.getBestRoute() != null) { VehicleRoute route = null; - for (VehicleRoute existingRoute : routeMap.values()) { + for (VehicleRoute existingRoute : getAllRoutes().values()) { if (isNewRouteOnOriginalRoute(response.getBestRoute().getConnectionIds(), existingRoute.getConnectionIds())) { route = existingRoute; break; @@ -456,7 +461,7 @@ public IRoadPosition refineRoadPosition(IRoadPosition roadPosition) { if (finalNode == null) { throw new IllegalArgumentException("finalNode is null."); } - List currentRouteNodes = routeMap.get(routeId).getNodeIds(); + List currentRouteNodes = getAllRoutes().get(routeId).getNodeIds(); if (!currentRouteNodes.contains(finalNode)) { return Double.POSITIVE_INFINITY; @@ -485,7 +490,7 @@ public IRoadPosition refineRoadPosition(IRoadPosition roadPosition) { * @return A node if a valid one is found, otherwise {@code null}. */ INode getNextNodeOnRoute(String routeId, IRoadPosition roadPosition, Predicate nodeCondition) { - VehicleRoute currentRoute = routeMap.get(routeId); + VehicleRoute currentRoute = getAllRoutes().get(routeId); int indexOfUpcomingNode = currentRoute.getNodeIds().indexOf(roadPosition.getUpcomingNode().getId()); // check if there is an upcoming node @@ -504,15 +509,6 @@ INode getNextNodeOnRoute(String routeId, IRoadPosition roadPosition, Predicate getRouteMap() { - return Collections.unmodifiableMap(routeMap); - } - /** * Approximates the costs for all {@link CandidateRoute}s passed to the function. * diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java index 65514f9d4..17b4c8449 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java @@ -16,6 +16,7 @@ package org.eclipse.mosaic.fed.application.ambassador.navigation; import org.eclipse.mosaic.lib.geo.GeoPoint; +import org.eclipse.mosaic.lib.objects.road.IConnection; import org.eclipse.mosaic.lib.objects.road.INode; import org.eclipse.mosaic.lib.objects.road.IRoadPosition; import org.eclipse.mosaic.lib.objects.vehicle.VehicleData; @@ -140,6 +141,14 @@ public interface INavigationModule { */ INode getNode(String node); + /** + * Returns data for the specified connection id. + * + * @param connection the id of the node + * @return the {@link IConnection} containing data for the specified connection id. + */ + IConnection getConnection(String connection); + /** * Returns the node which is closest to specified {@link GeoPoint}. * diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java index 140d9ccfa..92eceb808 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java @@ -16,6 +16,7 @@ package org.eclipse.mosaic.fed.application.ambassador.navigation; import org.eclipse.mosaic.lib.geo.GeoPoint; +import org.eclipse.mosaic.lib.objects.road.IConnection; import org.eclipse.mosaic.lib.objects.road.INode; import org.eclipse.mosaic.lib.objects.road.IRoadPosition; import org.eclipse.mosaic.lib.routing.RoutingParameters; @@ -45,6 +46,14 @@ public interface IRoutingModule { */ INode getNode(String nodeId); + /** + * Returns data for the specified connection id. + * + * @param connection the id of the node + * @return the {@link IConnection} containing data for the specified connection id. + */ + IConnection getConnection(String connection); + /** * Returns the node object, which is closest to the given {@link GeoPoint}. * diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java index 0855f5e1a..c66fc0509 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java @@ -18,6 +18,7 @@ import org.eclipse.mosaic.fed.application.ambassador.SimulationKernel; import org.eclipse.mosaic.fed.application.ambassador.simulation.AbstractSimulationUnit; import org.eclipse.mosaic.lib.geo.GeoPoint; +import org.eclipse.mosaic.lib.objects.road.IConnection; import org.eclipse.mosaic.lib.objects.road.INode; import org.eclipse.mosaic.lib.objects.road.IRoadPosition; import org.eclipse.mosaic.lib.objects.vehicle.VehicleData; @@ -122,6 +123,7 @@ public boolean switchRoute(CandidateRoute newRoute) { "NavigationModule#switchRoute: Could not switch to candidate route[{}]", StringUtils.join(newRoute.getConnectionIds(), ",") ); + belongingUnit.getOsLog().error("Reason", e); return false; } } @@ -194,7 +196,7 @@ public Collection retrieveAllValidRoutesToTarget(GeoPoint target private Collection retrieveAllValidExistingRoutesToTargetHelper(RoutingPosition targetPosition) { CentralNavigationComponent centNavComp = SimulationKernel.SimulationKernel.getCentralNavigationComponent(); ArrayList candidateRoutes = new ArrayList<>(); - for (Map.Entry entry : centNavComp.getRouteMap().entrySet()) { + for (Map.Entry entry : centNavComp.getAllRoutes().entrySet()) { VehicleRoute route = entry.getValue(); if (targetQuery(targetPosition, route, centNavComp.getTargetPositionOfRoute(route.getId())) && onRouteQuery(route)) { // length and time are no valid values at this point @@ -270,6 +272,11 @@ public INode getNode(String nodeId) { return SimulationKernel.SimulationKernel.getCentralNavigationComponent().getRouting().getNode(nodeId); } + @Override + public IConnection getConnection(String connectionId) { + return SimulationKernel.SimulationKernel.getCentralNavigationComponent().getRouting().getConnection(connectionId); + } + @Override public INode getClosestNode(GeoPoint geoPoint) { return SimulationKernel.SimulationKernel.getCentralNavigationComponent().getRouting().findClosestNode(geoPoint); diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassadorTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassadorTest.java index f352eea13..56cc5c5c1 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassadorTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassadorTest.java @@ -687,8 +687,8 @@ public void processInteraction_VehicleRoutesInitialization() throws InternalFede ambassador.processInteraction(vehicleRoutesInitialization); // ASSERT: vehicles routes have been propagated - assertEquals(routes.get("0"), SimulationKernel.SimulationKernel.getRoutesView().get("0")); - assertEquals(routes.get("1"), SimulationKernel.SimulationKernel.getRoutesView().get("1")); + assertEquals(routes.get("0"), SimulationKernel.SimulationKernel.getRoutes().get("0")); + assertEquals(routes.get("1"), SimulationKernel.SimulationKernel.getRoutes().get("1")); // finish simulation ambassador.processTimeAdvanceGrant(recentAdvanceTime); diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponentTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponentTest.java index a7854427b..177e0440b 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponentTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponentTest.java @@ -22,9 +22,11 @@ import static org.junit.Assert.assertSame; import static org.junit.Assert.assertTrue; import static org.mockito.ArgumentMatchers.argThat; +import static org.mockito.ArgumentMatchers.eq; import static org.mockito.ArgumentMatchers.isA; import static org.mockito.ArgumentMatchers.isNull; import static org.mockito.ArgumentMatchers.same; +import static org.mockito.Mockito.doReturn; import static org.mockito.Mockito.mock; import static org.mockito.Mockito.never; import static org.mockito.Mockito.times; @@ -35,8 +37,9 @@ import org.eclipse.mosaic.interactions.traffic.VehicleRoutesInitialization; import org.eclipse.mosaic.interactions.vehicle.VehicleRouteChange; import org.eclipse.mosaic.interactions.vehicle.VehicleRouteRegistration; -import org.eclipse.mosaic.lib.database.road.Node; import org.eclipse.mosaic.lib.geo.GeoPoint; +import org.eclipse.mosaic.lib.objects.road.IConnection; +import org.eclipse.mosaic.lib.objects.road.INode; import org.eclipse.mosaic.lib.objects.vehicle.VehicleData; import org.eclipse.mosaic.lib.objects.vehicle.VehicleRoute; import org.eclipse.mosaic.lib.routing.CandidateRoute; @@ -46,7 +49,6 @@ import org.eclipse.mosaic.lib.routing.RoutingPosition; import org.eclipse.mosaic.lib.routing.RoutingRequest; import org.eclipse.mosaic.lib.routing.config.CRouting; -import org.eclipse.mosaic.lib.routing.database.LazyLoadingNode; import org.eclipse.mosaic.lib.routing.norouting.NoRouting; import org.eclipse.mosaic.rti.TIME; import org.eclipse.mosaic.rti.api.IllegalValueException; @@ -59,7 +61,6 @@ import org.junit.Test; import org.junit.rules.RuleChain; import org.junit.rules.TemporaryFolder; -import org.mockito.Mockito; import java.io.File; import java.io.IOException; @@ -132,11 +133,10 @@ public void initialize_vehicleRoutesInitializationSent() throws InternalFederate public void getTargetPositionOfRoute() throws InternalFederateException { //PREPARE cnc.initialize(rtiAmbassadorMock); - when(routingMock.getNode(Mockito.eq("1"))).thenReturn(new LazyLoadingNode(new Node("1", GeoPoint.lonLat(1, 1)))); - when(routingMock.getNode(Mockito.eq("2"))).thenReturn(new LazyLoadingNode(new Node("2", GeoPoint.lonLat(2, 2)))); - when(routingMock.getNode(Mockito.eq("3"))).thenReturn(new LazyLoadingNode(new Node("3", GeoPoint.lonLat(3, 3)))); - when(routingMock.getNode(Mockito.eq("4"))).thenReturn(new LazyLoadingNode(new Node("4", GeoPoint.lonLat(4, 4)))); - + doReturn(createConnectionMock(GeoPoint.lonLat(1, 1))).when(routingMock).getConnection(eq("1_1_2_1")); + doReturn(createConnectionMock(GeoPoint.lonLat(2, 2))).when(routingMock).getConnection(eq("3_2_5_2")); + doReturn(createConnectionMock(GeoPoint.lonLat(3, 3))).when(routingMock).getConnection(eq("2_2_4_2")); + doReturn(createConnectionMock(GeoPoint.lonLat(4, 4))).when(routingMock).getConnection(eq("2_2_4_3")); // RUN + ASSERT (Last node of route "0" = 4) final GeoPoint gpEndOf0 = cnc.getTargetPositionOfRoute("0"); @@ -155,6 +155,13 @@ public void getTargetPositionOfRoute() throws InternalFederateException { assertNull(gpEndOf2); } + private IConnection createConnectionMock(GeoPoint endPoint) { + IConnection connection = mock(IConnection.class); + when(connection.getEndNode()).thenReturn(mock(INode.class)); + when(connection.getEndNode().getPosition()).thenReturn(endPoint); + return connection; + } + @Test public void findRoutes_requestRoutesOnly_noRouteChangeInitiated() throws InternalFederateException, IllegalValueException { // PREPARE diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModuleTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModuleTest.java index 68f767a38..7d4cae745 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModuleTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModuleTest.java @@ -109,7 +109,7 @@ public void setup() { }).when(cncMock).findRoutes(isA(RoutingRequest.class)); HashMap routeMap = new HashMap<>(); routeMap.put("123", new VehicleRoute("123", Collections.singletonList("edgeID"), Collections.singletonList("nodeID"), 0.0)); - when(cncMock.getRouteMap()).thenReturn(routeMap); + when(cncMock.getAllRoutes()).thenReturn(routeMap); when(cncMock.getTargetPositionOfRoute(ArgumentMatchers.anyString())).thenReturn(GeoPoint.latLon(30, 40)); } diff --git a/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/SumoAmbassador.java b/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/SumoAmbassador.java index de495390b..9bd8d7fc8 100644 --- a/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/SumoAmbassador.java +++ b/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/SumoAmbassador.java @@ -301,11 +301,13 @@ private void addNotYetAddedVehicles(long time) throws InternalFederateException bridge.getSimulationControl().addVehicle(vehicleId, routeId, vehicleType, laneId, departPos, departSpeed); - applyChangesInVehicleTypeForVehicle( - vehicleId, - vehicleRegistration.getMapping().getVehicleType(), - cachedVehicleTypesInitialization.getTypes().get(vehicleType) - ); + final VehicleType cachedType = cachedVehicleTypesInitialization.getTypes().get(vehicleType); + if (cachedType != null) { + applyChangesInVehicleTypeForVehicle(vehicleId, vehicleRegistration.getMapping().getVehicleType(), cachedType); + } else { + log.warn("Unknown vehicle type {}. Ensure that a suitable vType is configured in the SUMO configuration.", vehicleType); + } + if (externalVehicleState != null) { externalVehicleState.setAdded(true); } @@ -359,7 +361,6 @@ private void subscribeToNotYetSubscribedVehicles(long time) throws InternalFeder iterator.remove(); } } - } private void applyChangesInVehicleTypeForVehicle(String vehicleId, VehicleType actualVehicleType, VehicleType baseVehicleType) throws InternalFederateException { diff --git a/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/math/Aggregator.java b/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/math/Aggregator.java index 8f548e013..51d24acf9 100644 --- a/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/math/Aggregator.java +++ b/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/math/Aggregator.java @@ -27,6 +27,10 @@ public Aggregator() { } public Aggregator add(double value) { + if (Double.isNaN(value)) { + return this; + } + min = Math.min(min, value); max = Math.max(max, value); sum += value; diff --git a/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/vehicle/VehicleDeparture.java b/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/vehicle/VehicleDeparture.java index 50eab581a..4f5d30447 100644 --- a/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/vehicle/VehicleDeparture.java +++ b/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/vehicle/VehicleDeparture.java @@ -24,7 +24,7 @@ public class VehicleDeparture implements Serializable { private static final long serialVersionUID = 1L; public enum DepartureSpeedMode { - /* the vehicle departs with the speed given in the mapping definiton of the spawner */ + /* the vehicle departs with the speed given in the mapping definition of the spawner */ PRECISE, /* the vehicle departs with a random speed */ RANDOM, @@ -139,8 +139,8 @@ public static class Builder { private int departureConnectionIndex; private double departurePos; private double departureSpeed; - private DepartureSpeedMode departureSpeedMode; - private LaneSelectionMode laneSelectionMode; + private DepartureSpeedMode departureSpeedMode = DepartureSpeedMode.MAXIMUM; + private LaneSelectionMode laneSelectionMode = LaneSelectionMode.DEFAULT; public Builder(String routeId) { this.routeId = routeId; diff --git a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java index f257cc977..e9d91d634 100644 --- a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java +++ b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java @@ -118,10 +118,10 @@ public INode getNode(String nodeId) { } @Override - public IConnection getConnection(String nodeId) { - Connection c = scenarioDatabase.getConnection(nodeId); + public IConnection getConnection(String connectionId) { + Connection c = scenarioDatabase.getConnection(connectionId); if (c == null) { - throw new IllegalArgumentException(String.format("No such (%s) connetion existing.", nodeId)); + throw new IllegalArgumentException(String.format("No such (%s) connection existing.", connectionId)); } return new LazyLoadingConnection(c); } diff --git a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/graphhopper/GraphHopperRouting.java b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/graphhopper/GraphHopperRouting.java index c13ff0c60..be567a702 100644 --- a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/graphhopper/GraphHopperRouting.java +++ b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/graphhopper/GraphHopperRouting.java @@ -15,11 +15,15 @@ package org.eclipse.mosaic.lib.routing.graphhopper; +import static java.util.Objects.requireNonNull; + import org.eclipse.mosaic.lib.database.Database; import org.eclipse.mosaic.lib.database.DatabaseUtils; import org.eclipse.mosaic.lib.database.road.Connection; import org.eclipse.mosaic.lib.database.road.Node; import org.eclipse.mosaic.lib.enums.VehicleClass; +import org.eclipse.mosaic.lib.geo.GeoPoint; +import org.eclipse.mosaic.lib.geo.GeoUtils; import org.eclipse.mosaic.lib.routing.CandidateRoute; import org.eclipse.mosaic.lib.routing.RoutingCostFunction; import org.eclipse.mosaic.lib.routing.RoutingPosition; @@ -167,11 +171,11 @@ public List findRoutes(RoutingRequest routingRequest) { // convert paths to routes for (final Path path : paths) { - final CandidateRoute route = convertPath(path, queryGraph, querySource, queryTarget, target); + final CandidateRoute route = convertPath(path, queryGraph, querySource, queryTarget, source, target); if (route != null && !route.getConnectionIds().isEmpty() - && checkForDuplicate(route, duplicateSet) - && checkRouteOnRequiredSourceConnection(route, source)) { + && checkRouteOnRequiredSourceConnection(route, source) + && checkForDuplicate(route, duplicateSet)) { result.add(route); } else if (route != null && log.isDebugEnabled()) { log.debug("Path is invalid and will be ignored [" + StringUtils.join(route.getConnectionIds(), ",") + "]"); @@ -225,19 +229,24 @@ private QueryResult fixQueryResultIfNoClosestEdgeFound(QueryResult queryResult, } private QueryResult fixQueryResultIfSnappedPointIsTowerNode(QueryResult queryResult, RoutingPosition routingPosition, EdgeFilter fromEdgeFilter) { + if (queryResult.getSnappedPosition() != QueryResult.Position.TOWER) { + return queryResult; + } /* If the requested position is in front or behind the edge it is mapped either on the start or end of the edge (one of the tower nodes). * As a result, the resulting route can bypass turn restrictions in very rare cases. To avoid this, we choose an alternative - * node based on the queried connection.*/ - if (queryResult.getSnappedPosition() == QueryResult.Position.TOWER) { - // use the node before target node (index -2) as the alternative query node to find a QueryResult _on_ the connection. - Node alternativeQueryNode = DatabaseUtils.getNodeByIndex(db.getConnection(routingPosition.getConnectionId()), -2); - if (alternativeQueryNode != null) { - return ghApi.getLocationIndex().findClosest( - alternativeQueryNode.getPosition().getLatitude(), alternativeQueryNode.getPosition().getLongitude(), fromEdgeFilter - ); - } + * position which is located somewhere _on_ the queried connection.*/ + final Connection queryConnection = db.getConnection(routingPosition.getConnectionId()); + final GeoPoint alternativeQueryPosition; + if (queryConnection.getNodes().size() > 2) { + alternativeQueryPosition = requireNonNull(DatabaseUtils.getNodeByIndex(queryConnection, -2)).getPosition(); + } else { + alternativeQueryPosition = GeoUtils.getPointBetween( + queryConnection.getFrom().getPosition(), queryConnection.getTo().getPosition() + ); } - return queryResult; + return ghApi.getLocationIndex().findClosest( + alternativeQueryPosition.getLatitude(), alternativeQueryPosition.getLongitude(), fromEdgeFilter + ); } /** @@ -263,8 +272,11 @@ private EdgeFilter createEdgeFilterForRoutingPosition(final RoutingPosition posi return edgeState -> edgeState.getEdge() == forcedEdge; } - private CandidateRoute convertPath(Path newPath, QueryGraph queryGraph, QueryResult source, QueryResult target, RoutingPosition targetPosition) { + private CandidateRoute convertPath(Path newPath, QueryGraph queryGraph, QueryResult source, QueryResult target, RoutingPosition sourcePosition, RoutingPosition targetPosition) { PointList pointList = newPath.calcPoints(); + if (pointList.isEmpty()) { + return null; + } GHPoint pathTarget = Iterables.getLast(pointList); GHPoint origTarget = new GHPoint(targetPosition.getPosition().getLatitude(), targetPosition.getPosition().getLongitude()); double distanceToOriginalTarget = distanceCalculation.calcDist(pathTarget.lat, pathTarget.lon, origTarget.lat, origTarget.lon); @@ -323,10 +335,28 @@ private CandidateRoute convertPath(Path newPath, QueryGraph queryGraph, QueryRes log.debug(String.format("A connection could be resolved by internal ID %d.", ghEdge.getEdge())); } } - + fixFirstConnectionOfPathIfNotAsQueried(sourcePosition, pathConnections); return new CandidateRoute(pathConnections, newPath.getDistance(), newPath.getTime() / (double) 1000); } + /** + * In some very rare cases, if a source connection is given, the path returned by GraphHopper omits this first connection + * and continues on the subsequent one. As a workaround, this code checks if the outgoing connections of the queried source connection + * contains the first connection of the calculated path, and then adds the source connection to the beginning of the new path. + */ + private void fixFirstConnectionOfPathIfNotAsQueried(RoutingPosition sourcePosition, List pathConnections) { + String firstConnectionId = Iterables.getFirst(pathConnections, null); + if (sourcePosition.getConnectionId() != null && firstConnectionId != null + && !sourcePosition.getConnectionId().equals(firstConnectionId) + ) { + Connection sourceConnection = db.getConnection(sourcePosition.getConnectionId()); + Connection firstConnection = db.getConnection(firstConnectionId); + if (sourceConnection.getOutgoingConnections().contains(firstConnection)) { + pathConnections.add(0, sourceConnection.getId()); + } + } + } + private boolean checkRouteOnRequiredSourceConnection(CandidateRoute route, RoutingPosition source) { if (source.getConnectionId() != null) { return source.getConnectionId().equals(route.getConnectionIds().get(0)); diff --git a/lib/mosaic-utils/src/main/java/org/eclipse/mosaic/lib/util/scheduling/Event.java b/lib/mosaic-utils/src/main/java/org/eclipse/mosaic/lib/util/scheduling/Event.java index 793180075..4fe42d869 100644 --- a/lib/mosaic-utils/src/main/java/org/eclipse/mosaic/lib/util/scheduling/Event.java +++ b/lib/mosaic-utils/src/main/java/org/eclipse/mosaic/lib/util/scheduling/Event.java @@ -23,7 +23,6 @@ import org.apache.commons.lang3.builder.EqualsBuilder; import org.apache.commons.lang3.builder.HashCodeBuilder; -import java.util.ArrayList; import java.util.Collections; import java.util.List; import java.util.Objects; @@ -139,12 +138,12 @@ public Event(final long time, @Nonnull final EventProcessor processor, this.time = time; - Validate.isTrue(processors.size() > 0, "The processor list must contain at minimum one processor."); + Validate.isTrue(!processors.isEmpty(), "The processor list must contain at minimum one processor."); for (EventProcessor processor : processors) { Objects.requireNonNull(processor, "All event processors must not be null."); } - this.processors = Collections.unmodifiableList(new ArrayList<>(processors)); + this.processors = List.copyOf(processors); this.resource = resource; this.nice = nice; } diff --git a/test/mosaic-integration-tests/src/test/java/org/eclipse/mosaic/test/app/mosaicandsumovehicles/SumoVehicle.java b/test/mosaic-integration-tests/src/test/java/org/eclipse/mosaic/test/app/mosaicandsumovehicles/SumoVehicle.java index ba41b6870..e9ae8f34b 100644 --- a/test/mosaic-integration-tests/src/test/java/org/eclipse/mosaic/test/app/mosaicandsumovehicles/SumoVehicle.java +++ b/test/mosaic-integration-tests/src/test/java/org/eclipse/mosaic/test/app/mosaicandsumovehicles/SumoVehicle.java @@ -48,7 +48,7 @@ public void onStartup() { public void onShutdown() { getLog().infoSimTime(this, "Shutdown: I'm a vehicle defined in SUMO route file."); getLog().infoSimTime(this, "{} routes are known to the SimulationKernel.", - SimulationKernel.SimulationKernel.getRoutesView().size()); + SimulationKernel.SimulationKernel.getRoutes().size()); } @Override