Add U-Turns (graphhopper#1073)

* Added heading for first continue

* Added U-turn for contextual information

* Fixed angle calculation issue

* Added translations fo de+en

* Angle improvement

* Updated U_TURN translation and added it to the frontend

* Updated translation files

* Recognize U-turns over an additional edge

* Updated max distance

* Fixed Typo

* Removed comment

* Updated Translations

* Changed Orientation Diff

* Tmp Commit

* Improved the U-Turn recognition

* Limit U-turns to one-ways

* Fixed some checkstyle warnings

* Propose U-Turn left/right differentiation

* Merge main.js

Author: boldtrn

client-hc/src/main/java/com/graphhopper/api/GraphHopperWeb.java

@@ -174,6 +174,11 @@ PathWrapper createPathWrapper(JsonNode path,
                         instr = new FinishInstruction(text, instPL, 0);
                     } else {
                         instr = new Instruction(sign, text, ia, instPL);
+                        if(sign == Instruction.CONTINUE_ON_STREET){
+                            if(jsonObj.has("heading")){
+                                instr.setExtraInfo("heading", jsonObj.get("heading").asDouble());
+                            }
+                        }
                     }
 
                     // Usually, the translation is done from the routing service so just use the provided string

core/src/main/java/com/graphhopper/GraphHopper.java

@@ -1074,6 +1074,9 @@ else if (ALT_ROUTE.equalsIgnoreCase(algoStr))
                         setPathDetailsBuilders(pathBuilderFactory, request.getPathDetails()).
                         setSimplifyResponse(simplifyResponse && wayPointMaxDistance > 0);
 
+                if(request.hasFavoredHeading(0))
+                    pathMerger.setFavoredHeading(request.getFavoredHeading(0));
+
                 if (routingTemplate.isReady(pathMerger, tr))
                     break;
             }

core/src/main/java/com/graphhopper/routing/InstructionsFromEdges.java

