feat: various changes required for ride-hailing (#370)

* feat(routing): provide connection data via navigation module * fix: fix NPE if vehicle type is only known in SUMO config * refactor: better route handling via SimulationKernel * fix: fixed route failures if query location matches to start or end of the queried connection * fix(utils): Aggregator should ignore NaN --------- Signed-off-by: Karl Schrab <[email protected]>
eclipse · Jan 24, 2024 · b17fc24 · b17fc24
1 parent 7430df8
commit b17fc24
Show file tree

Hide file tree

Showing 16 changed files with 146 additions and 81 deletions.
diff --git a/...on/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java b/...on/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/...ication/src/main/java/org/eclipse/mosaic/fed/application/ambassador/SimulationKernel.java b/...ication/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<String, VehicleRoute> 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<String, VehicleRoute> 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/.../org/eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponent.java b/.../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<String, VehicleRoute> 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<String, VehicleRoute> 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<String, VehicleRoute> 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<String> currentRouteNodes = routeMap.get(routeId).getNodeIds();
+        List<String> 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<INode> 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<I
         return null;
     }
 
-    /**
-     * Returns an unmodifiable view of {@link #routeMap}.
-     *
-     * @return unmodifiable view of {@link #routeMap}
-     */
-    Map<String, VehicleRoute> getRouteMap() {
-        return Collections.unmodifiableMap(routeMap);
-    }
-
     /**
      * Approximates the costs for all {@link CandidateRoute}s passed to the function.
      *

diff --git a/...main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java b/...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/...rc/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java b/...rc/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/.../main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java b/.../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<CandidateRoute> retrieveAllValidRoutesToTarget(GeoPoint target
     private Collection<CandidateRoute> retrieveAllValidExistingRoutesToTargetHelper(RoutingPosition targetPosition) {
         CentralNavigationComponent centNavComp = SimulationKernel.SimulationKernel.getCentralNavigationComponent();
         ArrayList<CandidateRoute> candidateRoutes = new ArrayList<>();
-        for (Map.Entry<String, VehicleRoute> entry : centNavComp.getRouteMap().entrySet()) {
+        for (Map.Entry<String, VehicleRoute> 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/...rc/test/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassadorTest.java b/...rc/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/.../eclipse/mosaic/fed/application/ambassador/navigation/CentralNavigationComponentTest.java b/.../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/...t/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModuleTest.java b/...t/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModuleTest.java
@@ -109,7 +109,7 @@ public void setup() {
         }).when(cncMock).findRoutes(isA(RoutingRequest.class));
         HashMap<String, VehicleRoute> 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
@@ -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 {