Add late diversion constraint for drt detours

contribs/drt-extensions/src/main/java/org/matsim/contrib/drt/extension/edrt/run/EDrtModeOptimizerQSimModule.java

@@ -132,7 +132,7 @@ public EmptyVehicleChargingScheduler get() {
 						getter.getModal(QSimScopeForkJoinPoolHolder.class).getPool()))).asEagerSingleton();
 
 		bindModal(InsertionCostCalculator.class).toProvider(modalProvider(
-				getter -> new DefaultInsertionCostCalculator(getter.getModal(CostCalculationStrategy.class))));
+				getter -> new DefaultInsertionCostCalculator(getter.getModal(CostCalculationStrategy.class), drtCfg)));
 
 		install(DrtModeOptimizerQSimModule.getInsertionSearchQSimModule(drtCfg));
 

contribs/drt-extensions/src/main/java/org/matsim/contrib/drt/extension/operations/shifts/run/ShiftDrtModeOptimizerQSimModule.java

@@ -117,7 +117,7 @@ shiftsParams, new DefaultShiftStartLogic(), new DefaultAssignShiftToVehicleLogic
 
 		bindModal(InsertionCostCalculator.class).toProvider(modalProvider(
 				getter -> new ShiftInsertionCostCalculator(getter.get(MobsimTimer.class),
-						new DefaultInsertionCostCalculator(getter.getModal(CostCalculationStrategy.class)))));
+						new DefaultInsertionCostCalculator(getter.getModal(CostCalculationStrategy.class), drtCfg))));
 
 		bindModal(VehicleEntry.EntryFactory.class).toInstance(new ShiftVehicleDataEntryFactory(new VehicleDataEntryFactoryImpl(drtCfg)));
 

contribs/drt/src/main/java/org/matsim/contrib/drt/optimizer/DrtModeOptimizerQSimModule.java

@@ -100,7 +100,7 @@ protected void configureQSim() {
 						getter.getModal(QSimScopeForkJoinPoolHolder.class).getPool()))).asEagerSingleton();
 
 		bindModal(InsertionCostCalculator.class).toProvider(modalProvider(
-				getter -> new DefaultInsertionCostCalculator(getter.getModal(CostCalculationStrategy.class))));
+				getter -> new DefaultInsertionCostCalculator(getter.getModal(CostCalculationStrategy.class), drtCfg)));
 
 		install(getInsertionSearchQSimModule(drtCfg));
 

contribs/drt/src/main/java/org/matsim/contrib/drt/optimizer/insertion/DefaultInsertionCostCalculator.java

@@ -21,17 +21,22 @@
 
 import static org.matsim.contrib.drt.optimizer.insertion.InsertionGenerator.Insertion;
 
+import org.matsim.contrib.drt.optimizer.Waypoint;
 import org.matsim.contrib.drt.optimizer.insertion.InsertionDetourTimeCalculator.DetourTimeInfo;
 import org.matsim.contrib.drt.passenger.DrtRequest;
