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

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

Centre the image better
+ fiddle with default scale
+ better message if it fails to find a route
+ allow cycling along A-roads (we need to fix this!)

  • 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 ('primary','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, lat,lon, scale=1):
100    """Setup an image coordinate system"""
101    self.w = w
102    self.h = h
103    self.clat = lat
104    self.clon = lon
105    self.dlat = (self.maxLat - self.minLat) / scale
106    self.dlon = (self.maxLon - self.minLon) / scale
107 
108  def project(self, lat, lon):
109    """Convert from lat/long to image coordinates"""
110    x = self.w * (0.5 + 0.5 * (lon - self.clon) / (0.5 * self.dlon))
111    y = self.h * (0.5 - 0.5 * (lat - self.clat) / (0.5 * self.dlat))
112    return(x,y)
113 
114  def markNode(self,node,r,g,b):
115    """Mark a node on the map"""
116    self.ctx.set_source_rgb(r,g,b)
117    lat = self.nodes[node][0]
118    lon = self.nodes[node][1]
119    x,y = self.project(lat,lon)
120    self.ctx.arc(x,y,2, 0,2*3.14)
121    self.ctx.fill()
122 
123  def markLine(self,n1,n2,r,g,b):
124    """Draw a line on the map between two nodes"""
125    self.ctx.set_source_rgba(r,g,b,0.3)
126    lat = self.nodes[n1][0]
127    lon = self.nodes[n1][1]
128    x,y = self.project(lat,lon)
129    self.ctx.move_to(x,y)
130    lat = self.nodes[n2][0]
131    lon = self.nodes[n2][1]
132    x,y = self.project(lat,lon)
133    self.ctx.line_to(x,y)
134    self.ctx.stroke()
135
136  def distance(self,n1,n2):
137    """Calculate distance between two nodes"""
138    lat1 = self.nodes[n1][0]
139    lon1 = self.nodes[n1][1]
140    lat2 = self.nodes[n2][0]
141    lon2 = self.nodes[n2][1]
142    # TODO: projection issues
143    dlat = lat2 - lat1
144    dlon = lon2 - lon1
145    dist2 = dlat * dlat + dlon * dlon
146    return(math.sqrt(dist2))
147   
148  def doRouting(self, routeFrom, routeTo):
149    """Wrapper around the routing function, which creates the output image, etc"""
150    size = 800
151    scalemap = 5 # the bigger this is, the more the map zooms-in
152    # Centre the map halfway between start and finish
153    ctrLat = (self.nodes[routeFrom][0] + self.nodes[routeTo][0]) / 2
154    ctrLon = (self.nodes[routeFrom][1] + self.nodes[routeTo][1]) / 2
155    self.initProj(size, size, ctrLat, ctrLon, scalemap)
156    surface = cairo.ImageSurface(cairo.FORMAT_RGB24, self.w, self.h)
157   
158    self.ctx = cairo.Context(surface)
159    # Dump all the nodes onto the map, to give the routes some context
160    self.ctx.set_source_rgb(1.0, 0.0, 0.0)
161    self.ctx.set_line_cap(cairo.LINE_CAP_ROUND)
162    for id,n in self.nodes.items():
163      x,y = self.project(n[0], n[1])
164      self.ctx.move_to(x,y)
165      self.ctx.line_to(x,y)
166      self.ctx.stroke()
167   
168    # Do the routing itself
169    self.doRoute(routeFrom, routeTo)
170   
171    # Highlight which nodes were the start and end
172    self.markNode(routeFrom,1,1,1)
173    self.markNode(routeTo,1,1,0)
174   
175    # Image is complete
176    surface.write_to_png("output.png")
177 
178  def doRoute(self,start,end):
179    """Do the routing"""
180    self.searchEnd = end
181    closed = [start]
182    self.queue = []
183   
184    # Start by queueing all outbound links from the start node
185    blankQueueItem = {'end':-1,'distance':0,'nodes':str(start)}
186    for i in self.routing[start]:
187      self.addToQueue(start,i, blankQueueItem)
188   
189    # Limit for how long it will search (also useful for debugging step-by-step)
190    maxSteps = 10000
191    while maxSteps > 0:
192      maxSteps = maxSteps - 1
193      try:
194        nextItem = self.queue.pop(0)
195      except IndexError:
196        print "Failed to find any route"
197        return
198      x = nextItem['end']
199      if x in closed:
200        continue
201      self.markNode(x,0,0,1)
202      if x == end:
203        print "Success!"
204        self.printRoute(nextItem)
205        return
206      closed.append(x)
207      try:
208        for i in self.routing[x]:
209          if not i in closed:
210            self.addToQueue(x,i,nextItem)
211      except KeyError:
212        pass
213    else:
214      self.debugQueue()
215 
216  def debugQueue(self):
217    """Display some information about the state of our queue"""
218    print "Queue now %d items long" % len(self.queue)
219    # Display on map
220    for i in self.queue:
221      self.markNode(i['end'],0,0.5,0)
222 
223  def printRoute(self,item):
224    """Output stage, for printing the route once found"""
225   
226    # Route is stored as text initially. Split into a list
227    print "Route: %s" % item['nodes']
228    listNodes = [int(i) for i in item['nodes'].split(",")]
229   
230    # Display the route on the map
231    last = -1
232    for i in listNodes:
233      if last != -1:
234        self.markLine(last,i,0.5,1.0,0.5)
235      self.markNode(i,0.5,1.0,0.5)
236      last = i
237   
238    # Send the route to an OSM file
239    fout = open("route.osm", "w")
240    fout.write("<?xml version='1.0' encoding='UTF-8'?>");
241    fout.write("<osm version='0.5' generator='route.py'>");
242   
243    for i in listNodes:
244      fout.write("<node id='%d' lat='%f' lon='%f'>\n</node>\n" % ( \
245        i,
246        self.nodes[i][0],
247        self.nodes[i][1]))
248
249    fout.write("<way id='1'>\n")
250    for i in listNodes:
251      fout.write("<nd ref='%d' lat='%f' lon='%f' />\n" % ( \
252        i,
253        self.nodes[i][0],
254        self.nodes[i][0]))
255    fout.write("</way>\n")
256    fout.write("</osm>")
257    fout.close()
258 
259  def addToQueue(self,start,end, queueSoFar):
260    """Add another potential route to the queue"""
261   
262    # If already in queue
263    for test in self.queue:
264      if test['end'] == end:
265        return
266    distance = self.distance(start, end)
267   
268    # Create a hash for all the route's attributes
269    queueItem = {}
270    queueItem['distance'] = queueSoFar['distance'] + distance
271    queueItem['maxdistance'] = queueItem['distance'] + self.distance(end, self.searchEnd)
272    queueItem['nodes'] = queueSoFar['nodes'] + ","+str(end)
273    queueItem['end'] = end
274   
275    # Try to insert, keeping the queue ordered by decreasing worst-case distance
276    count = 0
277    for test in self.queue:
278      if test['maxdistance'] > queueItem['maxdistance']:
279        self.queue.insert(count,queueItem)
280        break
281      count = count + 1
282    else:
283      self.queue.append(queueItem)
284   
285    # Show on the map
286    self.markLine(start,end,0.5,0.5,0.5)
287
288# Parse the supplied OSM file
289print "Loading data..."
290obj = GetRoutes()
291parser = make_parser()
292parser.setContentHandler(obj)
293parser.parse(sys.argv[1])
294print "Routing..."
295# Do routing between the two specified nodes
296obj.doRouting(int(sys.argv[2]), int(sys.argv[3]))
297
Note: See TracBrowser for help on using the repository browser.