Add source phantom weight to first segment when merging legs (#4949)

Fix annotation values for annotations on edges where phantom nodes are snapped.

Author: danpat

CHANGELOG.md

@@ -1,3 +1,8 @@
+# 5.16.4
+  - Changes from 5.16.3:
+    - Bugfixes:
+      - FIXED: Properly calculate annotations for speeds, durations and distances when waypoints are used with mapmatching [#4949](https://github.com/Project-OSRM/osrm-backend/pull/4949)
+
 # 5.16.3
   - Changes from 5.16.2:
       - FIXED: Remove the last short annotation segment in `trimShortSegments` [#4946](https://github.com/Project-OSRM/osrm-backend/pull/4946)

features/testbot/matching.feature

@@ -718,3 +718,78 @@ Feature: Basic Map Matching
           | abe   | 1,0.99973,1.00027,0.99973,1.00027,1        | depart,turn left,arrive  | Ok   |
           | ahd   | 1,0.99973,1.00027,0.99973,1.00027,0.999461 | depart,turn right,arrive | Ok   |
           | ahe   | 1,0.99973,1.00027,0.99973,1.00027,1        | depart,turn left,arrive  | Ok   |
+
+    @match @testbot
+    Scenario: Regression test - add source phantoms properly (one phantom on one edge)
+        Given the profile "testbot"
+        Given a grid size of 10 meters
+        Given the node map
+          """
+          a--1-b2-cd3--e
+          """
+        And the ways
+          | nodes |
+          | ab    |
+          | bcd   |
+          | de    |
+        Given the query options
+          | geometries     | geojson         |
+          | overview       | full            |
+          | steps          | true            |
+          | waypoints      | 0;2             |
+          | annotations    | duration,weight |
+          | generate_hints | false           |
+        When I match I should get
+          | trace | geometry                                             | a:duration    | a:weight      | duration |
+          | 123   | 1.000135,1,1.000225,1,1.00036,1,1.000405,1,1.00045,1 | 1:1.5:0.5:0.5 | 1:1.5:0.5:0.5 | 3.5      |
+          | 321   | 1.00045,1,1.000405,1,1.00036,1,1.000225,1,1.000135,1 | 0.5:0.5:1.5:1 | 0.5:0.5:1.5:1 | 3.5      |
+
+    @match @testbot
+    Scenario: Regression test - add source phantom properly (two phantoms on one edge)
+        Given the profile "testbot"
+        Given a grid size of 10 meters
+        Given the node map
+          """
+          a--1-b23-c4--d
+          """
+        And the ways
+          | nodes |
+          | ab    |
+          | bc    |
+          | cd    |
+        Given the query options
+          | geometries     | geojson         |
+          | overview       | full            |
+          | steps          | true            |
+          | waypoints      | 0;3             |
+          | annotations    | duration,weight |
+          | generate_hints | false           |
+        When I match I should get
+          | trace | geometry                                   | a:duration | a:weight | duration |
+          | 1234  | 1.000135,1,1.000225,1,1.000405,1,1.00045,1 | 1:2:0.5    | 1:2:0.5  | 3.5      |
+          | 4321  | 1.00045,1,1.000405,1,1.000225,1,1.000135,1 | 0.5:2:1    | 0.5:2:1  | 3.5      |
+
+    @match @testbot
+    Scenario: Regression test - add source phantom properly (two phantoms on one edge)
+        Given the profile "testbot"
+        Given a grid size of 10 meters
+        Given the node map
+          """
+          a--12345-b
+          """
+        And the ways
+          | nodes |
+          | ab    |
+        Given the query options
+          | geometries     | geojson                  |
+          | overview       | full                     |
+          | steps          | true                     |
+          | waypoints      | 0;3                      |
+          | annotations    | duration,weight,distance |
+          | generate_hints | false                    |
+
+        # These should have the same weights/duration in either direction
+        When I match I should get
+          | trace | geometry             | a:distance | a:duration | a:weight | duration |
+          | 2345  | 1.00018,1,1.000315,1 | 15.013264  | 1.5        | 1.5      | 1.5      |
+          | 4321  | 1.00027,1,1.000135,1 | 15.013264  | 1.5        | 1.5      | 1.5      |

features/testbot/traffic_speeds.feature

@@ -136,13 +136,13 @@ Feature: Traffic - speeds
 
         When I route I should get
           | from | to | route    | speed   | weights       | a:datasources | a:speed | a:nodes|
-          | a    | b  | fb,fb    | 36 km/h | 329.4,0       | 0             | 7       | 6:2    |
+          | a    | b  | fb,fb    | 36 km/h | 329.4,0       | 0             | 10      | 6:2    |
           | a    | c  | fb,bc,bc | 30 km/h | 329.4,741.5,0 | 0:1           | 10:7.5  | 6:2:3  |
           | b    | c  | bc,bc    | 27 km/h | 741.5,0       | 1             | 7.5     | 2:3    |
           | a    | d  | fb,df,df | 36 km/h | 140,487.5,0   | 0:0           | 10:10   | 2:6:4  |
           | d    | c  | dc,dc    | 36 km/h | 956.8,0       | 0             | 10      | 4:3    |
-          | g    | b  | fb,fb    | 36 km/h | 164.7,0       | 0             | 3.5     | 6:2    |
-          | a    | g  | fb,fb    | 36 km/h | 164.7,0       | 0             | 5.4     | 6:2    |
+          | g    | b  | fb,fb    | 36 km/h | 164.7,0       | 0             | 10      | 6:2    |
+          | a    | g  | fb,fb    | 36 km/h | 164.7,0       | 0             | 10      | 6:2    |
 
 
     Scenario: Verify that negative values cause an error, they're not valid at all

features/testbot/weight.feature

@@ -53,8 +53,8 @@ Feature: Weight tests
 
         When I route I should get
             | waypoints | route   | distances | weights | times   | a:distance          | a:duration | a:weight | a:speed |
-            | s,t       | abc,abc | 20m,0m    | 2.1,0   | 2.1s,0s | 20.017685           | 3          | 3        | 6.7     |
-            | t,s       | abc,abc | 20m,0m    | 2.1,0   | 2.1s,0s | 20.017685           | 3.1        | 3.1      | 6.5     |
+            | s,t       | abc,abc | 20m,0m    | 2.1,0   | 2.1s,0s | 20.017685           | 2.1        | 2.1      | 9.5     |
+            | t,s       | abc,abc | 20m,0m    | 2.1,0   | 2.1s,0s | 20.017685           | 2.1        | 2.1      | 9.5     |
             | s,e       | abc,abc | 40m,0m    | 4.1,0   | 4.1s,0s | 30.026527:10.008842 | 3.1:1      | 3.1:1    | 9.7:10  |
             | e,s       | abc,abc | 40m,0m    | 4.1,0   | 4.1s,0s | 10.008842:30.026527 | 1:3.1      | 1:3.1    | 10:9.7  |
 

include/engine/guidance/assemble_geometry.hpp

@@ -12,6 +12,7 @@
 #include "util/coordinate_calculation.hpp"
 
 #include <algorithm>
+#include <cmath>
 #include <utility>
 #include <vector>
 
@@ -113,15 +114,39 @@ inline LegGeometry assembleGeometry(const datafacade::BaseDataFacade &facade,
     const std::vector<DatasourceID> forward_datasources =
         facade.GetUncompressedForwardDatasources(target_geometry_id);
 
-    // FIXME if source and target phantoms are on the same segment then duration and weight
-    // will be from one projected point till end of segment
-    // testbot/weight.feature:Start and target on the same and adjacent edge
-    geometry.annotations.emplace_back(LegGeometry::Annotation{
-        current_distance,
-        (reversed_target ? target_node.reverse_duration : target_node.forward_duration) / 10.,
-        (reversed_target ? target_node.reverse_weight : target_node.forward_weight) /
-            facade.GetWeightMultiplier(),
-        forward_datasources[target_node.fwd_segment_position]});
+    // This happens when the source/target are on the same edge-based-node
+    // There will be no entries in the unpacked path, thus no annotations.
+    // We will need to calculate the lone annotation by looking at the position
+    // of the source/target nodes, and calculating their differences.
+    if (geometry.annotations.empty())
+    {
+        auto duration =
+            std::abs(
+                (reversed_target ? target_node.reverse_duration : target_node.forward_duration) -
+                (reversed_source ? source_node.reverse_duration : source_node.forward_duration)) /
+            10.;
+        BOOST_ASSERT(duration >= 0);
+        auto weight =
+            std::abs((reversed_target ? target_node.reverse_weight : target_node.forward_weight) -
+                     (reversed_source ? source_node.reverse_weight : source_node.forward_weight)) /
+            facade.GetWeightMultiplier();
+        BOOST_ASSERT(weight >= 0);
+
+        geometry.annotations.emplace_back(
+            LegGeometry::Annotation{current_distance,
+                                    duration,
+                                    weight,
+                                    forward_datasources[target_node.fwd_segment_position]});
+    }
+    else
+    {
+        geometry.annotations.emplace_back(LegGeometry::Annotation{
+            current_distance,
+            (reversed_target ? target_node.reverse_duration : target_node.forward_duration) / 10.,
+            (reversed_target ? target_node.reverse_weight : target_node.forward_weight) /
+                facade.GetWeightMultiplier(),
+            forward_datasources[target_node.fwd_segment_position]});
+    }
 
     geometry.segment_offsets.push_back(geometry.locations.size());
     geometry.locations.push_back(target_node.location);

include/engine/internal_route_result.hpp

@@ -145,11 +145,32 @@ inline InternalRouteResult CollapseInternalRouteResult(const InternalRouteResult
             collapsed.target_traversed_in_reverse.back() =
                 leggy_result.target_traversed_in_reverse[i];
             // copy path segments into current leg
-            last_segment.insert(last_segment.end(),
-                                leggy_result.unpacked_path_segments[i].begin(),
-                                leggy_result.unpacked_path_segments[i].end());
+            if (!leggy_result.unpacked_path_segments[i].empty())
+            {
+                auto old_size = last_segment.size();
+                last_segment.insert(last_segment.end(),
+                                    leggy_result.unpacked_path_segments[i].begin(),
+                                    leggy_result.unpacked_path_segments[i].end());
+
+                // The first segment of the unpacked path is missing the weight of the
+                // source phantom.  We need to add those values back so that the total
+                // edge weight is correct
+                last_segment[old_size].weight_until_turn +=
+
+                    leggy_result.source_traversed_in_reverse[i]
+                        ? leggy_result.segment_end_coordinates[i].source_phantom.reverse_weight
+                        : leggy_result.segment_end_coordinates[i].source_phantom.forward_weight;
+
+                last_segment[old_size].duration_until_turn +=
+                    leggy_result.source_traversed_in_reverse[i]
+                        ? leggy_result.segment_end_coordinates[i].source_phantom.reverse_duration
+                        : leggy_result.segment_end_coordinates[i].source_phantom.forward_duration;
+            }
         }
     }
+
+    BOOST_ASSERT(collapsed.segment_end_coordinates.size() ==
+                 collapsed.unpacked_path_segments.size());
     return collapsed;
 }
 }

unit_tests/library/route.cpp

@@ -407,12 +407,20 @@ BOOST_AUTO_TEST_CASE(speed_annotation_matches_duration_and_distance)
     const auto &durations = annotation.values.at("duration").get<json::Array>().values;
     const auto &distances = annotation.values.at("distance").get<json::Array>().values;
     int length = speeds.size();
+
+    BOOST_CHECK_EQUAL(length, 1);
     for (int i = 0; i < length; i++)
     {
         auto speed = speeds[i].get<json::Number>().value;
         auto duration = durations[i].get<json::Number>().value;
         auto distance = distances[i].get<json::Number>().value;
-        BOOST_CHECK_EQUAL(speed, std::round(distance / duration * 10.) / 10.);
+        auto calc = std::round(distance / duration * 10.) / 10.;
+        BOOST_CHECK_EQUAL(speed, std::isnan(calc) ? 0 : calc);
+
+        // Because we route from/to the same location, all annotations should be 0;
+        BOOST_CHECK_EQUAL(speed, 0);
+        BOOST_CHECK_EQUAL(distance, 0);
+        BOOST_CHECK_EQUAL(duration, 0);
     }
 }