+import org.matsim.contrib.drt.run.DrtConfigGroup;
 
 /**
  * @author michalm
  */
 public class DefaultInsertionCostCalculator implements InsertionCostCalculator {
 	private final CostCalculationStrategy costCalculationStrategy;
+	private final DrtConfigGroup drtConfigGroup;
 
-	public DefaultInsertionCostCalculator(CostCalculationStrategy costCalculationStrategy) {
+	public DefaultInsertionCostCalculator(CostCalculationStrategy costCalculationStrategy,
+										  DrtConfigGroup drtConfigGroup) {
 		this.costCalculationStrategy = costCalculationStrategy;
+		this.drtConfigGroup = drtConfigGroup;
 	}
 
 	/**
@@ -55,6 +60,32 @@ public double calculate(DrtRequest drtRequest, Insertion insertion, DetourTimeIn
 			return INFEASIBLE_SOLUTION_COST;
 		}
 
+		// all stops after the new (potential) pickup but before the new dropoff
+		// are delayed by pickupDetourTimeLoss
+		double detour = detourTimeInfo.pickupDetourInfo.pickupTimeLoss;
+		if(vEntry.stops != null) {
+			for (int s = insertion.pickup.index; s < vEntry.stops.size(); s++) {
+				Waypoint.Stop stop = vEntry.stops.get(s);
+				// passengers are being dropped off == may be close to arrival
+				if (!stop.task.getDropoffRequests().isEmpty()) {
+					double nextArrival = stop.getArrivalTime();
+					double departureTime = insertion.vehicleEntry.start.getDepartureTime();
+					//arrival is very soon
+					if (nextArrival - departureTime < drtConfigGroup.allowDetourBeforeArrivalThreshold &&
+							detour > 0) {
+						return INFEASIBLE_SOLUTION_COST;
+					} else {
+						// all following stops are above the threshold
+						break;
+					}
+				}
+				if (s == insertion.dropoff.index) {
+					// all stops after the new (potential) dropoff are delayed by totalTimeLoss
+					detour = detourTimeInfo.getTotalTimeLoss();
+				}
+			}
+		}
+
 		return costCalculationStrategy.calcCost(drtRequest, insertion, detourTimeInfo);
 	}
 }

contribs/drt/src/main/java/org/matsim/contrib/drt/run/DrtConfigGroup.java

@@ -184,6 +184,15 @@ public enum OperationalScheme {
 	@PositiveOrZero
 	public double advanceRequestPlanningHorizon = 0; // beta-feature; planning horizon for advance (prebooked) requests
 
+	@Parameter
+	@Comment(
+			"Time before reaching a planned dropoff from which it is not allowed to insert new detours for new requests. I.e.," +
+					" if set to 180, then a vehicle will not divert to pickup or dropoff a new passenger once a boarded passenger is only " +
+					"3 minutes away from her destination, even though her time window would allow it." +
+					" Delayed detours just before arrival are usually perceived very negatively.")
+	@PositiveOrZero
+	public double allowDetourBeforeArrivalThreshold = 0; // [s];
+
 	@NotNull
 	private DrtInsertionSearchParams drtInsertionSearchParams;
 

contribs/drt/src/test/java/org/matsim/contrib/drt/optimizer/insertion/InsertionCostCalculatorTest.java

@@ -24,12 +24,18 @@
 import static org.matsim.contrib.drt.optimizer.insertion.InsertionCostCalculator.INFEASIBLE_SOLUTION_COST;
 import static org.matsim.contrib.drt.optimizer.insertion.InsertionDetourTimeCalculator.*;
 
+import com.google.common.collect.ImmutableList;
 import org.junit.Test;
 import org.matsim.api.core.v01.Id;
 import org.matsim.api.core.v01.network.Link;
 import org.matsim.contrib.drt.optimizer.VehicleEntry;
+import org.matsim.contrib.drt.optimizer.Waypoint;
 import org.matsim.contrib.drt.optimizer.insertion.InsertionGenerator.Insertion;
+import org.matsim.contrib.drt.passenger.AcceptedDrtRequest;
 import org.matsim.contrib.drt.passenger.DrtRequest;
+import org.matsim.contrib.drt.run.DrtConfigGroup;
+import org.matsim.contrib.drt.schedule.DefaultDrtStopTask;
+import org.matsim.contrib.drt.schedule.DrtStopTask;
 import org.matsim.testcases.fakes.FakeLink;
 
 /**
@@ -42,36 +48,133 @@ public class InsertionCostCalculatorTest {
 
 	@Test
 	public void testCalculate() {
-		VehicleEntry entry = entry(new double[] { 20, 50 });
+		VehicleEntry entry = entry(new double[] { 20, 50 }, null, null);
 		var insertion = insertion(entry, 0, 1);
 
 		//feasible solution
 		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(0, 11), new DropoffDetourInfo(0, 22)),
-				11 + 22);
+				11 + 22, new DrtConfigGroup(), drtRequest);
 
 		//feasible solution - longest possible pickup and dropoff time losses
 		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(0, 20), new DropoffDetourInfo(0, 30)),
-				20 + 30);
+				20 + 30, new DrtConfigGroup(), drtRequest);
 
 		//infeasible solution - time constraints at stop 0
 		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(0, 21), new DropoffDetourInfo(0, 29)),
-				INFEASIBLE_SOLUTION_COST);
+				INFEASIBLE_SOLUTION_COST, new DrtConfigGroup(), drtRequest);
 
 		//infeasible solution - vehicle time constraints
 		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(0, 20), new DropoffDetourInfo(0, 31)),
-				INFEASIBLE_SOLUTION_COST);
+				INFEASIBLE_SOLUTION_COST, new DrtConfigGroup(), drtRequest);
 	}
 
-	private void assertCalculate(Insertion insertion, DetourTimeInfo detourTimeInfo, double expectedCost) {
+
+	@Test
+	public void testAllowDetourBeforeArrivalThreshold() {
+
+
+		// start (0s) -----> new PU (60s) -----> existing DO (120s) -----> new DO (300s)
+
+		Waypoint.Start start = new Waypoint.Start(null, link("start"), 0, 0);
+
+		DrtStopTask existingDropoffTask = new DefaultDrtStopTask(120, 150, link("boardedDO"));
+		DrtRequest boardedRequest = DrtRequest.newBuilder().fromLink(link("boardedFrom")).toLink(link("boardedTo")).build();
+
+		AcceptedDrtRequest existingRequest = AcceptedDrtRequest.createFromOriginalRequest(boardedRequest);
+		existingDropoffTask.addDropoffRequest(existingRequest);
+
+		Waypoint.Stop[] stops = new Waypoint.Stop[1];
+		stops[0] = new Waypoint.Stop(existingDropoffTask, 0);
+
+		VehicleEntry entry = entry(new double[] {60, 300}, ImmutableList.copyOf(stops), start);
+		var insertion = insertion(entry, 0, 1);
+
+		DrtRequest drtRequest = DrtRequest.newBuilder()
+				.fromLink(fromLink)
+				.toLink(toLink)
+				.latestStartTime(120)
+				.latestArrivalTime(300)
+				.build();
+
+		DrtConfigGroup drtConfigGroup = new DrtConfigGroup();
+
+		// new insertion before dropoff of boarded passenger within threshold - infeasible solution
+		drtConfigGroup.allowDetourBeforeArrivalThreshold = 180;
+		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(60, 30), new DropoffDetourInfo(300, 30)),
+				INFEASIBLE_SOLUTION_COST, drtConfigGroup, drtRequest);
+
+		// new insertion before dropoff of boarded passenger, inside of threshold but no additional delay - feasible solution
+		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(120, 0), new DropoffDetourInfo(300, 30)),
+				30, drtConfigGroup, drtRequest);
+
+		// new insertion before dropoff of boarded passenger, but outside of threshold - feasible solution
+		drtConfigGroup.allowDetourBeforeArrivalThreshold = 120;
+		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(60, 30), new DropoffDetourInfo(300, 30)),
+				60, drtConfigGroup, drtRequest);
+
+
+		// new insertion after dropoff of boarded passenger - feasible solution
+		insertion = insertion(entry, 1, 1);
+		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(60, 30), new DropoffDetourInfo(300, 30)),
+				60, drtConfigGroup, drtRequest);
+	}
+
+	@Test
+	public void testAllowDetourBeforeArrivalThreshold2() {
+
+		// start (0s) -----> new PU (60s) -----> existing PU (120s) -----> existing DO (200s) -----> new DO (300s)
+
+		Waypoint.Start start = new Waypoint.Start(null, link("start"), 0, 0);
+
+		DrtStopTask existingPickupTask = new DefaultDrtStopTask(120, 150, link("scheduledPU"));
+		DrtRequest scheduledRequest = DrtRequest.newBuilder().fromLink(link("scheduledFrom")).toLink(link("scheduledTo")).build();
+		AcceptedDrtRequest acceptedScheduledRequest = AcceptedDrtRequest.createFromOriginalRequest(scheduledRequest);
+		existingPickupTask.addPickupRequest(acceptedScheduledRequest);
+
+		DrtStopTask existingDropoffTask = new DefaultDrtStopTask(200, 230, link("boardedDO"));
+		DrtRequest boardedRequest = DrtRequest.newBuilder().fromLink(link("boardedFrom")).toLink(link("boardedTo")).build();
+		AcceptedDrtRequest existingRequest = AcceptedDrtRequest.createFromOriginalRequest(boardedRequest);
+		existingDropoffTask.addDropoffRequest(existingRequest);
+
+		Waypoint.Stop[] stops = new Waypoint.Stop[2];
+		stops[0] = new Waypoint.Stop(existingPickupTask, 2);
+		stops[1] = new Waypoint.Stop(existingDropoffTask, 1);
+
+		VehicleEntry entry = entry(new double[] {60, 60, 300}, ImmutableList.copyOf(stops), start);
+
+
+		var insertion = insertion(entry, 0, 2);
+
+		DrtRequest drtRequest = DrtRequest.newBuilder()
+				.fromLink(fromLink)
+				.toLink(toLink)
+				.latestStartTime(120)
+				.latestArrivalTime(300)
+				.build();
+
+		DrtConfigGroup drtConfigGroup = new DrtConfigGroup();
+
+		// new insertion before dropoff of boarded passenger within threshold - infeasible solution
+		drtConfigGroup.allowDetourBeforeArrivalThreshold = 300;
+		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(60, 60), new DropoffDetourInfo(300, 60)),
+				INFEASIBLE_SOLUTION_COST, drtConfigGroup, drtRequest);
+
+		// new insertion before dropoff of boarded passenger outside of threshold - feasible solution
+		drtConfigGroup.allowDetourBeforeArrivalThreshold = 200;
+		assertCalculate(insertion, new DetourTimeInfo(new PickupDetourInfo(60, 60), new DropoffDetourInfo(300, 60)),
+				120, drtConfigGroup, drtRequest);
+	}
+
+	private void assertCalculate(Insertion insertion, DetourTimeInfo detourTimeInfo, double expectedCost, DrtConfigGroup drtConfigGroup, DrtRequest drtRequest) {
 		var insertionCostCalculator = new DefaultInsertionCostCalculator(
-				new CostCalculationStrategy.RejectSoftConstraintViolations());
+				new CostCalculationStrategy.RejectSoftConstraintViolations(), drtConfigGroup);
 		var insertionWithDetourData = new InsertionWithDetourData(insertion, null, detourTimeInfo);
 		assertThat(insertionCostCalculator.calculate(drtRequest, insertionWithDetourData.insertion,
 				insertionWithDetourData.detourTimeInfo)).isEqualTo(expectedCost);
 	}
 
-	private VehicleEntry entry(double[] slackTimes) {
-		return new VehicleEntry(null, null, null, slackTimes);
+	private VehicleEntry entry(double[] slackTimes, ImmutableList<Waypoint.Stop> stops, Waypoint.Start start) {
+		return new VehicleEntry(null, start, stops, slackTimes);
 	}
 
 	private Link link(String id) {