Add functionality to write turn lookups and read turn penalty files

.gitignore

@@ -80,6 +80,7 @@ stxxl.errlog
 /test/profile.lua
 /test/cache
 /test/speeds.csv
+/test/penalties.csv
 /test/data/monaco.*
 node_modules
 

features/car/traffic.feature → features/car/traffic_speeds.feature

@@ -31,7 +31,7 @@ Feature: Traffic - speeds
         4,1,27
         """
 
-    Scenario: Weighting not based on raster sources
+    Scenario: Weighting based on speed file
         Given the profile "testbot"
         Given the extract extra arguments "--generate-edge-lookup"
         Given the contract extra arguments "--segment-speed-file speeds.csv"

features/car/traffic_turn_penalties.feature

@@ -0,0 +1,97 @@
+@routing @speed @traffic
+Feature: Traffic - turn penalties
+
+    Background: Evenly spaced grid with multiple intersections
+        Given the node map
+            |   | a |   | b |   |
+            | c | d | e | f | g |
+            |   | h |   | i |   |
+            | j | k | l | m | n |
+            |   | o |   | p |   |
+        And the ways
+            | nodes | highway |
+            | ad    | primary |
+            | cd    | primary |
+            | de    | primary |
+            | dhk   | primary |
+
+            | bf    | primary |
+            | ef    | primary |
+            | fg    | primary |
+            | fim   | primary |
+
+            | jk    | primary |
+            | kl    | primary |
+            | ko    | primary |
+
+            | lm    | primary |
+            | mn    | primary |
+            | mp    | primary |
+        And the profile "car"
+        And the extract extra arguments "--generate-edge-lookup"
+
+    Scenario: Weighting not based on turn penalty file
+        When I route I should get
+            | from | to | route          | speed   | time      |
+            | a    | h  | ad,dhk,dhk     | 63 km/h | 11.5s +-1 |
+                                                                  # straight
+            | i    | g  | fim,fg,fg      | 59 km/h | 12s  +-1  |
+                                                                  # right
+            | a    | e  | ad,de,de       | 57 km/h | 12.5s +-1 |
+                                                                  # left
+            | c    | g  | cd,de,ef,fg,fg | 63 km/h | 23s +-1   |
+                                                                  # double straight
+            | p    | g  | mp,fim,fg,fg   | 61 km/h | 23.5s +-1 |
+                                                                  # straight-right
+            | a    | l  | ad,dhk,kl,kl   | 60 km/h | 24s +-1   |
+                                                                  # straight-left
+            | l    | e  | kl,dhk,de,de   | 59 km/h | 24.5s +-1 |
+                                                                  # double right
+            | g    | n  | fg,fim,mn,mn   | 57 km/h | 25s +-1   |
+                                                                  # double left
+
+    Scenario: Weighting based on turn penalty file
+        Given the turn penalty file
+            """
+            9,6,7,1.8
+            9,13,14,24.5
+            8,4,3,26
+            12,11,8,9
+            8,11,12,13
+            1,4,5,-0.2
+            """
+        And the contract extra arguments "--turn-penalty-file penalties.csv"
+        When I route I should get
+            | from | to | route                    | speed   | time      |
+            | a    | h  | ad,dhk,dhk               | 63 km/h | 11.5s +-1 |
+                                                                              # straight
+            | i    | g  | fim,fg,fg                | 55 km/h | 13s +-1   |
+                                                                              # right - ifg penalty
+            | a    | e  | ad,de,de                 | 64 km/h | 11s +-1   |
+                                                                              # left - faster because of negative ade penalty
+            | c    | g  | cd,de,ef,fg,fg           | 63 km/h | 23s +-1   |
+                                                                              # double straight
+            | p    | g  | mp,fim,fg,fg             | 59 km/h | 24.5s +-1 |
+                                                                              # straight-right - ifg penalty
+            | a    | l  | ad,de,ef,fim,lm,lm       | 61 km/h | 35.5s +-1 |
+                                                                              # was straight-left - forced around by hkl penalty
+            | l    | e  | lm,fim,ef,ef             | 57 km/h | 25s +-1   |
+                                                                              # double right - forced left by lkh penalty
+            | g    | n  | fg,fim,mn,mn             | 30 km/h | 47.5s +-1   |
+                                                                              # double left - imn penalty
+            | j    | c  | jk,kl,lm,fim,ef,de,cd,cd | 60 km/h | 48s +-1   |
+                                                                              # double left - hdc penalty ever so slightly higher than imn; forces all the way around
+
+    Scenario: Too-negative penalty clamps, but does not fail
+        Given the contract extra arguments "--turn-penalty-file penalties.csv"
+        And the profile "testbot"
+        And the turn penalty file
+            """
+            1,4,5,-10
+            """
+        When I route I should get
+            | from | to | route    | time    |
+            | a    | d  | ad,ad    | 10s +-1 |
+            | a    | e  | ad,de,de | 10s +-1 |
+            | b    | f  | bf,bf    | 10s +-1 |
+            | b    | g  | bf,fg,fg | 20s +-1 |

features/step_definitions/data.js

@@ -10,13 +10,11 @@ module.exports = function () {
     });
 
     this.Given(/^the extract extra arguments "(.*?)"$/, (args, callback) => {
-        this.setExtractArgs(args);
-        callback();
+        this.setExtractArgs(args, callback);
     });
 
     this.Given(/^the contract extra arguments "(.*?)"$/, (args, callback) => {
-        this.setContractArgs(args);
-        callback();
+        this.setContractArgs(args, callback);
     });
 
     this.Given(/^a grid size of (\d+) meters$/, (meters, callback) => {
@@ -228,6 +226,10 @@ module.exports = function () {
         fs.writeFile(path.resolve(this.TEST_FOLDER, 'speeds.csv'), data, callback);
     });
 
+    this.Given(/^the turn penalty file$/, (data, callback) => {
+        fs.writeFile(path.resolve(this.TEST_FOLDER, 'penalties.csv'), data, callback);
+    });
+
     this.Given(/^the data has been saved to disk$/, (callback) => {
         try {
             this.reprocess(callback);

features/support/config.js

@@ -105,11 +105,16 @@ module.exports = function () {
         } else cb();
     };
 
-    this.setExtractArgs = (args) => {
+    this.setExtractArgs = (args, callback) => {
         this.extractArgs = args;
+        this.forceExtract = true;
+        this.forceContract = true;
+        callback();
     };
 
-    this.setContractArgs = (args) => {
+    this.setContractArgs = (args, callback) => {
         this.contractArgs = args;
+        this.forceContract = true;
+        callback();
     };
 };

features/support/data.js

@@ -295,9 +295,10 @@ module.exports = function () {
         this.writeAndExtract((e) => {
             if (e) return callback(e);
             this.isContracted((isContracted) => {
-                var contractFn = isContracted ? noop : this.contractData;
+                var contractFn = (isContracted && !this.forceContract) ? noop : this.contractData;
                 if (isContracted) this.log('Already contracted ' + this.osmData.contractedFile, 'preprocess');
                 contractFn((e) => {
+                    this.forceContract = false;
                     if (e) return callback(e);
                     this.logPreprocessDone();
                     callback();
@@ -311,9 +312,10 @@ module.exports = function () {
             this.writeInputData((e) => {
                 if (e) return callback(e);
                 this.isExtracted((isExtracted) => {
-                    var extractFn = isExtracted ? noop : this.extractData;
+                    var extractFn = (isExtracted && !this.forceExtract) ? noop : this.extractData;
                     if (isExtracted) this.log('Already extracted ' + this.osmData.extractedFile, 'preprocess');
                     extractFn((e) => {
+                        this.forceExtract = false;
                         callback(e);
                     });
                 });

features/support/hooks.js

@@ -24,10 +24,12 @@ module.exports = function () {
     });
 
     this.After((scenario, callback) => {
-        this.setExtractArgs('');
-        this.setContractArgs('');
-        if (this.loadMethod === 'directly' && !!this.OSRMLoader.loader) this.OSRMLoader.shutdown(callback);
-        else callback();
+        this.setExtractArgs('', () => {
+            this.setContractArgs('', () => {
+                if (this.loadMethod === 'directly' && !!this.OSRMLoader.loader) this.OSRMLoader.shutdown(callback);
+                else callback();
+            });
+        });
     });
 
     this.Around('@stress', (scenario, callback) => {

features/testbot/traffic_turn_penalties.feature

@@ -0,0 +1,37 @@
+@routing @speed @traffic
+Feature: Traffic - turn penalties applied to turn onto which a phantom node snaps
+
+    Background: Simple map with phantom nodes
+        Given the node map
+            |   | 1 |   | 2 |   | 3 |   |
+            | a |   | b |   | c |   | d |
+            |   |   |   |   |   |   |   |
+            |   |   | e |   | f |   | g |
+        And the ways
+            | nodes | highway |
+            | ab    | primary |
+            | bc    | primary |
+            | cd    | primary |
+
+            | be    | primary |
+            | cf    | primary |
+            | dg    | primary |
+        And the profile "testbot"
+        # Since testbot doesn't have turn penalties, a penalty from file of 0 should produce a neutral effect
+        And the extract extra arguments "--generate-edge-lookup"
+
+    Scenario: Weighting based on turn penalty file, with an extreme negative value -- clamps and does not fail
+        Given the turn penalty file
+            """
+            1,2,5,0
+            3,4,7,-20
+            """
+        And the contract extra arguments "--turn-penalty-file penalties.csv"
+        When I route I should get
+            | from | to | route    | speed   | time    |
+            | a    | e  | ab,be,be | 36 km/h | 40s +-1 |
+            | 1    | e  | ab,be,be | 36 km/h | 30s +-1 |
+            | b    | f  | bc,cf,cf | 36 km/h | 40s +-1 |
+            | 2    | f  | bc,cf,cf | 36 km/h | 30s +-1 |
+            | c    | g  | cd,dg,dg | 71 km/h | 20s +-1 |
+            | 3    | g  | cd,dg,dg | 54 km/h | 20s +-1 |

include/contractor/contractor.hpp

@@ -84,6 +84,7 @@ class Contractor
                           const std::string &edge_segment_lookup_path,
                           const std::string &edge_penalty_path,
                           const std::vector<std::string> &segment_speed_path,
+                          const std::vector<std::string> &turn_penalty_path,
                           const std::string &nodes_filename,
                           const std::string &geometry_filename,
                           const std::string &datasource_names_filename,

include/contractor/contractor_config.hpp

@@ -81,6 +81,7 @@ struct ContractorConfig
     double core_factor;
 
     std::vector<std::string> segment_speed_lookup_paths;
+    std::vector<std::string> turn_penalty_lookup_paths;
     std::string datasource_indexes_path;
     std::string datasource_names_path;
 };

include/engine/routing_algorithms/routing_base.hpp

@@ -301,8 +301,6 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
                 auto total_weight = std::accumulate(weight_vector.begin(), weight_vector.end(), 0);
 
                 BOOST_ASSERT(weight_vector.size() == id_vector.size());
-                // ed.distance should be total_weight + penalties (turn, stop, etc)
-                BOOST_ASSERT(ed.distance >= total_weight);
                 const bool is_first_segment = unpacked_path.empty();
 
                 const std::size_t start_index =
@@ -350,7 +348,8 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
                 start_index =
                     id_vector.size() - phantom_node_pair.source_phantom.fwd_segment_position - 1;
             }
-            end_index = id_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position - 1;
+            end_index =
+                id_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position - 1;
         }
         else
         {
@@ -396,8 +395,17 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
             // However the first segment duration needs to be adjusted to the fact that the source
             // phantom is in the middle of the segment. We do this by subtracting v--s from the
             // duration.
-            BOOST_ASSERT(unpacked_path.front().duration_until_turn >= source_weight);
-            unpacked_path.front().duration_until_turn -= source_weight;
+
+            // Since it's possible duration_until_turn can be less than source_weight here if
+            // a negative enough turn penalty is used to modify this edge weight during
+            // osrm-contract, we clamp to 1 here so as not to return a negative duration
+            // for this segment.
+
+            // TODO this creates a scenario where it's possible the duration from a phantom
+            // node to the first turn would be the same as from end to end of a segment,
+            // which is obviously incorrect and not ideal...
+            unpacked_path.front().duration_until_turn =
+                std::max(unpacked_path.front().duration_until_turn - source_weight, 0);
         }
 
         // there is no equivalent to a node-based node in an edge-expanded graph.

include/extractor/compressed_edge_container.hpp

@@ -40,7 +40,9 @@ class CompressedEdgeContainer
     void SerializeInternalVector(const std::string &path) const;
     unsigned GetPositionForID(const EdgeID edge_id) const;
     const EdgeBucket &GetBucketReference(const EdgeID edge_id) const;
+    bool IsTrivial(const EdgeID edge_id) const;
     NodeID GetFirstEdgeTargetID(const EdgeID edge_id) const;
+    NodeID GetLastEdgeTargetID(const EdgeID edge_id) const;
     NodeID GetLastEdgeSourceID(const EdgeID edge_id) const;
 
   private:

include/extractor/guidance/toolkit.hpp

@@ -103,7 +103,7 @@ getCoordinateFromCompressedRange(util::Coordinate current_coordinate,
 }
 } // namespace detail
 
-// Finds a (potentially inteprolated) coordinate that is DESIRED_SEGMENT_LENGTH away
+// Finds a (potentially interpolated) coordinate that is DESIRED_SEGMENT_LENGTH away
 // from the start of an edge
 inline util::Coordinate
 getRepresentativeCoordinate(const NodeID from_node,