source: subversion/applications/routing/pyroute/pyroute.py @ 5294

Last change on this file since 5294 was 5294, checked in by ojw, 12 years ago

Actually, if KeyError? is raised, we don't need to be told a second
time that the 'from' node needs creating

  • Property svn:executable set to *
File size: 9.3 KB
Line 
1#!/usr/bin/python
2#----------------------------------------------------------------
3# pyRoute, a routing program for OpenStreetMap-style data
4#
5#------------------------------------------------------
6# Usage:
7#  pyroute.py [input OSM file] [start node] [end node]
8#------------------------------------------------------
9# Copyright 2007, Oliver White
10#
11# This program is free software: you can redistribute it and/or modify
12# it under the terms of the GNU General Public License as published by
13# the Free Software Foundation, either version 3 of the License, or
14# (at your option) any later version.
15#
16# This program is distributed in the hope that it will be useful,
17# but WITHOUT ANY WARRANTY; without even the implied warranty of
18# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19# GNU General Public License for more details.
20#
21# You should have received a copy of the GNU General Public License
22# along with this program.  If not, see <http://www.gnu.org/licenses/>.
23#------------------------------------------------------
24# Changelog:
25#  2007-11-03  OJW  Created
26#------------------------------------------------------
27import sys
28import cairo
29import math 
30from xml.sax import make_parser, handler
31
32class GetRoutes(handler.ContentHandler):
33  """Parse an OSM file looking for routing information, and do routing with it"""
34  def __init__(self):
35    """Initialise an OSM-file parser"""
36    self.routing = {}
37    self.nodes = {}
38    self.minLon = 180
39    self.minLat = 90
40    self.maxLon = -180
41    self.maxLat = -90
42  def startElement(self, name, attrs):
43    """Handle XML elements"""
44    if name in('node','way','relation'):
45      if name == 'node':
46        """Nodes need to be stored"""
47        id = int(attrs.get('id'))
48        lat = float(attrs.get('lat'))
49        lon = float(attrs.get('lon'))
50        self.nodes[id] = (lat,lon)
51        if lon < self.minLon:
52          self.minLon = lon
53        if lat < self.minLat:
54          self.minLat = lat
55        if lon > self.maxLon:
56          self.maxLon = lon
57        if lat > self.maxLat:
58          self.maxLat = lat
59      self.tags = {}
60      self.waynodes = []
61    elif name == 'nd':
62      """Nodes within a way -- add them to a list"""
63      self.waynodes.append(int(attrs.get('ref')))
64    elif name == 'tag':
65      """Tags - store them in a hash"""
66      k,v = (attrs.get('k'), attrs.get('v'))
67      if not k in ('created_by'):
68        self.tags[k] = v
69  def endElement(self, name):
70    """Handle ways in the OSM data"""
71    if name == 'way':
72      last = -1
73      highway = self.tags.get('highway', '')
74      railway = self.tags.get('railway', '')
75      oneway = self.tags.get('oneway', '')
76      reversible = not oneway in('yes','true','1')
77      cyclable = highway in ('secondary','tertiary','unclassified','minor','cycleway','residential', 'service')
78      if cyclable:
79        for i in self.waynodes:
80          if last != -1:
81            #print "%d -> %d & v.v." % (last, i)
82            self.addLink(last, i)
83            if reversible:
84              self.addLink(i, last)
85          last = i
86
87  def addLink(self,fr,to):
88    """Add a routeable edge to the scenario"""
89    # Look for existing
90    try:
91      if to in self.routing[fr]:
92        #print "duplicate %d from %d" % (to,fr)
93        return
94      # Try to add to list. If list doesn't exist, create it
95      self.routing[fr].append(to)
96    except KeyError:
97      self.routing[fr] = [to]
98 
99  def initProj(self,w,h, centrenode, scale=1):
100    """Setup an image coordinate system"""
101    self.w = w
102    self.h = h
103    if centrenode == 0:
104      self.clat = (self.maxLat + self.minLat) / 2
105      self.clon = (self.maxLon + self.minLon) / 2
106    else:
107      self.clat = self.nodes[centrenode][0]
108      self.clon = self.nodes[centrenode][1]
109    self.dlat = (self.maxLat - self.minLat) / scale
110    self.dlon = (self.maxLon - self.minLon) / scale
111 
112  def project(self, lat, lon):
113    """Convert from lat/long to image coordinates"""
114    x = self.w * (0.5 + 0.5 * (lon - self.clon) / (0.5 * self.dlon))
115    y = self.h * (0.5 - 0.5 * (lat - self.clat) / (0.5 * self.dlat))
116    return(x,y)
117 
118  def markNode(self,node,r,g,b):
119    """Mark a node on the map"""
120    self.ctx.set_source_rgb(r,g,b)
121    lat = self.nodes[node][0]
122    lon = self.nodes[node][1]
123    x,y = self.project(lat,lon)
124    self.ctx.arc(x,y,2, 0,2*3.14)
125    self.ctx.fill()
126 
127  def markLine(self,n1,n2,r,g,b):
128    """Draw a line on the map between two nodes"""
129    self.ctx.set_source_rgba(r,g,b,0.3)
130    lat = self.nodes[n1][0]
131    lon = self.nodes[n1][1]
132    x,y = self.project(lat,lon)
133    self.ctx.move_to(x,y)
134    lat = self.nodes[n2][0]
135    lon = self.nodes[n2][1]
136    x,y = self.project(lat,lon)
137    self.ctx.line_to(x,y)
138    self.ctx.stroke()
139
140  def distance(self,n1,n2):
141    """Calculate distance between two nodes"""
142    lat1 = self.nodes[n1][0]
143    lon1 = self.nodes[n1][1]
144    lat2 = self.nodes[n2][0]
145    lon2 = self.nodes[n2][1]
146    # TODO: projection issues
147    dlat = lat2 - lat1
148    dlon = lon2 - lon1
149    dist2 = dlat * dlat + dlon * dlon
150    return(math.sqrt(dist2))
151   
152  def doRouting(self, routeFrom, routeTo):
153    """Wrapper around the routing function, which creates the output image, etc"""
154    size = 800
155    scalemap = 10 # the bigger this is, the more the map zooms-in
156    self.initProj(size, size, routeFrom, scalemap)
157    surface = cairo.ImageSurface(cairo.FORMAT_RGB24, self.w, self.h)
158   
159    self.ctx = cairo.Context(surface)
160    # Dump all the nodes onto the map, to give the routes some context
161    self.ctx.set_source_rgb(1.0, 0.0, 0.0)
162    self.ctx.set_line_cap(cairo.LINE_CAP_ROUND)
163    for id,n in self.nodes.items():
164      x,y = self.project(n[0], n[1])
165      self.ctx.move_to(x,y)
166      self.ctx.line_to(x,y)
167      self.ctx.stroke()
168   
169    # Do the routing itself
170    self.doRoute(routeFrom, routeTo)
171   
172    # Highlight which nodes were the start and end
173    self.markNode(routeFrom,1,1,1)
174    self.markNode(routeTo,1,1,0)
175   
176    # Image is complete
177    surface.write_to_png("output.png")
178 
179  def doRoute(self,start,end):
180    """Do the routing"""
181    self.searchEnd = end
182    closed = [start]
183    self.queue = []
184   
185    # Start by queueing all outbound links from the start node
186    blankQueueItem = {'end':-1,'distance':0,'nodes':str(start)}
187    for i in self.routing[start]:
188      self.addToQueue(start,i, blankQueueItem)
189   
190    # Limit for how long it will search (also useful for debugging step-by-step)
191    maxSteps = 320
192    while maxSteps > 0:
193      maxSteps = maxSteps - 1
194      try:
195        nextItem = self.queue.pop(0)
196      except IndexError:
197        print "Queue empty"
198        return
199      x = nextItem['end']
200      if x in closed:
201        continue
202      self.markNode(x,0,0,1)
203      if x == end:
204        print "Success!"
205        self.printRoute(nextItem)
206        return
207      closed.append(x)
208      try:
209        for i in self.routing[x]:
210          if not i in closed:
211            self.addToQueue(x,i,nextItem)
212      except KeyError:
213        pass
214    else:
215      self.debugQueue()
216 
217  def debugQueue(self):
218    """Display some information about the state of our queue"""
219    print "Queue now %d items long" % len(self.queue)
220    # Display on map
221    for i in self.queue:
222      self.markNode(i['end'],0,0.5,0)
223 
224  def printRoute(self,item):
225    """Output stage, for printing the route once found"""
226   
227    # Route is stored as text initially. Split into a list
228    print "Route: %s" % item['nodes']
229    listNodes = [int(i) for i in item['nodes'].split(",")]
230   
231    # Display the route on the map
232    last = -1
233    for i in listNodes:
234      if last != -1:
235        self.markLine(last,i,0.5,1.0,0.5)
236      self.markNode(i,0.5,1.0,0.5)
237      last = i
238   
239    # Send the route to an OSM file
240    fout = open("route.osm", "w")
241    fout.write("<?xml version='1.0' encoding='UTF-8'?>");
242    fout.write("<osm version='0.5' generator='route.py'>");
243   
244    for i in listNodes:
245      fout.write("<node id='%d' lat='%f' lon='%f'>\n</node>\n" % ( \
246        i,
247        self.nodes[i][0],
248        self.nodes[i][1]))
249
250    fout.write("<way id='1'>\n")
251    for i in listNodes:
252      fout.write("<nd ref='%d' lat='%f' lon='%f' />\n" % ( \
253        i,
254        self.nodes[i][0],
255        self.nodes[i][0]))
256    fout.write("</way>\n")
257    fout.write("</osm>")
258    fout.close()
259 
260  def addToQueue(self,start,end, queueSoFar):
261    """Add another potential route to the queue"""
262   
263    # If already in queue
264    for test in self.queue:
265      if test['end'] == end:
266        return
267    distance = self.distance(start, end)
268   
269    # Create a hash for all the route's attributes
270    queueItem = {}
271    queueItem['distance'] = queueSoFar['distance'] + distance
272    queueItem['maxdistance'] = queueItem['distance'] + self.distance(end, self.searchEnd)
273    queueItem['nodes'] = queueSoFar['nodes'] + ","+str(end)
274    queueItem['end'] = end
275   
276    # Try to insert, keeping the queue ordered by decreasing worst-case distance
277    count = 0
278    for test in self.queue:
279      if test['maxdistance'] > queueItem['maxdistance']:
280        self.queue.insert(count,queueItem)
281        break
282      count = count + 1
283    else:
284      self.queue.append(queueItem)
285   
286    # Show on the map
287    self.markLine(start,end,0.5,0.5,0.5)
288
289# Parse the supplied OSM file
290print "Loading data..."
291obj = GetRoutes()
292parser = make_parser()
293parser.setContentHandler(obj)
294parser.parse(sys.argv[1])
295print "Routing..."
296# Do routing between the two specified nodes
297obj.doRouting(int(sys.argv[2]), int(sys.argv[3]))
298
Note: See TracBrowser for help on using the repository browser.