Allow freight agents to use the correct freight vehicles

src/main/scala/beam/agentsim/agents/PersonAgent.scala

@@ -1075,7 +1075,7 @@ class PersonAgent(
       potentiallyChargingBeamVehicles.remove(vehicle.id)
       goto(ProcessingNextLegOrStartActivity)
     case Event(NotAvailable(_), basePersonData: BasePersonData) =>
-      log.warning(f"${this.id} replanning because vehicle not available when trying to board", this.id.toString)
+      log.warning(f"${this.id} replanning because vehicle not available when trying to board")
       val replanningReason = getReplanningReasonFrom(basePersonData, ReservationErrorCode.ResourceUnavailable.entryName)
       val currentCoord =
         beamServices.geo.wgs2Utm(basePersonData.restOfCurrentTrip.head.beamLeg.travelPath.startPoint).loc
@@ -1104,6 +1104,7 @@ class PersonAgent(
           numberOfReplanningAttempts = basePersonData.numberOfReplanningAttempts + 1
         ),
         SpaceTime(currentCoord, _currentTick.get),
+        isWithinTripReplanning = true,
         excludeModes =
           if (canUseCars(currentCoord, nextCoord)) Set.empty
           else Set(BeamMode.RIDE_HAIL, BeamMode.CAR, BeamMode.CAV)

src/main/scala/beam/agentsim/agents/modalbehaviors/ChoosesMode.scala

@@ -51,6 +51,8 @@ import scala.concurrent.{ExecutionContext, Future}
 trait ChoosesMode {
   this: PersonAgent => // Self type restricts this trait to only mix into a PersonAgent
 
+  private val BUFFER_PER_REPLANNING_ATTEMPT_IN_SEC: Int = 5
+
   private val dummyRHVehicle: StreetVehicle = createDummyVehicle(
     "dummyRH",
     beamServices.beamConfig.beam.agentsim.agents.rideHail.managers.head.initialization.procedural.vehicleTypeId,
@@ -179,10 +181,25 @@ trait ChoosesMode {
       // If we're on a walk based tour but using a vehicle for access/egress
       case (data: ChoosesModeData, Some(BIKE_TRANSIT | DRIVE_TRANSIT), Some(WALK_BASED))
           if data.personData.currentTourPersonalVehicle.isDefined =>
-        self ! MobilityStatusResponse(
-          Vector(beamVehicles(data.personData.currentTourPersonalVehicle.get)),
-          getCurrentTriggerIdOrGenerate
-        )
+        val currentTourPersonalVehicleId = data.personData.currentTourPersonalVehicle.get
+        if (beamVehicles.contains(currentTourPersonalVehicleId)) {
+          self ! MobilityStatusResponse(
+            Vector(beamVehicles(currentTourPersonalVehicleId)),
+            getCurrentTriggerIdOrGenerate
+          )
+        } else {
+          logger.error(
+            s"Person ${this.id} could not find vehicle $currentTourPersonalVehicleId. " +
+            s"The cause is unknown. We will request an available vehicle from the vehicle manager."
+          )
+          implicit val executionContext: ExecutionContext = context.system.dispatcher
+          requestAvailableVehicles(
+            vehicleFleets,
+            data.currentLocation,
+            currentActivity(data.personData),
+            Some(VehicleCategory.Car)
+          ) pipeTo self
+        }
       // Create teleportation vehicle if we are told to use teleportation
       case (data: ChoosesModeData, Some(HOV2_TELEPORTATION | HOV3_TELEPORTATION), _) =>
         val teleportationVehicle = createSharedTeleportationVehicle(data.currentLocation)
@@ -426,6 +443,10 @@ trait ChoosesMode {
         triggerId
       )
 
+      // Note that remainingAvailableVehicles includes all vehicles that were available,
+      // and any unused vehicles will be released.
+      // That's why we remove any drive_transit vehicles after
+      // replanning -- so they don't get released.
       val newPersonData = choosesModeData.copy(
         personData = personData
           .copy(
@@ -1244,7 +1265,10 @@ trait ChoosesMode {
         .flatMap(v => beamVehicles.get(v))
         .filterNot(_.vehicle.isSharedVehicle)
         .toVector
-        .distinct
+        .groupBy(_.id)
+        .values
+        .map(_.head)
+        .toVector
 
       val availableEmergencyVehicles =
         beamVehicles.filterKeys(k => k.toString.startsWith(f"${this.id.toString}-emergency")).values.toVector
@@ -1405,6 +1429,36 @@ trait ChoosesMode {
                 )
                 gotoFinishingModeChoice(bushwhackingTrip)
               }
+            case Some(mode @ (HOV2_TELEPORTATION | HOV3_TELEPORTATION)) =>
+              logger.warn(
+                f"Routing request for teleportation person ${this.id} failed. Creating a bushwhacking trip from " +
+                f"$currentPersonLocation to ${nextAct.getCoord}"
+              )
+              val teleportationTripWithoutRoute = createExpensiveVehicleTrip(
+                currentPersonLocation,
+                nextAct,
+                allAvailableStreetVehicles,
+                routingResponse,
+                mode match {
+                  case HOV2_TELEPORTATION => CAR_HOV2
+                  case _                  => CAR_HOV3
+                }
+              )
+              gotoFinishingModeChoice(teleportationTripWithoutRoute)
+            case Some(CAR) if choosesModeData.personData.currentTourMode.contains(FREIGHT_TOUR) =>
+              logger.error(
+                f"Routing request for freight agent ${this.id} failed. Creating a bushwhacking CAR trip from " +
+                f"$currentPersonLocation to ${nextAct.getCoord}"
+              )
+              val expensiveFreightTrip =
+                createExpensiveVehicleTrip(
+                  currentPersonLocation,
+                  nextAct,
+                  allAvailableStreetVehicles,
+                  routingResponse,
+                  CAR
+                )
+              gotoFinishingModeChoice(expensiveFreightTrip)
             case Some(CAR)
                 if newAndTourVehicles.isEmpty &&
                   beamScenario.beamConfig.beam.agentsim.agents.vehicles.generateEmergencyHouseholdVehicleWhenPlansRequireIt =>
@@ -1461,6 +1515,63 @@ trait ChoosesMode {
       }
   }
 
+  private def createExpensiveVehicleTrip(
+    currentPersonLocation: SpaceTime,
+    nextAct: Activity,
+    availableStreetVehicles: Vector[VehicleOrToken],
+    routingResponse: RoutingResponse,
+    mode: BeamMode
+  ) = {
+    availableStreetVehicles.find(_.streetVehicle.mode == CAR) match {
+      case Some(availableVehicle) =>
+        val bushwhackingLeg = RoutingWorker
+          .createBushwackingTrip(
+            currentPersonLocation.loc,
+            nextAct.getCoord,
+            _currentTick.get,
+            availableVehicle.streetVehicle,
+            beamServices.geo,
+            mode = mode,
+            unbecomeDriverOnCompletion = false
+          )
+          .legs
+          .head
+        EmbodiedBeamTrip(
+          Vector(
+            EmbodiedBeamLeg.dummyLegAt(
+              _currentTick.get,
+              body.id,
+              isLastLeg = false,
+              beamServices.geo.utm2Wgs(currentPersonLocation.loc),
+              WALK,
+              body.beamVehicleType.id
+            )
+          ) :+ bushwhackingLeg :+
+          EmbodiedBeamLeg.dummyLegAt(
+            _currentTick.get + bushwhackingLeg.beamLeg.duration,
+            availableVehicle.id,
+            isLastLeg = true,
+            beamServices.geo.utm2Wgs(nextAct.getCoord),
+            mode,
+            availableVehicle.vehicle.beamVehicleType.id
+          ) :+ EmbodiedBeamLeg.dummyLegAt(
+            _currentTick.get + bushwhackingLeg.beamLeg.duration,
+            body.id,
+            isLastLeg = true,
+            beamServices.geo.utm2Wgs(nextAct.getCoord),
+            WALK,
+            body.beamVehicleType.id
+          )
+        )
+      case _ =>
+        logger.warn(
+          f"Failed to create bushwhacking vehicle trip for agent ${routingResponse.request.flatMap(_.personId)} " +
+          "because no  vehicle are available"
+        )
+        createExpensiveWalkTrip(currentPersonLocation, nextAct, routingResponse)
+    }
+  }
+
   private def createExpensiveWalkTrip(
     currentPersonLocation: SpaceTime,
     nextAct: Activity,
@@ -1490,19 +1601,40 @@ trait ChoosesMode {
 
   private def gotoChoosingModeWithoutPredefinedMode(choosesModeData: ChoosesModeData) = {
     // TODO: Check modes for subsequent trips here
-    val onFirstTrip = isFirstTripWithinTour(currentActivity(choosesModeData.personData))
-    val outcomeTourMode = if (onFirstTrip) { None }
+    val onFirstTripWithinTour: Boolean = isFirstTripWithinTour(currentActivity(choosesModeData.personData))
+    val withinReplanning: Boolean = choosesModeData.isWithinTripReplanning
+    val agentStillAtTourOrigin: Boolean = onFirstTripWithinTour && !withinReplanning
+    val outcomeTourMode = if (agentStillAtTourOrigin) { None }
     else { Some(WALK_BASED) }
+    val isAccessEgressInTour: Boolean = choosesModeData.personData.currentTourMode.contains(WALK_BASED)
     val newTourVehicle = choosesModeData.personData.currentTourPersonalVehicle match {
       case Some(id) if beamVehicles.contains(id) =>
-        if (choosesModeData.personData.currentTourMode.contains(WALK_BASED) & !onFirstTrip) {
+        if (isAccessEgressInTour && !agentStillAtTourOrigin) {
+          /*
+           * This code block only runs when someone needs to re-plan and re-do mode choice.
+           * If for instance they were going to take a bike trip but no bike route was available
+           * they need to release the bike so others can use it.
+           * But if they're in the middle of a tour and just can't find a transit route,
+           * for instance, but they took drive_transit on their first leg and need to take it home,
+           * we keep the original vehicle in beamVehicles so we can use it later
+           *
+           * The problem is that when someone gets a resourceCapacityExhausted error on the first leg of a drive_transit tour,
+           * the existing logic thinks that we're in the first scenario (didn't use a vehicle so we can release it)
+           * rather than the second one (have already used a vehicle and need to return to it at the end of our tour).
+           *
+           * For that matter, we are adding "choosesModeData.isWithinTripReplanning". As long as we are still replanning
+           * we don't release the vehicle until their last tour trip of their tour.
+           *
+           * e.g., they'll just get on the next train, go about their drive_transit tour, and
+           * then take drive_transit as the mode for the last leg of their tour and pick up their car on the way home
+           * */
           Some(id)
         } else {
           val vehicle = beamVehicles(id).vehicle
           vehicle.setMustBeDrivenHome(false)
           beamVehicles.remove(vehicle.id)
           vehicle.getManager.get ! ReleaseVehicle(vehicle, getCurrentTriggerId.get)
-          if (!onFirstTrip) {
+          if (!agentStillAtTourOrigin) {
             logger.warn(
               s"Abandoning vehicle $id because no return ${choosesModeData.personData.currentTripMode} " +
               s"itinerary is available"
@@ -2207,7 +2339,12 @@ trait ChoosesMode {
             } else {
               // Reset available vehicles so we don't release our car that we've left during this replanning
               resetVehicles = true
-              makeRequestWith(withTransit = true, Vector(bodyStreetVehicle))
+              makeRequestWith(
+                withTransit = true,
+                Vector(bodyStreetVehicle),
+                departureBuffer =
+                  choosesModeData.personData.numberOfReplanningAttempts * BUFFER_PER_REPLANNING_ATTEMPT_IN_SEC
+              )
               responsePlaceholders = makeResponsePlaceholders(withRouting = true)
             }
           case (`lastTripIndex`, Some(currentTourPersonalVehicle)) =>

src/main/scala/beam/agentsim/agents/planning/BeamPlan.scala

@@ -159,7 +159,10 @@ class BeamPlan extends Plan {
 //  }
 
   private def getTourModeFromMatsimLeg(leg: Leg): Option[BeamTourMode] = {
-    Option(leg.getAttributes.getAttribute("tour_mode")).flatMap(x => BeamTourMode.fromString(x.toString))
+    Option(leg.getAttributes.getAttribute("PayloadWeightInKg")) match {
+      case Some(_) => Some(BeamTourMode.FREIGHT_TOUR)
+      case _       => Option(leg.getAttributes.getAttribute("tour_mode")).flatMap(x => BeamTourMode.fromString(x.toString))
+    }
   }
 
   private def getTourVehicleFromMatsimLeg(leg: Leg): Option[Id[BeamVehicle]] = {

src/main/scala/beam/router/RoutingWorker.scala

@@ -530,7 +530,8 @@ object RoutingWorker {
     atTime: Int,
     vehicle: StreetVehicle,
     geo: GeoUtils,
-    mode: BeamMode = WALK
+    mode: BeamMode = WALK,
+    unbecomeDriverOnCompletion: Boolean = true
   ): EmbodiedBeamTrip = {
     EmbodiedBeamTrip(
       Vector(
@@ -540,7 +541,7 @@ object RoutingWorker {
           vehicle.vehicleTypeId,
           asDriver = true,
           0,
-          unbecomeDriverOnCompletion = true
+          unbecomeDriverOnCompletion = unbecomeDriverOnCompletion
         )
       ),
       Some("Bushwhacking")

src/main/scala/beam/router/r5/CarWeightCalculator.scala

@@ -11,7 +11,7 @@ class CarWeightCalculator(workerParams: R5Parameters, travelTimeNoiseFraction: D
   private val networkHelper = workerParams.networkHelper
   private val transportNetwork = workerParams.transportNetwork
 
-  val maxFreeSpeed: Double = networkHelper.allLinks.map(_.getFreespeed).max
+  val maxFreeSpeed: Double = networkHelper.allLinks.map(_.getFreespeed).max / 0.621371 // Convert kph to mph
   private val minSpeed = workerParams.beamConfig.beam.physsim.minCarSpeedInMetersPerSecond
 
   private val noiseIdx: AtomicInteger = new AtomicInteger(0)

src/main/scala/beam/router/r5/R5Wrapper.scala

@@ -228,15 +228,15 @@ class R5Wrapper(workerParams: R5Parameters, travelTime: TravelTime, travelTimeNo
     // which means that this value must be an over(!)estimation, otherwise we will miss optimal routes,
     // particularly in the presence of tolls.
     profileRequest.carSpeed = carWeightCalculator.maxFreeSpeed.toFloat
-    profileRequest.maxWalkTime = 30
+    profileRequest.maxWalkTime = 60
     profileRequest.maxCarTime = 30
     profileRequest.maxBikeTime = 30
     // Maximum number of transit segments. This was previously hardcoded as 4 in R5, now it is a parameter
     // that defaults to 8 unless I reset it here. It is directly related to the amount of work the
     // transit router has to do.
-    profileRequest.maxRides = 4
-    profileRequest.streetTime = 2 * 60
-    profileRequest.maxTripDurationMinutes = 4 * 60
+    profileRequest.maxRides = 3
+    profileRequest.streetTime = 6 * 60
+    profileRequest.maxTripDurationMinutes = 6 * 60
     profileRequest.wheelchair = false
     profileRequest.bikeTrafficStress = 4
     profileRequest.zoneId = transportNetwork.getTimeZone

src/main/scala/beam/sim/common/GeoUtils.scala

@@ -27,7 +27,7 @@ case class EdgeWithCoord(edgeIndex: Int, wgsCoord: Coordinate)
 trait GeoUtils extends ExponentialLazyLogging {
 
   def localCRS: String
-  val defaultMaxRadiusForMapSearch = 10000
+  val defaultMaxRadiusForMapSearch = 20000
   private lazy val notExponentialLogger = Logger(LoggerFactory.getLogger(getClass.getName))
 
   lazy val utm2Wgs: GeotoolsTransformation =

src/main/scala/beam/sim/population/PopulationAttributes.scala

@@ -93,7 +93,8 @@ case class AttributesOfIndividual(
   ): Double = {
     //NOTE: This gives answers in hours
     embodiedBeamLeg.beamLeg.mode match {
-      case CAR => // NOTE: Ride hail legs are classified as CAR mode. For now we only need to loop through links here
+      case CAR
+          if embodiedBeamLeg.beamLeg.travelPath.linkIds.nonEmpty => // NOTE: Ride hail legs are classified as CAR mode. For now we only need to loop through links here
         val idsAndTravelTimes =
           embodiedBeamLeg.beamLeg.travelPath.linkIds.tail // ignore the first link because activities are located along links
             .zip(embodiedBeamLeg.beamLeg.travelPath.linkTravelTime.tail.map(time => math.round(time.toFloat)))