@@ -66,13 +66,17 @@ public class InstructionsFromEdges implements Path.EdgeVisitor {
     private double doublePrevLat, doublePrevLon; // Lat and Lon of node t-2
     private int prevNode;
     private double prevOrientation;
+    private double prevInstructionPrevOrientation = Double.NaN;
     private Instruction prevInstruction;
     private boolean prevInRoundabout;
     private String prevName;
+    private String prevInstructionName;
     private InstructionAnnotation prevAnnotation;
     private EdgeExplorer outEdgeExplorer;
     private EdgeExplorer crossingExplorer;
 
+    private final int MAX_U_TURN_DISTANCE = 35;
+
     public InstructionsFromEdges(int tmpNode, Graph graph, Weighting weighting, FlagEncoder encoder, NodeAccess nodeAccess, Translation tr, InstructionList ways) {
         this.weighting = weighting;
         this.encoder = encoder;
@@ -119,6 +123,10 @@ public void next(EdgeIteratorState edge, int index, int prevEdgeId) {
         {
             int sign = Instruction.CONTINUE_ON_STREET;
             prevInstruction = new Instruction(sign, name, annotation, new PointList(10, nodeAccess.is3D()));
+            double startLat = nodeAccess.getLat(baseNode);
+            double startLon = nodeAccess.getLon(baseNode);
+            double heading = Helper.ANGLE_CALC.calcAzimuth(startLat, startLon, latitude, longitude);
+            prevInstruction.setExtraInfo("heading", Helper.round(heading, 2));
             ways.add(prevInstruction);
             prevName = name;
             prevAnnotation = annotation;
@@ -130,6 +138,7 @@ public void next(EdgeIteratorState edge, int index, int prevEdgeId) {
                 int sign = Instruction.USE_ROUNDABOUT;
                 RoundaboutInstruction roundaboutInstruction = new RoundaboutInstruction(sign, name,
                         annotation, new PointList(10, nodeAccess.is3D()));
+                prevInstructionPrevOrientation = prevOrientation;
                 if (prevName != null) {
                     // check if there is an exit at the same node the roundabout was entered
                     EdgeIterator edgeIter = outEdgeExplorer.setBaseNode(baseNode);
@@ -192,16 +201,64 @@ public void next(EdgeIteratorState edge, int index, int prevEdgeId) {
                     .setDirOfRotation(deltaOut)
                     .setExited();
 
+            prevInstructionName = prevName;
             prevName = name;
             prevAnnotation = annotation;
 
         } else {
             int sign = getTurn(edge, baseNode, prevNode, adjNode, annotation, name);
 
             if (sign != Instruction.IGNORE) {
-                prevInstruction = new Instruction(sign, name, annotation, new PointList(10, nodeAccess.is3D()));
-                ways.add(prevInstruction);
-                prevAnnotation = annotation;
+                /*
+                    Check if the next instruction is likely to only be a short connector to execute a u-turn
+                    --A->--
+                           |    <-- This is the short connector
+                    --B-<--
+                    Road A and Road B have to have the same name and roughly the same, but opposite orientation, otherwise we are assuming this is no u-turn.
+
+                    Note: This approach only works if there a turn instruction fro A->Connector and Connector->B.
+                    Currently we don't create a turn instruction if there is no other possible turn
+                    We only create a u-turn if edge B is a one-way, see #1073 for more details.
+                  */
+
+                boolean isUTurn = false;
+                int uTurnType = Instruction.U_TURN_UNKNOWN;
+                if (!Double.isNaN(prevInstructionPrevOrientation)
+                        && prevInstruction.getDistance() < MAX_U_TURN_DISTANCE
+                        && (sign < 0) == (prevInstruction.getSign() < 0)
+                        && (Math.abs(sign) == Instruction.TURN_SLIGHT_RIGHT || Math.abs(sign) == Instruction.TURN_RIGHT || Math.abs(sign) == Instruction.TURN_SHARP_RIGHT)
+                        && (Math.abs(prevInstruction.getSign()) == Instruction.TURN_SLIGHT_RIGHT || Math.abs(prevInstruction.getSign()) == Instruction.TURN_RIGHT || Math.abs(prevInstruction.getSign()) == Instruction.TURN_SHARP_RIGHT)
+                        && edge.isForward(encoder) != edge.isBackward(encoder)
+                        && InstructionsHelper.isNameSimilar(prevInstructionName, name)) {
+                    // Chances are good that this is a u-turn, we only need to check if the orientation matches
+                    GHPoint point = InstructionsHelper.getPointForOrientationCalculation(edge, nodeAccess);
+                    double lat = point.getLat();
+                    double lon = point.getLon();
+                    double currentOrientation = Helper.ANGLE_CALC.calcOrientation(prevLat, prevLon, lat, lon, false);
+
+                    double diff = Math.abs(prevInstructionPrevOrientation - currentOrientation);
+                    if (diff > (Math.PI * .9) && diff < (Math.PI * 1.1)) {
+                        isUTurn = true;
+                        if (sign < 0) {
+                            uTurnType = Instruction.U_TURN_LEFT;
+                        } else {
+                            uTurnType = Instruction.U_TURN_RIGHT;
+                        }
+                    }
+
+                }
+
+                if (isUTurn) {
+                    prevInstruction.setSign(uTurnType);
+                    prevInstruction.setName(name);
+                } else {
+                    prevInstruction = new Instruction(sign, name, annotation, new PointList(10, nodeAccess.is3D()));
+                    // Remember the Orientation and name of the road, before doing this maneuver
+                    prevInstructionPrevOrientation = prevOrientation;
+                    prevInstructionName = prevName;
+                    ways.add(prevInstruction);
+                    prevAnnotation = annotation;
+                }
             }
             // Updated the prevName, since we don't always create an instruction on name changes the previous
             // name can be an old name. This leads to incorrect turn instructions due to name changes
@@ -236,7 +293,11 @@ public void finish() {
             ((RoundaboutInstruction) prevInstruction).setRadian(delta);
 
         }
-        ways.add(new FinishInstruction(nodeAccess, prevEdge.getAdjNode()));
+
+        Instruction finishInstruction = new FinishInstruction(nodeAccess, prevEdge.getAdjNode());
+        // This is the heading how the edge ended
+        finishInstruction.setExtraInfo("last_heading", Helper.ANGLE_CALC.calcAzimuth(doublePrevLat, doublePrevLon, prevLat, prevLon));
+        ways.add(finishInstruction);
     }
 
     private int getTurn(EdgeIteratorState edge, int baseNode, int prevNode, int adjNode, InstructionAnnotation annotation, String name) {

core/src/main/java/com/graphhopper/routing/InstructionsHelper.java

@@ -39,9 +39,6 @@ static int calculateSign(double prevLatitude, double prevLongitude, double latit
         double delta = calculateOrientationDelta(prevLatitude, prevLongitude, latitude, longitude, prevOrientation);
         double absDelta = Math.abs(delta);
 
-        // TODO not only calculate the mathematical orientation, but also compare to other streets
-        // TODO If there is one street turning slight right and one right, but no straight street
-        // TODO We can assume the slight right street would be a continue
         if (absDelta < 0.2) {
             // 0.2 ~= 11°
             return Instruction.CONTINUE_ON_STREET;

core/src/main/java/com/graphhopper/util/AngleCalc.java

@@ -115,7 +115,7 @@ public double calcAzimuth(double lat1, double lon1, double lat2, double lon2) {
         if (orientation < 0)
             orientation += 2 * Math.PI;
 
-        return Math.toDegrees(Helper.round4(orientation))%360;
+        return Math.toDegrees(Helper.round4(orientation)) % 360;
     }
 
     String azimuth2compassPoint(double azimuth) {

core/src/main/java/com/graphhopper/util/Instruction.java

@@ -17,12 +17,14 @@
  */
 package com.graphhopper.util;
 
-import java.util.Collections;
+import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
 
 public class Instruction {
     public static final int UNKNOWN = -99;
+    public static final int U_TURN_UNKNOWN = -98;
+    public static final int U_TURN_LEFT = -8;
     public static final int KEEP_LEFT = -7;
     public static final int LEAVE_ROUNDABOUT = -6; // for future use
     public static final int TURN_SHARP_LEFT = -3;
@@ -37,6 +39,7 @@ public class Instruction {
     public static final int USE_ROUNDABOUT = 6;
     public static final int IGNORE = Integer.MIN_VALUE;
     public static final int KEEP_RIGHT = 7;
+    public static final int U_TURN_RIGHT = 8;
     public static final int PT_START_TRIP = 101;
     public static final int PT_TRANSFER = 102;
     public static final int PT_END_TRIP = 103;
@@ -48,6 +51,7 @@ public class Instruction {
     protected String name;
     protected double distance;
     protected long time;
+    protected Map<String, Object> extraInfo = new HashMap<>(3);
 
     /**
      * The points, distances and times have exactly the same count. The last point of this
@@ -79,6 +83,10 @@ public int getSign() {
         return sign;
     }
 
+    public void setSign(int sign) {
+        this.sign = sign;
+    }
+
     public String getName() {
         return name;
     }
@@ -88,11 +96,11 @@ public void setName(String name) {
     }
 
     public Map<String, Object> getExtraInfoJSON() {
-        return Collections.<String, Object>emptyMap();
+        return extraInfo;
     }
 
     public void setExtraInfo(String key, Object value) {
-        throw new IllegalArgumentException("Key" + key + " is not a valid option");
+        extraInfo.put(key, value);
     }
 
     /**
@@ -270,6 +278,15 @@ public String getTurnDescription(Translation tr) {
         } else {
             String dir = null;
             switch (indi) {
+                case Instruction.U_TURN_UNKNOWN:
+                    dir = tr.tr("u_turn");
+                    break;
+                case Instruction.U_TURN_LEFT:
+                    dir = tr.tr("u_turn");
+                    break;
+                case Instruction.U_TURN_RIGHT:
+                    dir = tr.tr("u_turn");
+                    break;
                 case Instruction.KEEP_LEFT:
                     dir = tr.tr("keep_left");
                     break;

core/src/main/java/com/graphhopper/util/PathMerger.java

@@ -48,6 +48,7 @@ public class PathMerger {
     private boolean calcPoints = true;
     private PathDetailsBuilderFactory pathBuilderFactory;
     private List<String> requestedPathDetails = Collections.EMPTY_LIST;
+    private double favoredHeading = Double.NaN;
 
     public PathMerger setCalcPoints(boolean calcPoints) {
         this.calcPoints = calcPoints;
@@ -135,8 +136,10 @@ public void doWork(PathWrapper altRsp, List<Path> paths, Translation tr) {
                 calcAscendDescend(altRsp, fullPoints);
         }
 
-        if (enableInstructions)
+        if (enableInstructions) {
+            fullInstructions = updateInstructionsWithContext(fullInstructions);
             altRsp.setInstructions(fullInstructions);
+        }
 
         if (!allFound) {
             altRsp.addError(new ConnectionNotFoundException("Connection between locations not found", Collections.<String, Object>emptyMap()));
@@ -173,6 +176,51 @@ public static void merge(List<PathDetail> pathDetails, List<PathDetail> otherDet
         pathDetails.addAll(otherDetails);
     }
 
+    /**
+     * This method iterates over all instructions and uses the available context to improve the instructions.
+     * If the requests contains a heading, this method can transform the first continue to a u-turn if the heading
+     * points into the opposite direction of the route.
+     * At a waypoint it can transform the continue to a u-turn if the route involves turning.
+     */
+    private InstructionList updateInstructionsWithContext(InstructionList instructions) {
+        Instruction instruction;
+        Instruction nextInstruction;
+
+        for (int i = 0; i < instructions.size() - 1; i++) {
+            instruction = instructions.get(i);
+
+            if (i == 0 && !Double.isNaN(favoredHeading) && instruction.extraInfo.containsKey("heading")) {
+                double heading = (double) instruction.extraInfo.get("heading");
+                double diff = Math.abs(heading - favoredHeading) % 360;
+                if (diff > 170 && diff < 190) {
+                    // The requested heading points into the opposite direction of the calculated heading
+                    // therefore we change the continue instruction to a u-turn
+                    instruction.setSign(Instruction.U_TURN_UNKNOWN);
+                }
+            }
+
+            if (instruction.getSign() == Instruction.REACHED_VIA) {
+                nextInstruction = instructions.get(i + 1);
+                if (nextInstruction.getSign() != Instruction.CONTINUE_ON_STREET
+                        || !instruction.extraInfo.containsKey("last_heading")
+                        || !nextInstruction.extraInfo.containsKey("heading")) {
+                    // TODO throw exception?
+                    continue;
+                }
+                double lastHeading = (double) instruction.extraInfo.get("last_heading");
+                double heading = (double) nextInstruction.extraInfo.get("heading");
+
+                // Since it's supposed to go back the same edge, we can be very strict with the diff
+                double diff = Math.abs(lastHeading - heading) % 360;
+                if (diff > 179 && diff < 181) {
+                    nextInstruction.setSign(Instruction.U_TURN_UNKNOWN);
+                }
+            }
+        }
+
+        return instructions;
+    }
+
     private void calcAscendDescend(final PathWrapper rsp, final PointList pointList) {
         double ascendMeters = 0;
         double descendMeters = 0;
@@ -192,4 +240,8 @@ private void calcAscendDescend(final PathWrapper rsp, final PointList pointList)
         rsp.setAscend(ascendMeters);
         rsp.setDescend(descendMeters);
     }
+
+    public void setFavoredHeading(double favoredHeading) {
+        this.favoredHeading = favoredHeading;
+    }
 }

core/src/main/java/com/graphhopper/util/ViaInstruction.java

@@ -31,6 +31,7 @@ public ViaInstruction(Instruction instr) {
         this(instr.getName(), instr.getAnnotation(), instr.getPoints());
         setDistance(instr.getDistance());
         setTime(instr.getTime());
+        this.extraInfo = instr.extraInfo;
     }
 
     @Override

core/src/test/java/com/graphhopper/routing/PathTest.java

@@ -597,6 +597,76 @@ public void testCalcInstructionSlightTurn() {
         assertEquals(-1, wayList.get(1).getSign());
     }
 
+    @Test
+    public void testUTurnLeft() {
+        final Graph g = new GraphBuilder(carManager).create();
+        final NodeAccess na = g.getNodeAccess();
+
+        // Real Situation: point=48.402116%2C9.994367&point=48.402198%2C9.99507
+        //       7
+        //       |
+        //  4----5----6
+        //       |
+        //  1----2----3
+        na.setNode(1, 48.402116, 9.994367);
+        na.setNode(2, 48.402198, 9.99507);
+        na.setNode(3, 48.402344, 9.996266);
+        na.setNode(4, 48.402191, 9.994351);
+        na.setNode(5, 48.402298, 9.995053);
+        na.setNode(6, 48.402422, 9.996067);
+        na.setNode(7, 48.402604, 9.994962);
+
+        g.edge(1, 2, 5, false).setName("Olgastraße");
+        g.edge(2, 3, 5, false).setName("Olgastraße");
+        g.edge(6, 5, 5, false).setName("Olgastraße");
+        g.edge(5, 4, 5, false).setName("Olgastraße");
+        g.edge(2, 5, 5, true).setName("Neithardtstraße");
+        g.edge(5, 7, 5, true).setName("Neithardtstraße");
+
+        Path p = new Dijkstra(g, new ShortestWeighting(encoder), TraversalMode.NODE_BASED)
+                .calcPath(1, 4);
+        assertTrue(p.isFound());
+        InstructionList wayList = p.calcInstructions(tr);
+
+        assertEquals(3, wayList.size());
+        assertEquals(Instruction.U_TURN_LEFT, wayList.get(1).getSign());
+    }
+
+    @Test
+    public void testUTurnRight() {
+        final Graph g = new GraphBuilder(carManager).create();
+        final NodeAccess na = g.getNodeAccess();
+
+        // Real Situation: point=-33.885758,151.181472&point=-33.885692,151.181445
+        //       7
+        //       |
+        //  4----5----6
+        //       |
+        //  3----2----1
+        na.setNode(1, -33.885758,151.181472);
+        na.setNode(2, -33.885852,151.180968);
+        na.setNode(3, -33.885968,151.180501);
+        na.setNode(4, -33.885883,151.180442);
+        na.setNode(5, -33.885772,151.180941);
+        na.setNode(6, -33.885692,151.181445);
+        na.setNode(7, -33.885692,151.181445);
+
+        g.edge(1, 2, 5, false).setName("Parramatta Road");
+        g.edge(2, 3, 5, false).setName("Parramatta Road");
+        g.edge(4, 5, 5, false).setName("Parramatta Road");
+        g.edge(5, 6, 5, false).setName("Parramatta Road");
+        g.edge(2, 5, 5, true).setName("Larkin Street");
+        g.edge(5, 7, 5, true).setName("Larkin Street");
+
+        Path p = new Dijkstra(g, new ShortestWeighting(encoder), TraversalMode.NODE_BASED)
+                .calcPath(1, 6);
+        assertTrue(p.isFound());
+        InstructionList wayList = p.calcInstructions(tr);
+
+        assertEquals(3, wayList.size());
+        assertEquals(Instruction.U_TURN_RIGHT, wayList.get(1).getSign());
+    }
+
     @Test
     public void testCalcInstructionsForTurn() {
         // The street turns left, but there is not turn