diff --git a/src/main/scala/beam/agentsim/agents/modalbehaviors/ChoosesMode.scala b/src/main/scala/beam/agentsim/agents/modalbehaviors/ChoosesMode.scala index 5939e51b82b..45a97a83a87 100755 --- a/src/main/scala/beam/agentsim/agents/modalbehaviors/ChoosesMode.scala +++ b/src/main/scala/beam/agentsim/agents/modalbehaviors/ChoosesMode.scala @@ -956,7 +956,7 @@ trait ChoosesMode { .head } val expensiveWalkTrip = EmbodiedBeamTrip( - Vector(originalWalkTripLeg.copy(cost = 100)) + Vector(originalWalkTripLeg.copy(replanningPenalty = 100.0)) ) goto(FinishingModeChoice) using choosesModeData.copy( diff --git a/src/main/scala/beam/agentsim/agents/modalbehaviors/ModeChoiceCalculator.scala b/src/main/scala/beam/agentsim/agents/modalbehaviors/ModeChoiceCalculator.scala index 2235eae3d8f..171d57cbda5 100755 --- a/src/main/scala/beam/agentsim/agents/modalbehaviors/ModeChoiceCalculator.scala +++ b/src/main/scala/beam/agentsim/agents/modalbehaviors/ModeChoiceCalculator.scala @@ -89,7 +89,7 @@ trait ModeChoiceCalculator extends HasServices { case _ => embodiedBeamTrip.costEstimate } - totalCost + totalCost + embodiedBeamTrip.replanningPenalty } def computeAllDayUtility( diff --git a/src/main/scala/beam/router/model/EmbodiedBeamLeg.scala b/src/main/scala/beam/router/model/EmbodiedBeamLeg.scala index 02feffdc001..63b79744d48 100644 --- a/src/main/scala/beam/router/model/EmbodiedBeamLeg.scala +++ b/src/main/scala/beam/router/model/EmbodiedBeamLeg.scala @@ -14,7 +14,8 @@ case class EmbodiedBeamLeg( asDriver: Boolean, cost: Double, unbecomeDriverOnCompletion: Boolean, - isPooledTrip: Boolean = false + isPooledTrip: Boolean = false, + replanningPenalty: Double = 0 ) { val isHumanBodyVehicle: Boolean = diff --git a/src/main/scala/beam/router/model/EmbodiedBeamTrip.scala b/src/main/scala/beam/router/model/EmbodiedBeamTrip.scala index 70169090ed7..a901ef280b7 100644 --- a/src/main/scala/beam/router/model/EmbodiedBeamTrip.scala +++ b/src/main/scala/beam/router/model/EmbodiedBeamTrip.scala @@ -33,6 +33,9 @@ case class EmbodiedBeamTrip(legs: IndexedSeq[EmbodiedBeamLeg]) { !_.asDriver ) + @transient + lazy val replanningPenalty: Double = legs.map(_.replanningPenalty).sum + val totalTravelTimeInSecs: Int = legs.lastOption.map(_.beamLeg.endTime - legs.head.beamLeg.startTime).getOrElse(0) def beamLegs(): IndexedSeq[BeamLeg] = diff --git a/src/main/scala/beam/sim/common/GeoUtils.scala b/src/main/scala/beam/sim/common/GeoUtils.scala index c40002608ad..c372dbe6971 100755 --- a/src/main/scala/beam/sim/common/GeoUtils.scala +++ b/src/main/scala/beam/sim/common/GeoUtils.scala @@ -2,15 +2,20 @@ package beam.sim.common import beam.agentsim.events.SpaceTime import beam.sim.config.BeamConfig +import beam.utils.ProfilingUtils import beam.utils.logging.ExponentialLazyLogging +import beam.utils.map.GpxPoint import com.conveyal.r5.profile.StreetMode -import com.conveyal.r5.streets.{Split, StreetLayer} +import com.conveyal.r5.streets.{EdgeStore, Split, StreetLayer} import com.google.inject.{ImplementedBy, Inject} -import com.vividsolutions.jts.geom.Envelope +import com.vividsolutions.jts.geom.{Coordinate, Envelope} +import org.matsim.api.core.v01 import org.matsim.api.core.v01.Coord import org.matsim.api.core.v01.network.Link import org.matsim.core.utils.geometry.transformations.GeotoolsTransformation +case class EdgeWithCoord(edgeIndex: Int, wgsCoord: Coordinate) + /** * Created by sfeygin on 4/2/17. */ @@ -64,7 +69,21 @@ trait GeoUtils extends ExponentialLazyLogging { def getNearestR5Edge(streetLayer: StreetLayer, coordWGS: Coord, maxRadius: Double = 1E5): Int = { val theSplit = getR5Split(streetLayer, coordWGS, maxRadius, StreetMode.WALK) if (theSplit == null) { - Int.MinValue + val closestEdgesToTheCorners = ProfilingUtils + .timed("getEdgesCloseToBoundingBox", x => logger.info(x)) { + getEdgesCloseToBoundingBox(streetLayer) + } + .map { case (edgeWithCoord, gpxPoint) => edgeWithCoord } + val closest = closestEdgesToTheCorners.minBy { edge => + val matsimUtmCoord = wgs2Utm(new v01.Coord(edge.wgsCoord.x, edge.wgsCoord.y)) + distUTMInMeters(matsimUtmCoord, wgs2Utm(coordWGS)) + } + val distUTM = distUTMInMeters(wgs2Utm(coordWGS), wgs2Utm(new v01.Coord(closest.wgsCoord.x, closest.wgsCoord.y))) + logger.warn( + s"""The split is `null` for StreetLayer.BoundingBox: ${streetLayer.getEnvelope}, coordWGS: $coordWGS, maxRadius: $maxRadius. + | Will return closest to the corner: $closest which is $distUTM meters far away""".stripMargin + ) + closest.edgeIndex } else { theSplit.edge } @@ -106,6 +125,69 @@ trait GeoUtils extends ExponentialLazyLogging { } theSplit } + + def getEdgesCloseToBoundingBox(streetLayer: StreetLayer): Array[(EdgeWithCoord, GpxPoint)] = { + val cursor = streetLayer.edgeStore.getCursor() + val iter = new Iterator[EdgeStore#Edge] { + override def hasNext: Boolean = cursor.advance() + + override def next(): EdgeStore#Edge = cursor + } + + val boundingBox = streetLayer.envelope + + val insideBoundingBox = iter + .flatMap { edge => + Option(edge.getGeometry.getBoundary.getCoordinate).map { coord => + EdgeWithCoord(edge.getEdgeIndex, coord) + } + } + .withFilter(x => boundingBox.contains(x.wgsCoord)) + .toArray + + /* + min => x0,y0 + max => x1,y1 +x0,y1 (TOP LEFT) ._____._____. x1,y1 (TOP RIGHT) + | | + | | + . . + | | + | | +x0,y0 (BOTTOM LEFT) ._____._____. x1, y0 (BOTTOM RIGHT) + */ + + val bottomLeft = new Coord(boundingBox.getMinX, boundingBox.getMinY) + val topLeft = new Coord(boundingBox.getMinX, boundingBox.getMaxY) + val topRight = new Coord(boundingBox.getMaxX, boundingBox.getMaxY) + val bottomRight = new Coord(boundingBox.getMaxX, boundingBox.getMinY) + val midLeft = new Coord((bottomLeft.getX + topLeft.getX) / 2, (bottomLeft.getY + topLeft.getY) / 2) + val midTop = new Coord((topLeft.getX + topRight.getX) / 2, (topLeft.getY + topRight.getY) / 2) + val midRight = new Coord((topRight.getX + bottomRight.getX) / 2, (topRight.getY + bottomRight.getY) / 2) + val midBottom = new Coord((bottomLeft.getX + bottomRight.getX) / 2, (bottomLeft.getY + bottomRight.getY) / 2) + + val corners = Array( + GpxPoint("BottomLeft", bottomLeft), + GpxPoint("TopLeft", topLeft), + GpxPoint("TopRight", topRight), + GpxPoint("BottomRight", bottomRight), + GpxPoint("MidLeft", midLeft), + GpxPoint("MidTop", midTop), + GpxPoint("MidRight", midRight), + GpxPoint("MidBottom", midBottom) + ) + + val closestEdges = corners.map { gpxPoint => + val utmCornerCoord = wgs2Utm(gpxPoint.wgsCoord) + val closestEdge: EdgeWithCoord = insideBoundingBox.minBy { + case x => + val utmCoord = wgs2Utm(new Coord(x.wgsCoord.x, x.wgsCoord.y)) + distUTMInMeters(utmCornerCoord, utmCoord) + } + (closestEdge, gpxPoint) + } + closestEdges + } } object GeoUtils { @@ -217,6 +299,7 @@ object GeoUtils { rad } } + } class GeoUtilsImpl @Inject()(val beamConfig: BeamConfig) extends GeoUtils { diff --git a/src/main/scala/beam/utils/GpxWriter.scala b/src/main/scala/beam/utils/GpxWriter.scala deleted file mode 100644 index 82d8acea638..00000000000 --- a/src/main/scala/beam/utils/GpxWriter.scala +++ /dev/null @@ -1,36 +0,0 @@ -package beam.utils -import org.matsim.api.core.v01.Coord -import org.matsim.core.utils.io.IOUtils - -case class GpxPoint(name: String, wgsCoord: Coord) - -object GpxWriter { - - def write(filePath: String, points: Iterable[GpxPoint]): Unit = { - val outWriter = IOUtils.getBufferedWriter(filePath) - outWriter.write( - """ - |""".stripMargin - ) - try { - points.foreach { - case point => - val longitude = point.wgsCoord.getX - val latitude = point.wgsCoord.getY - val name = point.name - outWriter.write(s"""""") - outWriter.newLine() - outWriter.write(s"""$name""") - outWriter.newLine() - outWriter.write("") - outWriter.newLine() - } - } finally { - outWriter.write("") - - outWriter.flush() - outWriter.close() - } - } - -} diff --git a/src/main/scala/beam/utils/map/EnvelopeToGpx.scala b/src/main/scala/beam/utils/map/EnvelopeToGpx.scala new file mode 100644 index 00000000000..13c1f49b410 --- /dev/null +++ b/src/main/scala/beam/utils/map/EnvelopeToGpx.scala @@ -0,0 +1,69 @@ +package beam.utils.map + +import com.typesafe.scalalogging.LazyLogging +import com.vividsolutions.jts.geom.Envelope +import org.matsim.api.core.v01.Coord + +class EnvelopeToGpx extends LazyLogging { + + val geoUtils = new beam.sim.common.GeoUtils { + override def localCRS: String = "epsg:26910" + } + + def render(envelope: Envelope, wgsCoord: Coord, outputPath: String): Unit = { + val start = System.currentTimeMillis() + + // We have min(x0, y0) and max(x1,y1). Need to add two extra points to draw rectangle + /* + min => x0,y0 + max => x1,y1 +x0,y1 .___________. x1,y1 + | | + | | + | | + | | +x0,y0 .___________. x1, y0 + */ + + val envelopePoints = Array[GpxPoint]( + GpxPoint("x0,y0", new Coord(envelope.getMinX, envelope.getMinY)), + GpxPoint("x0,y1", new Coord(envelope.getMinX, envelope.getMaxY)), + GpxPoint("x1,y1", new Coord(envelope.getMaxX, envelope.getMaxY)), + GpxPoint("x1,y0", new Coord(envelope.getMaxX, envelope.getMinY)) + ) + + val gpxWriter = new GpxWriter(outputPath, geoUtils) + try { + + val middle = GpxPoint( + "Middle", + new Coord((envelope.getMinX + envelope.getMaxX) / 2, (envelope.getMinY + envelope.getMaxY) / 2) + ) + gpxWriter.drawMarker(middle) + val searchPoint = GpxPoint("Search", wgsCoord) + gpxWriter.drawMarker(searchPoint) + gpxWriter.drawSourceToDest(middle, searchPoint) + + envelopePoints.foreach(point => gpxWriter.drawMarker(point)) + + gpxWriter.drawRectangle(envelopePoints) + + } finally { + gpxWriter.close() + } + val end = System.currentTimeMillis() + logger.info(s"Created '$outputPath' in ${end - start} ms") + } +} + +object EnvelopeToGpx { + def longitude(coord: Coord): Double = coord.getX + + def latitude(coord: Coord): Double = coord.getY + + def main(args: Array[String]): Unit = { + val en1 = new Envelope(-122.5447336, -122.3592068, 37.6989794, 37.843628) + val envelopeToGpx = new EnvelopeToGpx + envelopeToGpx.render(en1, new Coord(-123.180062255, 38.7728279981), "ex1.gpx") + } +} diff --git a/src/main/scala/beam/utils/GeoJsonToGpxConvertor.scala b/src/main/scala/beam/utils/map/GeoJsonToGpxConvertor.scala similarity index 95% rename from src/main/scala/beam/utils/GeoJsonToGpxConvertor.scala rename to src/main/scala/beam/utils/map/GeoJsonToGpxConvertor.scala index e0d6d72d2f0..ea5aac0d906 100644 --- a/src/main/scala/beam/utils/GeoJsonToGpxConvertor.scala +++ b/src/main/scala/beam/utils/map/GeoJsonToGpxConvertor.scala @@ -1,4 +1,6 @@ -package beam.utils +package beam.utils.map + +import beam.utils.GeoJsonReader import com.typesafe.scalalogging.LazyLogging import com.vividsolutions.jts.geom.Geometry import org.matsim.api.core.v01.Coord diff --git a/src/main/scala/beam/utils/map/GpxWriter.scala b/src/main/scala/beam/utils/map/GpxWriter.scala new file mode 100644 index 00000000000..4890f81d06e --- /dev/null +++ b/src/main/scala/beam/utils/map/GpxWriter.scala @@ -0,0 +1,124 @@ +package beam.utils.map + +import java.io.BufferedWriter + +import beam.sim.common.GeoUtils +import beam.utils.map.EnvelopeToGpx.{latitude, longitude} +import org.matsim.api.core.v01.Coord +import org.matsim.core.utils.io.IOUtils + +import scala.util.Try + +case class GpxPoint(name: String, wgsCoord: Coord) + +class GpxWriter(filePath: String, geoUtils: GeoUtils) extends AutoCloseable { + + import GpxWriter._ + + val outWriter: BufferedWriter = { + val writer = IOUtils.getBufferedWriter(filePath) + writer.write(XML_HEADER) + writer.newLine() + + writer.write(GPX_START) + writer.newLine() + writer + } + + def drawRectangle(envelopePoints: Array[GpxPoint]): Unit = { + GpxWriter.drawRectangle(outWriter, geoUtils, envelopePoints) + } + + def drawMarker(point: GpxPoint): Unit = { + GpxWriter.drawMarker(outWriter, point) + } + + def drawSourceToDest(source: GpxPoint, dest: GpxPoint): Unit = { + GpxWriter.drawSourceToDest(outWriter, geoUtils, source, dest) + } + + override def close(): Unit = { + Try(outWriter.write(GPX_END)) + outWriter.flush() + outWriter.close() + } +} + +object GpxWriter { + val XML_HEADER: String = """""" + + val GPX_START: String = + """""" + + val GPX_END: String = "" + + def write(filePath: String, points: Iterable[GpxPoint]): Unit = { + val outWriter = IOUtils.getBufferedWriter(filePath) + outWriter.write(XML_HEADER) + outWriter.newLine() + + outWriter.write(GPX_START) + outWriter.newLine() + + try { + points.foreach { + case point => + val longitude = point.wgsCoord.getX + val latitude = point.wgsCoord.getY + val name = point.name + outWriter.write(s"""""") + outWriter.newLine() + outWriter.write(s"""$name""") + outWriter.newLine() + outWriter.write("") + outWriter.newLine() + } + } finally { + outWriter.write(GPX_END) + + outWriter.flush() + outWriter.close() + } + } + + def drawRectangle(outWriter: BufferedWriter, geoUtils: GeoUtils, envelopePoints: Array[GpxPoint]): Unit = { + val p = envelopePoints.sliding(2).toArray :+ Array(envelopePoints(0), envelopePoints(3)) + + p.foreach { + case p => + val source = p(0) + val dest = p(1) + drawSourceToDest(outWriter, geoUtils, source, dest) + } + } + + def drawMarker(outWriter: BufferedWriter, point: GpxPoint): Unit = { + outWriter.write(s"""""") + outWriter.newLine() + outWriter.write(s"""${point.name}""") + outWriter.newLine() + outWriter.write("") + outWriter.newLine() + } + + def drawSourceToDest(outWriter: BufferedWriter, geoUtils: GeoUtils, source: GpxPoint, dest: GpxPoint): Unit = { + outWriter.write(s"""""") + outWriter.newLine() + outWriter.write( + s"""Src""" + ) + outWriter.newLine() + outWriter.write( + s"""Dest""" + ) + outWriter.newLine() + + val distanceUTM = geoUtils.distUTMInMeters(geoUtils.wgs2Utm(source.wgsCoord), geoUtils.wgs2Utm(dest.wgsCoord)) + + outWriter.write(s"""Distance: $distanceUTM""") + outWriter.newLine() + outWriter.write("") + outWriter.newLine() + } + +} diff --git a/src/main/scala/beam/utils/LinkIdsToGpx.scala b/src/main/scala/beam/utils/map/LinkIdsToGpx.scala similarity index 98% rename from src/main/scala/beam/utils/LinkIdsToGpx.scala rename to src/main/scala/beam/utils/map/LinkIdsToGpx.scala index b5d94f82a9f..dc5970de8bf 100644 --- a/src/main/scala/beam/utils/LinkIdsToGpx.scala +++ b/src/main/scala/beam/utils/map/LinkIdsToGpx.scala @@ -1,4 +1,4 @@ -package beam.utils +package beam.utils.map import org.matsim.api.core.v01.network.Link import org.matsim.api.core.v01.{BasicLocation, Id} diff --git a/src/main/scala/beam/utils/map/R5NetworkPlayground.scala b/src/main/scala/beam/utils/map/R5NetworkPlayground.scala new file mode 100644 index 00000000000..fabee507bb9 --- /dev/null +++ b/src/main/scala/beam/utils/map/R5NetworkPlayground.scala @@ -0,0 +1,50 @@ +package beam.utils.map + +import beam.router.r5.DefaultNetworkCoordinator +import beam.sim.BeamHelper +import beam.sim.common.EdgeWithCoord +import beam.sim.config.BeamConfig +import org.matsim.api.core.v01.Coord + +object R5NetworkPlayground extends BeamHelper { + + def main(args: Array[String]): Unit = { + val (arg, cfg) = prepareConfig(Array("--config", "production/sfbay/smart/smart-c-hightech.conf"), true) + val beamConfig = BeamConfig(cfg) + + val nc = new DefaultNetworkCoordinator(beamConfig) + nc.init() + + val geoUtils = new beam.sim.common.GeoUtils { + override def localCRS: String = "epsg:26910" + } + + val transportNetwork = nc.transportNetwork + + // split is null + geoUtils.getNearestR5Edge(transportNetwork.streetLayer, new Coord(-123.358396043, 38.7670573007)) + // split is null + geoUtils.getNearestR5Edge(transportNetwork.streetLayer, new Coord(-123.180062255, 38.7728279981)) + + val gpxWriter = new GpxWriter("corners_production.gpx", geoUtils) + + val r: Array[(EdgeWithCoord, GpxPoint)] = geoUtils.getEdgesCloseToBoundingBox(transportNetwork.streetLayer) + val corners = r.map { case (_, gpxPoint) => gpxPoint } + try { + corners.foreach(point => gpxWriter.drawMarker(point)) + // First 4 points reprecent LEFT, TOP, RIGHT, BOTTOM coordinates + gpxWriter.drawRectangle(corners.take(4)) + + r.foreach { + case (edgeWithCoord, cornerPoint) => + val point = GpxPoint( + s"${edgeWithCoord.edgeIndex}_${cornerPoint.name}", + new Coord(edgeWithCoord.wgsCoord.x, edgeWithCoord.wgsCoord.y) + ) + gpxWriter.drawMarker(point) + } + } finally { + gpxWriter.close() + } + } +}