/*
* Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
*
* This file is part of PeerfactSim.KOM.
*
* PeerfactSim.KOM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* PeerfactSim.KOM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PeerfactSim.KOM. If not, see .
*
*/
package de.tud.kom.p2psim.impl.topology.movement.local;
import java.util.HashMap;
import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Locale;
import java.util.Map;
import java.util.Random;
import java.util.UUID;
import com.graphhopper.GHRequest;
import com.graphhopper.GHResponse;
import com.graphhopper.GraphHopper;
import com.graphhopper.PathWrapper;
import com.graphhopper.reader.osm.GraphHopperOSM;
import com.graphhopper.routing.util.EdgeFilter;
import com.graphhopper.routing.util.EncodingManager;
import com.graphhopper.routing.util.TraversalMode;
import com.graphhopper.routing.weighting.BlockAreaWeighting;
import com.graphhopper.storage.index.LocationIndex;
import com.graphhopper.storage.index.QueryResult;
import com.graphhopper.util.DistanceCalc2D;
import com.graphhopper.util.EdgeIteratorState;
import com.graphhopper.util.Instruction;
import com.graphhopper.util.InstructionList;
import com.graphhopper.util.Parameters;
import com.graphhopper.util.Parameters.Routing;
import com.graphhopper.util.PointList;
import com.graphhopper.util.shapes.GHPoint;
import de.tud.kom.p2psim.api.topology.Topology;
import de.tud.kom.p2psim.api.topology.movement.SimLocationActuator;
import de.tud.kom.p2psim.impl.topology.movement.modularosm.GPSCalculation;
import de.tud.kom.p2psim.impl.topology.movement.modularosm.ModularMovementModelViz;
import de.tud.kom.p2psim.impl.topology.movement.modularosm.RouteSensorComponent;
import de.tud.kom.p2psim.impl.topology.movement.modularosm.attraction.RandomAttractionGenerator;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tud.kom.p2psim.impl.util.Either;
import de.tud.kom.p2psim.impl.util.Left;
import de.tudarmstadt.maki.simonstrator.api.Binder;
import de.tudarmstadt.maki.simonstrator.api.Monitor;
import de.tudarmstadt.maki.simonstrator.api.Randoms;
import de.tudarmstadt.maki.simonstrator.api.Monitor.Level;
import de.tudarmstadt.maki.simonstrator.api.component.ComponentNotAvailableException;
import de.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;
/**
* This movement strategy uses the data from osm and navigates the nodes throught streets to the destination
*
* 13.03.2017 Clemens Krug: Fixed a bug. When the GraphHopper routing had errors the nodes would move to the
* top right corner and not, as intended, straight to their destination.
*
* @author Martin Hellwig
* @version 1.0, 07.07.2015
*
* - added a possibility to allow the usage of alternative routes
* - set the probability for this alternatives
* - allow a dynamic reload of graphhopper files when switching routing algorithms
* - allow to define blocked areas, that are not used for routing
*
* @author Julian Zobel
* @version 02.04.2020
*
*/
public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
private Random random = Randoms.getRandom(RealWorldStreetsMovement.class);
private static PositionVector worldDimensions;
private GraphHopper hopper;
private LocationIndex index;
private boolean init = false;
private static HashMap currentRoutes = new HashMap<>();
private Map routeSensorComponents = new LinkedHashMap<>();
private String osmFileLocation; //use pbf-format, because osm-format causes problems (xml-problems)
private String graphFolderFiles;
private String movementType; //car, bike or foot
private String defaultMovement;
private String navigationalType; //fastest,
private String blockedAreas; // default null
private static double latLower; //Values from -90 to 90; always smaller than latUpper
private static double latUpper; //Values from -90 to 90
private static double lonLeft; //Values from -180 to 180; Always smaller than lonRight
private static double lonRight; //Values from -180 to 180
private boolean uniqueFolders;
private boolean allowAlternativeRoutes = false;
private double probabilityForAlternativeRoute = 0.0;
/**
* Tolerance in meters (if the node reached a waypoint up to "tolerance"
* meters, it will select the next waypoint in the path.
*/
private double tolerance = 1;
@XMLConfigurableConstructor({"blockedAreas"})
public RealWorldStreetsMovement(String blockedAreas) {
this();
this.setBlockedAreas(blockedAreas);
}
public RealWorldStreetsMovement() {
worldDimensions = Binder.getComponentOrNull(Topology.class)
.getWorldDimensions();
latLower = GPSCalculation.getLatLower();
latUpper = GPSCalculation.getLatUpper();
lonLeft = GPSCalculation.getLonLeft();
lonRight = GPSCalculation.getLonRight();
}
private void init() {
hopper = new GraphHopperOSM().forServer();
hopper.setDataReaderFile(osmFileLocation);
// where to store graphhopper files?
if (uniqueFolders) {
Monitor.log(RealWorldStreetsMovement.class, Level.WARN,
"Using per-simulation unique folders for GraphHopper temporary data in %s. Remember to delete them to prevent your disk from filling up.",
graphFolderFiles);
hopper.setGraphHopperLocation(graphFolderFiles + "/"
+ UUID.randomUUID().toString());
} else {
hopper.setGraphHopperLocation(graphFolderFiles + "/"
+ osmFileLocation.hashCode() + movementType);
}
hopper.setEncodingManager(EncodingManager.create(movementType));
// if we have blocked areas defined, the CH in Graphhopper needs to be disabled!
// this also requires a new import
if((blockedAreas != null && !blockedAreas.isEmpty()) || allowAlternativeRoutes) {
hopper.setCHEnabled(false);
}
// try to load a cached import. If the cached import was with a different CH_ENABLED setting then now specified, this will fail.
// in this case, clean cache and re-import.
try {
hopper.importOrLoad();
} catch (Exception e) {
System.err.println("[RealWorldStreetMovement] GraphHopper graph file must be imported again!");
hopper.clean();
hopper.importOrLoad();
}
index = hopper.getLocationIndex();
init = true;
}
@Override
public Either nextPosition(
SimLocationActuator comp, PositionVector destination) {
return nextPosition(comp, destination, defaultMovement);
}
public Either nextPosition(SimLocationActuator comp,
PositionVector destination, String movementType) {
if (movementType == null || movementType.equals("")
|| !this.movementType.contains(movementType)) {
throw new AssertionError("Invalid movement type: " + movementType);
}
if (!init) {
init();
}
RouteSensorComponent routeSensor = routeSensorComponents.get(comp);
if (routeSensor == null) {
try {
routeSensorComponents.put(comp, comp.getHost()
.getComponent(RouteSensorComponent.class));
routeSensor = routeSensorComponents.get(comp);
} catch (ComponentNotAvailableException e) {
throw new AssertionError(
"The movement model needs to provide RouteSensorComponents.");
}
}
PositionVector newPosition = null;
if (destination
.distanceTo(comp.getRealPosition()) > getMovementSpeed(comp)) {
//if not set already for this node or new destination is different than last one
RouteImpl trajectory = currentRoutes.get(comp);
if(trajectory == null || destination.distanceTo(trajectory.getDestination()) > 1.0) {
double[] startPosition = transformOwnWorldWindowToGPS(comp.getRealPosition().getX(), comp.getRealPosition().getY());
double[] destinationPosition = transformOwnWorldWindowToGPS(destination.getX(), destination.getY());
GHRequest req = new GHRequest(startPosition[0], startPosition[1], destinationPosition[0], destinationPosition[1]).
setWeighting(navigationalType).
setVehicle(movementType).
setLocale(Locale.GERMANY);
// blocks one or multiple given areas from being used by the routing
// https://github.com/graphhopper/graphhopper/blob/master/docs/web/api-doc.md#flexible
if(blockedAreas != null && !blockedAreas.isEmpty()) {
req.getHints().put(Routing.BLOCK_AREA, blockedAreas);
}
if(allowAlternativeRoutes) {
// see: https://discuss.graphhopper.com/t/alternative-routes/424/11
req.setAlgorithm(Parameters.Algorithms.ALT_ROUTE);
req.getHints().put(Parameters.Algorithms.AltRoute.MAX_WEIGHT, "2.0"); // how much longer can alternatives be?
req.getHints().put(Parameters.Algorithms.AltRoute.MAX_PATHS, "5"); // how much alternatives should be returned at max?
}
GHResponse rsp = hopper.route(req);
//If the requested point is not in the map data, simple return the destination as next point
if(rsp.hasErrors()) {
Monitor.log(this.getClass(), Monitor.Level.ERROR, "Routing request for Host %s with starting point (" +
"%f,%f), destination (%f,%f) and type %s failed with error: %s.", comp.getHost().getId().valueAsString(),startPosition[0], startPosition[1],
destinationPosition[0], destinationPosition[1], movementType, rsp.getErrors());
PointList pointList = new PointList();
pointList.add(new GHPoint(destinationPosition[0], destinationPosition[1]));
trajectory = new RouteImpl(comp.getRealPosition(), destination, pointList);
currentRoutes.put(comp, trajectory);
} else {
PointList pointList = rsp.getBest().getPoints();
// alternatives for routing are allowed for distances bigger than 50
if(allowAlternativeRoutes && rsp.hasAlternatives() && rsp.getBest().getDistance() > 50) {
// alternative route is taken with a certain chance
if(random.nextDouble() <= probabilityForAlternativeRoute) {
List paths = rsp.getAll();
// remove the best, we do not want this!
paths.remove(rsp.getBest());
PathWrapper choice;
if(paths.size() == 1) {
choice = paths.get(0);
}
else {
choice = paths.get(random.nextInt(paths.size() - 1) + 1);
}
pointList = choice.getPoints();
}
}
if (isCalculateRouteSegments()) {
/*
* Obtain segment IDs along the route.
*/
trajectory = new RouteImpl(comp.getRealPosition(),
destination, pointList,
routeSensor.getSegmentListeners(), routeSensor,
calculateSegments(pointList));
} else {
trajectory = new RouteImpl(comp.getRealPosition(),
destination, pointList);
}
currentRoutes.put(comp, trajectory);
}
routeSensor.setNewRoute(trajectory);
}
newPosition = trajectory.updateCurrentLocation(comp, getMovementSpeed(comp), tolerance);
if (trajectory.reachedDestination()) {
routeSensor.reachedDestination();
}
}
return new Left(newPosition);
}
/**
* Calculate RouteSegments using the PathWrapper
* @param pathWrapper
* @return
*/
private List calculateSegments(PathWrapper pathWrapper) {
InstructionList instructions = pathWrapper.getInstructions();
int ptIdx = 0;
List segments = new LinkedList<>();
RouteSegmentImpl lastSegment = null;
for (Instruction instr : instructions) {
RouteSegmentImpl segment = null;
PointList points = instr.getPoints();
String name = instr.getName();
if (name.isEmpty()) {
name = Integer.toString(instr.getPoints().get(0).hashCode());
//name = Integer.toString(instr.getPoints().toGHPoint(0).hashCode()); @deprecated for old GraphHopper version
}
for (GHPoint point : points) {
if (segment == null) {
if (lastSegment != null) {
lastSegment.addRouteLocation(transformGPSWindowToOwnWorld(
point.getLat(), point.getLon()));
}
segment = new RouteSegmentImpl(name, ptIdx, transformGPSWindowToOwnWorld(point.getLat(),
point.getLon()));
} else {
segment.addRouteLocation(transformGPSWindowToOwnWorld(
point.getLat(), point.getLon()));
}
ptIdx++;
}
lastSegment = segment;
segments.add(segment);
}
return segments;
}
/**
* Calculate the route segments along the given point list.
*
* @return
*/
private List calculateSegments(PointList pointList) {
int ptIdx = 0;
int prevEdgeId = -1;
List segments = new LinkedList<>();
RouteSegmentImpl lastSegment = null;
for (GHPoint point : pointList) {
QueryResult qr = index.findClosest(point.getLat(), point.getLon(),
EdgeFilter.ALL_EDGES);
EdgeIteratorState nearEdge = qr.getClosestEdge();
if (nearEdge != null) {
int edgeId = nearEdge.getEdge();
if (edgeId != prevEdgeId) {
if (lastSegment != null) {
lastSegment.addRouteLocation(
transformGPSWindowToOwnWorld(point.getLat(),
point.getLon()));
}
lastSegment = new RouteSegmentImpl(Integer.toString(edgeId),
ptIdx, transformGPSWindowToOwnWorld(point.getLat(),
point.getLon()));
segments.add(lastSegment);
} else {
lastSegment.addRouteLocation(transformGPSWindowToOwnWorld(
point.getLat(), point.getLon()));
}
}
ptIdx++;
}
return segments;
}
/**
* Projects the world coordinates in the given gps window to the
* gps-coordinates
*
* @param x
* @param y
* @return The projected position in gps-coordinates (lat, long)
*/
private double[] transformOwnWorldWindowToGPS(double x, double y) {
double[] gps_coordinates = new double[2];
gps_coordinates[0] = latLower + (latUpper - latLower) * (worldDimensions.getY() - y)/worldDimensions.getY();
gps_coordinates[1] = lonLeft + (lonRight - lonLeft) * x/worldDimensions.getX();
return gps_coordinates;
}
/**
* Projects the gps coordinates in the given gps window to the world-coordinates given in world-dimensions
* @param lat
* @param lon
* @return The projected position in world-dimensions
*/
public static PositionVector transformGPSWindowToOwnWorld(double lat, double lon) {
double x = worldDimensions.getX() * (lon - lonLeft)/(lonRight - lonLeft);
double y = worldDimensions.getY() - worldDimensions.getY() * (lat - latLower)/(latUpper - latLower);
x = Math.max(0, x);
x = Math.min(worldDimensions.getX(), x);
y = Math.max(0, y);
y = Math.min(worldDimensions.getY(), y);
return new PositionVector(x, y);
}
/**
* Returns a list of points representing the current route of the component. Points are
* in x / y values of the own world.
* @param ms the component
* @return list of movement points.
*/
public List getMovementPoints(SimLocationActuator ms)
{
List positions = new LinkedList<>();
PointList pointList = currentRoutes.get(ms).getPointList();
pointList.forEach(p -> positions.add(new PositionVector(transformGPSWindowToOwnWorld(p.getLat(), p.getLon()))));
return positions;
}
/**
* Calculates the length of a route in meters.
* @param start Starting position in own world coordinates (x / y)
* @param destination Destination on own world coordinates (x / y)
* @return the length of the route in meters.
*/
public double calculateRouteLength(PositionVector start, PositionVector destination)
{
PointList pointList;
double[] startPosition = transformOwnWorldWindowToGPS(start.getX(), start.getY());
double[] destinationPosition = transformOwnWorldWindowToGPS(destination.getX(), destination.getY());
GHRequest req = new GHRequest(startPosition[0], startPosition[1], destinationPosition[0], destinationPosition[1]).
setWeighting(navigationalType).
setVehicle(movementType).
setLocale(Locale.GERMANY);
GHResponse rsp = hopper.route(req);
//If the requested point is not in the map data, return -1
if(rsp.hasErrors()) {
return -1;
}
else {
pointList = rsp.getBest().getPoints();
return pointList.calcDistance(new DistanceCalc2D());
}
}
/**
* Calculates the length of the current route of the SimLocationActuator.
* @param ms the component
* @return the length of the current route
*/
public double getCurrentRouteLength(SimLocationActuator ms)
{
return currentRoutes.get(ms).getPointList().calcDistance(new DistanceCalc2D());
}
public void setOsmFileLocation(String osmFileLocation) {
this.osmFileLocation = osmFileLocation;
}
public void setGraphFolderFiles(String graphFolderFiles) {
this.graphFolderFiles = graphFolderFiles;
}
public void setMovementType(String movementType) {
this.movementType = movementType;
defaultMovement = movementType.split(",")[0];
}
public void setNavigationalType(String navigationalType) {
this.navigationalType = navigationalType;
}
public void setWaypointTolerance(double tolerance) {
this.tolerance = tolerance;
}
/**
* For large batch simulations, we need to prevent same-time access to
* garphhopper temp data. Therefore, this flag creates unique folders for
* each run (which, obviously, wastes a lot of space and comp-resources and
* should not be used in standalone, single-threaded demo mode...)
*
* @param uniqueFolders
*/
public void setCreateUniqueFolders(boolean uniqueFolders) {
this.uniqueFolders = uniqueFolders;
}
public void setBlockedAreas(String blockedAreas) {
if(!blockedAreas.isEmpty()) {
this.blockedAreas = blockedAreas;
}
}
public String getBlockedAreas() {
return this.blockedAreas;
}
public void setAllowAlternativeRoutes(boolean allowAlternativeRoutes) {
this.allowAlternativeRoutes = allowAlternativeRoutes;
}
public void setProbabilityForAlternativeRoute(double p) {
this.probabilityForAlternativeRoute = p;
}
}