add tracking for cars when using intermodal access/egress

matsim/src/main/java/ch/sbb/matsim/routing/pt/raptor/DefaultRaptorStopFinder.java

@@ -121,16 +121,20 @@ private List<InitialStop> findIntermodalStops(Facility fromFacility, Facility to
         switch (srrCfg.getIntermodalAccessEgressModeSelection()) {
             case CalcLeastCostModePerStop:
                 for (IntermodalAccessEgressParameterSet parameterSet : srrCfg.getIntermodalAccessEgressParameterSets()) {
-                    addInitialStopsForParamSet(fromFacility, toFacility, person, departureTime, routingAttributes, direction, parameters, data, x, y, initialStops, parameterSet);
+					if (considerParameterSet(srrCfg, parameterSet, direction, person, data)) {
+						addInitialStopsForParamSet(fromFacility, toFacility, person, departureTime, routingAttributes, direction, parameters, data, x, y, initialStops, parameterSet);
+					}
                 }
                 break;
             case RandomSelectOneModePerRoutingRequestAndDirection:
                 int counter = 0;
                 do {
                     int rndSelector = random.nextInt(srrCfg.getIntermodalAccessEgressParameterSets().size());
-                    addInitialStopsForParamSet(fromFacility, toFacility, person, departureTime, routingAttributes, direction, parameters, data, x, y,
-                            initialStops, srrCfg.getIntermodalAccessEgressParameterSets().get(rndSelector));
-                    counter++;
+					IntermodalAccessEgressParameterSet parameterSet = srrCfg.getIntermodalAccessEgressParameterSets().get(rndSelector);
+					if (considerParameterSet(srrCfg, parameterSet, direction, person, data)) {
+						addInitialStopsForParamSet(fromFacility, toFacility, person, departureTime, routingAttributes, direction, parameters, data, x, y, initialStops, parameterSet);
+						counter++;
+					}
                     // try again if no initial stop was found for the parameterset. Avoid infinite loop by limiting number of tries.
                 } while (initialStops.isEmpty() && counter < 2 * srrCfg.getIntermodalAccessEgressParameterSets().size());
                 break;
@@ -140,6 +144,33 @@ private List<InitialStop> findIntermodalStops(Facility fromFacility, Facility to
         return initialStops;
     }
 
+	private boolean vehicleTrackingEnabled(SwissRailRaptorConfigGroup config, IntermodalAccessEgressParameterSet parameterSet) {
+		// todo: check config settings
+		// todo: change default to false after implementing
+		if (parameterSet.getMode().equals("car")) {
+			return true;
+		}
+		return false;
+	}
+
+	private boolean considerParameterSet(SwissRailRaptorConfigGroup config, IntermodalAccessEgressParameterSet parameterSet,
+		Direction direction, Person person, SwissRailRaptorData data) {
+		if (vehicleTrackingEnabled(config, parameterSet)) {
+			String mode = parameterSet.getMode();
+			switch (direction) {
+				case ACCESS:
+					// if vehicle has not been left somewhere and agent is trying to access a stop, assume they have
+					// access to their vehicle
+					return !data.vehicleWasLeftAtStop(person, mode);
+				case EGRESS:
+					// if vehicle has been left parked at a stop and agent is looking to egress a stop, allow them the
+					// opportunity to look for stops using that mode as intermodal egress
+					return data.vehicleWasLeftAtStop(person, mode);
+			}
+		}
+		return true;
+	}
+
     private void addInitialStopsForParamSet(Facility fromFacility, Facility toFacility, Person person, double departureTime, Attributes routingAttributes, Direction direction, RaptorParameters parameters, SwissRailRaptorData data, double x, double y, List<InitialStop> initialStops, IntermodalAccessEgressParameterSet paramset) {
         String mode = paramset.getMode();
         String linkIdAttribute = paramset.getLinkIdAttribute();
@@ -191,7 +222,12 @@ private void addInitialStopsForParamSet(Facility fromFacility, Facility toFacili
                     RoutingModule module = this.routingModules.get(mode);
                     routeParts = module.calcRoute(DefaultRoutingRequest.of(facility, stopFacility, departureTime, person, routingAttributes));
                 } else { // it's Egress
-                    // We don't know the departure time for the egress trip, so just use the original departureTime,
+					// todo: soft-code car
+					if (mode.equals("car") && !data.vehicleWasLeftAtStop(person, mode, stop)) {
+						// the vehicle of tracked mode was not found at this stop, skip this choice
+						continue;
+					}
+					// We don't know the departure time for the egress trip, so just use the original departureTime,
                     // although it is wrong and might result in a wrong traveltime and thus wrong route.
                     RoutingModule module = this.routingModules.get(mode);
                     routeParts = module.calcRoute(DefaultRoutingRequest.of(stopFacility, facility, departureTime, person, routingAttributes));

matsim/src/main/java/ch/sbb/matsim/routing/pt/raptor/RaptorUtils.java

@@ -11,10 +11,7 @@
 import org.matsim.api.core.v01.Id;
 import org.matsim.api.core.v01.TransportMode;
 import org.matsim.api.core.v01.network.Link;
-import org.matsim.api.core.v01.population.Activity;
-import org.matsim.api.core.v01.population.Leg;
-import org.matsim.api.core.v01.population.PlanElement;
-import org.matsim.api.core.v01.population.Route;
+import org.matsim.api.core.v01.population.*;
 import org.matsim.core.config.Config;
 import org.matsim.core.config.ConfigUtils;
 import org.matsim.core.config.groups.PlanCalcScoreConfigGroup;
@@ -159,4 +156,62 @@ public static List<? extends PlanElement> convertRouteToLegs(RaptorRoute route,
 
         return legs;
     }
+
+    public static void recordLegChoices(RaptorRoute route, Person person, SwissRailRaptorData data) {
+        int i = 0;
+        for (RaptorRoute.RoutePart part : route.parts) {
+            if (part.planElements != null) {
+                for (PlanElement pe : part.planElements) {
+                    if (pe instanceof Leg) {
+                        Leg leg = (Leg) pe;
+                        // todo soft-code car
+                        if (leg.getMode().equals("car")) {
+                            RaptorRoute.RoutePart nextRoutePart = routePartAtIndex(route, i + 1);
+                            if (nextRoutePart != null && routePartIsPT(nextRoutePart)) {
+                                // if there is a next route part, and it's PT, that means a tracked mode
+                                // was used to access it, record the vehicle at the stop
+                                data.parkVehicleAtStop(person, leg.getMode(), nextRoutePart.fromStop);
+                            } else if (nextRoutePart != null) {
+                                // there may be a transfer walk connecting that mode and PT
+                                // we skip the transfer walk and look at the next route part
+                                RaptorRoute.RoutePart nextNextRoutePart = routePartAtIndex(route, i + 2);
+                                if (nextNextRoutePart != null && routePartIsPT(nextNextRoutePart)) {
+                                    data.parkVehicleAtStop(person, leg.getMode(), nextNextRoutePart.fromStop);
+                                }
+                            } else {
+                                // a tracked mode was used to egress a PT stop, release the vehicle from that stop
+                                RaptorRoute.RoutePart prevRoutePart = routePartAtIndex(route, i - 1);
+                                if (prevRoutePart != null && routePartIsPT(prevRoutePart)) {
+                                    data.releaseVehicleFromStop(person, leg.getMode());
+                                } else if (prevRoutePart != null) {
+                                    // previous route part must have been a transfer walk, try an earlier route part
+                                    RaptorRoute.RoutePart prevPrevRoutePart = routePartAtIndex(route, i - 2);
+                                    if (prevPrevRoutePart != null && routePartIsPT(prevPrevRoutePart)) {
+                                        data.releaseVehicleFromStop(person, leg.getMode());
+                                    }
+                                } else {
+                                    // the tracked vehicle mode exists in isolation, which I don't think is possible
+                                    // unless you configure walking as a tracked mode
+                                    break;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+            i++;
+        }
+    }
+
+    private static RaptorRoute.RoutePart routePartAtIndex(RaptorRoute route, int i) {
+        if (route.parts.size() > i) {
+            return route.parts.get(i);
+        }
+        return null;
+    }
+
+    private static Boolean routePartIsPT(RaptorRoute.RoutePart routePart) {
+        return routePart.planElements == null && routePart.line != null;
+    }
+
 }

matsim/src/main/java/ch/sbb/matsim/routing/pt/raptor/SwissRailRaptor.java

@@ -119,6 +119,7 @@ public List<? extends PlanElement> calcRoute(RoutingRequest request) {
         }
 
 		List<? extends PlanElement> legs = RaptorUtils.convertRouteToLegs(foundRoute, this.data.config.getTransferWalkMargin());
+        RaptorUtils.recordLegChoices(foundRoute, person, this.data);
         return legs;
     }
 

matsim/src/main/java/ch/sbb/matsim/routing/pt/raptor/SwissRailRaptorData.java

@@ -10,6 +10,7 @@
 import org.matsim.api.core.v01.TransportMode;
 import org.matsim.api.core.v01.network.Link;
 import org.matsim.api.core.v01.network.Network;
+import org.matsim.api.core.v01.population.Person;
 import org.matsim.core.population.routes.NetworkRoute;
 import org.matsim.core.utils.collections.QuadTree;
 import org.matsim.core.utils.geometry.CoordUtils;
@@ -58,6 +59,7 @@ public class SwissRailRaptorData {
     final QuadTree<TransitStopFacility> stopsQT;
     final Map<String, Map<String, QuadTree<TransitStopFacility>>> stopFilterAttribute2Value2StopsQT;
     final OccupancyData occupancyData;
+    Map<Id<Person>, Map<String, Id<TransitStopFacility>>> vehiclesAtStops = new HashMap<>();
 
     private SwissRailRaptorData(RaptorStaticConfig config, int countStops,
                                 RRoute[] routes, int[] departures, Vehicle[] departureVehicles, Id<Departure>[] departureIds, RRouteStop[] routeStops,
@@ -470,6 +472,28 @@ public TransitStopFacility findNearestStop(double x, double y) {
         return this.stopsQT.getClosest(x, y);
     }
 
+    public void parkVehicleAtStop(Person person, String mode, TransitStopFacility stop) {
+        vehiclesAtStops.putIfAbsent(person.getId(), new HashMap<>());
+        vehiclesAtStops.get(person.getId()).put(mode, stop.getId());
+    }
+
+    public void releaseVehicleFromStop(Person person, String mode) {
+        vehiclesAtStops.putIfAbsent(person.getId(), new HashMap<>());
+        vehiclesAtStops.get(person.getId()).remove(mode);
+    }
+
+    public boolean vehicleWasLeftAtStop(Person person, String mode) {
+        return vehiclesAtStops.containsKey(person.getId()) && vehiclesAtStops.get(person.getId()).containsKey(mode);
+    }
+
+    public boolean vehicleWasLeftAtStop(Person person, String mode, TransitStopFacility stop) {
+        Map<String, Id<TransitStopFacility>> personTracker = vehiclesAtStops.getOrDefault(person.getId(), null);
+        if (personTracker != null) {
+            return personTracker.getOrDefault(mode, null).equals(stop.getId());
+        }
+        return false;
+    }
+
     /**
      * "Translates" an internally used {@link RTransfer} into a publicly usable {@link Transfer} object.
      * @param transfer

matsim/src/test/java/ch/sbb/matsim/routing/pt/raptor/RaptorStopFinderTest.java

@@ -1842,6 +1842,95 @@ public void testDefaultStopFinder_testMultipleModes() {
         }
     }
 
+    @Test
+    public void testModeConsistencyWithVehicleTracking() {
+        StopFinderFixture f0 = new StopFinderFixture(1., 1., 1., 1.);
+        // add PT line to allow return to origin
+        f0.withReversedPTline("BLine", "BB", "XB", 1., 10 * 3600);
+        Facility facAA = new FakeFacility(new Coord(-10, 0), Id.create("AA-ish", Link.class));
+        Facility facXX = new FakeFacility(new Coord(100010, 0), Id.create("XX-ish", Link.class));
+
+        String[] accessibleStops = new String[]{"B","X"};
+        for (String stop : accessibleStops) {
+            f0.scenario.getTransitSchedule().getFacilities().get(Id.create(stop, TransitStopFacility.class)).getAttributes().putAttribute("carAccessible", "true") ;
+            f0.scenario.getTransitSchedule().getFacilities().get(Id.create(stop, TransitStopFacility.class)).getAttributes().putAttribute("walkAccessible", "true") ;
+        }
+        Map<String, RoutingModule> routingModules = new HashMap<>();
+        routingModules.put(
+                TransportMode.walk,
+                new TeleportationRoutingModule(TransportMode.walk, f0.scenario, 1.5, 1.0)
+        );
+        routingModules.put(
+                TransportMode.car,
+                new TeleportationRoutingModule(TransportMode.car, f0.scenario, 5., 1.0)
+        );
+
+        f0.srrConfig.setUseIntermodalAccessEgress(true);
+        SwissRailRaptorConfigGroup.IntermodalAccessEgressParameterSet walkAccess = new SwissRailRaptorConfigGroup.IntermodalAccessEgressParameterSet();
+        walkAccess.setMode(TransportMode.walk);
+        walkAccess.setMaxRadius(600); // Includes stops B and X
+        walkAccess.setInitialSearchRadius(600);
+        walkAccess.setSearchExtensionRadius(0);
+        walkAccess.setStopFilterAttribute("walkAccessible");
+        walkAccess.setStopFilterValue("true");
+        f0.srrConfig.addIntermodalAccessEgress(walkAccess);
+        SwissRailRaptorConfigGroup.IntermodalAccessEgressParameterSet carAccess = new SwissRailRaptorConfigGroup.IntermodalAccessEgressParameterSet();
+        carAccess.setMode(TransportMode.car);
+        carAccess.setMaxRadius(600); // Includes stops B and X
+        carAccess.setInitialSearchRadius(600);
+        carAccess.setSearchExtensionRadius(0);
+        carAccess.setStopFilterAttribute("carAccessible");
+        carAccess.setStopFilterValue("true");
+        f0.srrConfig.addIntermodalAccessEgress(carAccess);
+        // checks all options and picks one with the smallest disutility
+        f0.srrConfig.setIntermodalAccessEgressModeSelection(
+                SwissRailRaptorConfigGroup.IntermodalAccessEgressModeSelection.CalcLeastCostModePerStop);
+        // make car the preferred option
+        f0.dummyPerson.getAttributes().putAttribute("subpopulation", "default");
+        f0.scenario.getConfig().planCalcScore().getScoringParameters("default")
+                .getOrCreateModeParams("walk").setMarginalUtilityOfTraveling(-10);
+        f0.scenario.getConfig().planCalcScore().getScoringParameters("default")
+                .getOrCreateModeParams("car").setMarginalUtilityOfTraveling(-1);
+
+        SwissRailRaptorData data = SwissRailRaptorData.create(f0.scenario.getTransitSchedule(), null, RaptorUtils.createStaticConfig(f0.config), f0.scenario.getNetwork(), null);
+        DefaultRaptorStopFinder stopFinder = new DefaultRaptorStopFinder(new DefaultRaptorIntermodalAccessEgress(), routingModules);
+        SwissRailRaptor raptor = new SwissRailRaptor.Builder(data, f0.scenario.getConfig()).with(stopFinder).build();
+
+        // test that you can't use a vehicle-tracked mode on both sides of a PT trip
+        List<? extends PlanElement> outboundLegs = raptor.calcRoute(DefaultRoutingRequest.withoutAttributes(facAA, facXX, 7 * 3600, f0.dummyPerson));
+        System.out.println("Outbound Trip Legs");
+        for (PlanElement leg : outboundLegs) {
+            System.out.println(leg);
+        }
+
+        Assert.assertEquals("wrong number of legs.", 3, outboundLegs.size());
+        Assert.assertEquals(TransportMode.car, ((Leg) outboundLegs.get(0)).getMode());
+        Assert.assertEquals(TransportMode.pt, ((Leg) outboundLegs.get(1)).getMode());
+        Assert.assertEquals(TransportMode.walk, ((Leg) outboundLegs.get(2)).getMode());
+
+        // test that you can take the car on the way back
+        List<? extends PlanElement> inboundLegs = raptor.calcRoute(DefaultRoutingRequest.withoutAttributes(facXX, facAA, 9 * 3600, f0.dummyPerson));
+        System.out.println("Inbound Trip Legs");
+        for (PlanElement leg : inboundLegs) {
+            System.out.println(leg);
+        }
+
+        Assert.assertEquals("wrong number of legs.", 3, inboundLegs.size());
+        Assert.assertEquals(TransportMode.walk, ((Leg) inboundLegs.get(0)).getMode());
+        Assert.assertEquals(TransportMode.pt, ((Leg) inboundLegs.get(1)).getMode());
+        Assert.assertEquals(TransportMode.car, ((Leg) inboundLegs.get(2)).getMode());
+
+        // test that you can't take the car on the way back twice in the row
+        // the vehicle has been picked up from the stop in previous trip
+        List<? extends PlanElement> inboundAgainLegs = raptor.calcRoute(DefaultRoutingRequest.withoutAttributes(facXX, facAA, 9 * 3600, f0.dummyPerson));
+        System.out.println("Inbound Again Trip Legs");
+        for (PlanElement leg : inboundAgainLegs) {
+            System.out.println(leg);
+        }
+
+        Assert.assertEquals(TransportMode.walk, ((Leg) inboundAgainLegs.get(2)).getMode());
+    }
+
     private static class StopFinderFixture {
 
         final SwissRailRaptorConfigGroup srrConfig;
@@ -2018,5 +2107,30 @@ public StopFinderFixture(double offsetB, double offsetC, double offsetD, double
             this.dummyPerson = this.scenario.getPopulation().getFactory().createPerson(Id.create("dummy", Person.class));
 
         }
+
+        void withReversedPTline(String lineId, String stopLinkId, String reverseLinkId, double offset, int depTime) {
+            TransitSchedule schedule = this.scenario.getTransitSchedule();
+            TransitScheduleFactory sf = schedule.getFactory();
+
+            TransitLine line = this.scenario.getTransitSchedule().getTransitLines().get(Id.create(lineId, TransitLine.class));
+
+            // Reversed transit line
+            TransitLine ptLine = sf.createTransitLine(Id.create("reversed-" + lineId, TransitLine.class));
+            TransitRoute ptRoute = line.getRoutes().values().iterator().next();
+
+            NetworkRoute networkRoute = RouteUtils.createLinkNetworkRouteImpl(
+                    Id.create(stopLinkId, Link.class),
+                    new Id[]{Id.create(reverseLinkId, Link.class)},
+                    Id.create(stopLinkId, Link.class));
+            List<TransitRouteStop> stops = new ArrayList<>(2);
+            stops.add(sf.createTransitRouteStopBuilder(ptRoute.getStops().get(1).getStopFacility()).departureOffset(0.0).build());
+            stops.add(sf.createTransitRouteStopBuilder(ptRoute.getStops().get(0).getStopFacility()).arrivalOffset(offset).build());
+            TransitRoute route = sf.createTransitRoute(Id.create("rev-route-" + lineId, TransitRoute.class),
+                    networkRoute, stops, ptRoute.getTransportMode());
+            route.addDeparture(sf.createDeparture(Id.create("1", Departure.class), depTime));
+            ptLine.addRoute(route);
+
+            schedule.addTransitLine(ptLine);
+        }
     }
 }