← Back to branch summary

~valhalla-routing/+junk/valhalla_2.1.9-0ubuntu1

« back to all changes in this revision

Viewing changes to libvalhalla/src/thor/trafficalgorithm.cc

  • Committer: valhalla
  • Date: 2017-04-24 20:20:53 UTC
  • Revision ID: valhalla@mapzen.com-20170424202053-7o69b9nwx9ee0tw3
Packaging for 2.1.9-0ubuntu1.

Show diffs side-by-side

added added

removed removed

Lines of Context:
libvalhalla/src/thor/trafficalgorithm.cc
 
1
#include <iostream> // TODO remove if not needed
 
2
#include <map>
 
3
#include <algorithm>
 
4
#include "thor/trafficalgorithm.h"
 
5
#include "baldr/datetime.h"
 
6
#include "midgard/logging.h"
 
7
 
 
8
using namespace valhalla::baldr;
 
9
using namespace valhalla::sif;
 
10
 
 
11
// TODO: make a class that extends std::exception, with messages and
 
12
// error codes and return the appropriate error codes
 
13
 
 
14
namespace valhalla {
 
15
namespace thor {
 
16
 
 
17
// Default constructor
 
18
TrafficAlgorithm::TrafficAlgorithm()
 
19
    : AStarPathAlgorithm() {
 
20
}
 
21
 
 
22
// Destructor
 
23
TrafficAlgorithm::~TrafficAlgorithm() {
 
24
  Clear();
 
25
}
 
26
 
 
27
// Calculate best path. This method is single mode, not time-dependent.
 
28
std::vector<PathInfo> TrafficAlgorithm::GetBestPath(PathLocation& origin,
 
29
             PathLocation& destination, GraphReader& graphreader,
 
30
             const std::shared_ptr<DynamicCost>* mode_costing,
 
31
             const TravelMode mode) {
 
32
  // Set the mode and costing
 
33
  mode_ = mode;
 
34
  const auto& costing = mode_costing[static_cast<uint32_t>(mode_)];
 
35
 
 
36
  // Initialize - create adjacency list, edgestatus support, A*, etc.
 
37
  Init(origin.edges.front().projected, destination.edges.front().projected, costing);
 
38
  float mindist = astarheuristic_.GetDistance(origin.edges.front().projected);
 
39
 
 
40
  // Initialize the origin and destination locations. Initialize the
 
41
  // destination first in case the origin edge includes a destination edge.
 
42
  uint32_t density = SetDestination(graphreader, destination, costing);
 
43
  SetOrigin(graphreader, origin, destination, costing);
 
44
 
 
45
  // Find shortest path
 
46
  uint32_t nc = 0;       // Count of iterations with no convergence
 
47
                         // towards destination
 
48
  const GraphTile* tile;
 
49
  while (true) {
 
50
    // Get next element from adjacency list. Check that it is valid. An
 
51
    // invalid label indicates there are no edges that can be expanded.
 
52
    uint32_t predindex = adjacencylist_->pop();
 
53
    if (predindex == kInvalidLabel) {
 
54
      LOG_ERROR("Route failed after iterations = " +
 
55
                     std::to_string(edgelabels_.size()));
 
56
      return { };
 
57
    }
 
58
 
 
59
    // Copy the EdgeLabel for use in costing. Check if this is a destination
 
60
    // edge and potentially complete the path.
 
61
    EdgeLabel pred = edgelabels_[predindex];
 
62
    if (destinations_.find(pred.edgeid()) != destinations_.end()) {
 
63
      // Check if a trivial path. Skip if no predecessor and not
 
64
      // trivial (cannot reach destination along this one edge).
 
65
      if (pred.predecessor() == kInvalidLabel) {
 
66
        if (IsTrivial(pred.edgeid(), origin, destination)) {
 
67
          return FormPath(predindex);
 
68
        }
 
69
      } else {
 
70
        return FormPath(predindex);
 
71
      }
 
72
    }
 
73
 
 
74
    // Mark the edge as permanently labeled. Do not do this for an origin
 
75
    // edge (this will allow loops/around the block cases)
 
76
    if (!pred.origin()) {
 
77
      edgestatus_->Update(pred.edgeid(), EdgeSet::kPermanent);
 
78
    }
 
79
 
 
80
    // Check that distance is converging towards the destination. Return route
 
81
    // failure if no convergence for TODO iterations
 
82
    float dist2dest = pred.distance();
 
83
    if (dist2dest < mindist) {
 
84
      mindist = dist2dest;
 
85
      nc = 0;
 
86
    } else if (nc++ > 500000) {
 
87
      LOG_ERROR("No convergence to destination after = " +
 
88
                           std::to_string(edgelabels_.size()));
 
89
      return {};
 
90
    }
 
91
 
 
92
    // Get the end node of the prior directed edge. Skip if tile not found
 
93
    // (can happen with regional data sets).
 
94
    GraphId node = pred.endnode();
 
95
    if ((tile = graphreader.GetGraphTile(node)) == nullptr) {
 
96
      continue;
 
97
    }
 
98
 
 
99
    // Check access at the node
 
100
    const NodeInfo* nodeinfo = tile->node(node);
 
101
    if (!costing->Allowed(nodeinfo)) {
 
102
      continue;
 
103
    }
 
104
 
 
105
    // Check if this tile has real-time speeds
 
106
    std::vector<uint8_t>& speeds = GetRealTimeSpeeds(node.tileid(), graphreader);
 
107
 
 
108
    // Expand from end node.
 
109
    GraphId edgeid(node.tileid(), node.level(), nodeinfo->edge_index());
 
110
    const DirectedEdge* directededge = tile->directededge(nodeinfo->edge_index());
 
111
    for (uint32_t i = 0; i < nodeinfo->edge_count();
 
112
                i++, directededge++, ++edgeid) {
 
113
      // Disable upward transitions for traffic...for now only support traffic
 
114
      // for short routes since no shortcuts have traffic yet
 
115
      if (directededge->trans_up()) {
 
116
        continue;
 
117
      }
 
118
 
 
119
      // Skip if no access is allowed to this edge (based on costing method)
 
120
      if (!costing->Allowed(directededge, pred, tile, edgeid)) {
 
121
        continue;
 
122
      }
 
123
 
 
124
      // Get the current set. Skip this edge if permanently labeled (best
 
125
      // path already found to this directed edge).
 
126
      EdgeStatusInfo edgestatus = edgestatus_->Get(edgeid);
 
127
      if (edgestatus.set() == EdgeSet::kPermanent) {
 
128
        continue;
 
129
      }
 
130
 
 
131
      // TODO - want to add a traffic costing method in sif
 
132
      Cost edge_cost;
 
133
      Cost tc = costing->TransitionCost(directededge, nodeinfo, pred);
 
134
      if (speeds.size() == 0 || speeds[edgeid.id()] == 0) {
 
135
        edge_cost = costing->EdgeCost(directededge);
 
136
      } else {
 
137
        // Traffic exists for this edge
 
138
        float sec = directededge->length() * (kSecPerHour * 0.001f) /
 
139
                static_cast<float>(speeds[edgeid.id()]);
 
140
        edge_cost = { sec, sec };
 
141
 
 
142
        // For now reduce transition cost by half...thought is that traffic
 
143
        // will account for some of the transition cost
 
144
        tc.cost *= 0.5f;
 
145
        tc.secs *= 0.5f;
 
146
      }
 
147
 
 
148
      // Compute the cost to the end of this edge
 
149
      Cost newcost = pred.cost() + edge_cost + tc;
 
150
 
 
151
      // If this edge is a destination, subtract the partial/remainder cost
 
152
      // (cost from the dest. location to the end of the edge).
 
153
      auto p = destinations_.find(edgeid);
 
154
      if (p != destinations_.end()) {
 
155
        newcost -= p->second;
 
156
      }
 
157
 
 
158
      // Check if edge is temporarily labeled and this path has less cost. If
 
159
      // less cost the predecessor is updated and the sort cost is decremented
 
160
      // by the difference in real cost (A* heuristic doesn't change)
 
161
      if (edgestatus.set() == EdgeSet::kTemporary) {
 
162
        EdgeLabel& lab = edgelabels_[edgestatus.index()];
 
163
        if (newcost.cost <  lab.cost().cost) {
 
164
          float newsortcost = lab.sortcost() - (lab.cost().cost - newcost.cost);
 
165
          adjacencylist_->decrease(edgestatus.index(), newsortcost);
 
166
          lab.Update(predindex, newcost, newsortcost);
 
167
        }
 
168
        continue;
 
169
      }
 
170
 
 
171
      // If this is a destination edge the A* heuristic is 0. Otherwise the
 
172
      // sort cost (with A* heuristic) is found using the lat,lng at the
 
173
      // end node of the directed edge.
 
174
      float dist = 0.0f;
 
175
      float sortcost = newcost.cost;
 
176
      if (p == destinations_.end()) {
 
177
        const GraphTile* t2 = directededge->leaves_tile() ?
 
178
            graphreader.GetGraphTile(directededge->endnode()) : tile;
 
179
        if (t2 == nullptr) {
 
180
          continue;
 
181
        }
 
182
        sortcost += astarheuristic_.Get(
 
183
                    t2->node(directededge->endnode())->latlng(), dist);
 
184
      }
 
185
 
 
186
      // Add to the adjacency list and edge labels.
 
187
      AddToAdjacencyList(edgeid, sortcost);
 
188
      edgelabels_.emplace_back(predindex, edgeid, directededge,
 
189
                    newcost, sortcost, dist, mode_, 0);
 
190
    }
 
191
  }
 
192
  return {};      // Should never get here
 
193
}
 
194
 
 
195
std::vector<uint8_t>& TrafficAlgorithm::GetRealTimeSpeeds(const uint32_t tileid,
 
196
                               GraphReader& graphreader) {
 
197
  // Check if this tile has real-time speeds
 
198
  auto rts = real_time_speeds_.find(tileid);
 
199
  if (rts == real_time_speeds_.end()) {
 
200
    // Try to load the speeds file
 
201
    std::ifstream rtsfile;
 
202
    std::string traffic_dir = graphreader.tile_dir() + "/traffic/";
 
203
    std::string fname = traffic_dir + std::to_string(tileid) + ".spd";
 
204
    rtsfile.open(fname, std::ios::binary | std::ios::in | std::ios::ate);
 
205
    if (rtsfile.is_open()) {
 
206
      uint32_t count = rtsfile.tellg();
 
207
      LOG_INFO("Load real time speeds: count = " + std::to_string(count));
 
208
      rtsfile.seekg(0, rtsfile.beg);
 
209
      std::vector<uint8_t> spds(count);
 
210
      rtsfile.read((char*)(&spds.front()), count);
 
211
      rtsfile.close();
 
212
      real_time_speeds_[tileid] = spds;
 
213
      return real_time_speeds_[tileid];
 
214
    } else {
 
215
      return empty_speeds_;
 
216
    }
 
217
  } else {
 
218
    return rts->second;
 
219
  }
 
220
}
 
221
 
 
222
 
 
223
}
 
224
}