def update_geo_path(self):
if (self.waypoints and self.zone and self.band):
waypoints_geo = []
for point in self.waypoints:
x, y = point
geo = UTMPoint(x, y, zone=self.zone, band=self.band).toMsg()
wp = WayPoint()
wp.id = unique_id.toMsg(unique_id.fromRandom())
wp.position = geo
waypoints_geo.append(wp)
segments_geo = []
last_wp = waypoints_geo[0].id
for wp in waypoints_geo:
seg = RouteSegment()
seg.id = unique_id.toMsg(unique_id.fromRandom())
seg.start = last_wp
seg.end = wp.id
last_wp = wp.id
segments_geo.append(seg)
self.route_network.points = waypoints_geo
self.route_network.segments = segments_geo
else:
rospy.logwarn('[update_geo_path] waypoints, zone, and/or band\n' +
'\thas not been set/updated.\n\tIgnoring...')
def makeWayPoint(id, lat, lon):
w = WayPoint()
w.id = UniqueID(id)
w.position = GeoPoint(latitude=lat, longitude=lon)
return w
def get_osm(url, bounds):
"""Get GeographicMap from Open Street Map XML data.
The latitude and longitude bounding box returned may differ from
the requested bounds.
:param url: Uniform Resource Locator for map.
:param bounds: Desired bounding box for map (presently ignored).
:returns: GeographicMap message with header not filled in.
"""
# parse the URL
filename = ''
if url.startswith('file:///'):
filename = url[7:]
elif url.startswith('package://'):
pkg_name, slash, pkg_path = url[10:].partition('/')
pkg_dir = roslib.packages.get_pkg_dir(pkg_name)
filename = pkg_dir + '/' + pkg_path
else:
raise ValueError('unsupported URL: ' + url)
map = GeographicMap(id=geodesy.gen_uuid.makeUniqueID(url))
xm = None
try:
f = open(filename, 'r')
xm = ElementTree.parse(f)
except IOError:
raise ValueError('unable to read ' + str(url))
except ElementTree.ParseError:
raise ValueError('XML parse failed for ' + str(url))
osm = xm.getroot()
# get map bounds
for el in osm.iterfind('bounds'):
map.bounds.min_latitude = float(get_required_attribute(el, 'minlat'))
map.bounds.min_longitude = float(get_required_attribute(el, 'minlon'))
map.bounds.max_latitude = float(get_required_attribute(el, 'maxlat'))
map.bounds.max_longitude = float(get_required_attribute(el, 'maxlon'))
# get map way-point nodes
for el in osm.iterfind('node'):
way = WayPoint()
id = el.get('id')
if id == None:
raise ValueError('node id missing')
way.id = makeOsmUniqueID('node', id)
way.position.latitude = float(get_required_attribute(el, 'lat'))
way.position.longitude = float(get_required_attribute(el, 'lon'))
way.position.altitude = float(el.get('ele', float('nan')))
for tag_list in el.iterfind('tag'):
kv = get_tag(tag_list)
if kv != None:
way.props.append(kv)
map.points.append(way)
# get map paths
for el in osm.iterfind('way'):
feature = MapFeature()
id = el.get('id')
if id == None:
raise ValueError('way id missing')
feature.id = makeOsmUniqueID('way', id)
for nd in el.iterfind('nd'):
way_id = get_required_attribute(nd, 'ref')
feature.components.append(makeOsmUniqueID('node', way_id))
for tag_list in el.iterfind('tag'):
kv = get_tag(tag_list)
if kv != None:
feature.props.append(kv)
map.features.append(feature)
# get relations
for el in osm.iterfind('relation'):
feature = MapFeature()
id = el.get('id')
if id == None:
raise ValueError('relation id missing')
feature.id = makeOsmUniqueID('relation', id)
for mbr in el.iterfind('member'):
mbr_type = get_required_attribute(mbr, 'type')
if mbr_type in set(['node', 'way', 'relation']):
mbr_id = get_required_attribute(mbr, 'ref')
feature.components.append(makeOsmUniqueID(mbr_type, mbr_id))
else:
print('unknown relation member type: ' + mbr_type)
for tag_list in el.iterfind('tag'):
kv = get_tag(tag_list)
if kv != None:
feature.props.append(kv)
map.features.append(feature)
return map
def get_osm(url, bounds):
"""Get `geographic_msgs/GeographicMap`_ from Open Street Map XML data.
The latitude and longitude of the bounding box returned may differ
from the requested bounds.
:param url: Uniform Resource Locator for map.
:param bounds: Desired `geographic_msgs/BoundingBox`_ for map (presently ignored).
:returns: `geographic_msgs/GeographicMap`_ message (header not filled in).
"""
# parse the URL
filename = ''
if url.startswith('file:///'):
filename = url[7:]
elif url.startswith('package://'):
pkg_name, slash, pkg_path = url[10:].partition('/')
pkg_dir = roslib.packages.get_pkg_dir(pkg_name)
filename = pkg_dir + '/' + pkg_path
else:
raise ValueError('unsupported URL: ' + url)
gmap = GeographicMap(id = geodesy.gen_uuid.makeUniqueID(url))
xm = None
try:
f = open(filename, 'r')
xm = ElementTree.parse(f)
except IOError:
raise ValueError('unable to read ' + str(url))
except ElementTree.ParseError:
raise ValueError('XML parse failed for ' + str(url))
osm = xm.getroot()
# get map bounds
for el in osm.iterfind('bounds'):
minlat = float(get_required_attribute(el, 'minlat'))
minlon = float(get_required_attribute(el, 'minlon'))
maxlat = float(get_required_attribute(el, 'maxlat'))
maxlon = float(get_required_attribute(el, 'maxlon'))
gmap.bounds = bounding_box.makeBounds2D(minlat, minlon, maxlat, maxlon)
# get map way-point nodes
for el in osm.iterfind('node'):
way = WayPoint()
el_id = el.get('id')
if el_id is None:
raise ValueError('node id missing')
way.id = makeOsmUniqueID('node', el_id)
way.position.latitude = float(get_required_attribute(el, 'lat'))
way.position.longitude = float(get_required_attribute(el, 'lon'))
way.position.altitude = float(el.get('ele', float('nan')))
for tag_list in el.iterfind('tag'):
kv = get_tag(tag_list)
if kv != None:
way.props.append(kv)
gmap.points.append(way)
# get map paths
for el in osm.iterfind('way'):
feature = MapFeature()
el_id = el.get('id')
if el_id is None:
raise ValueError('way id missing')
feature.id = makeOsmUniqueID('way', el_id)
for nd in el.iterfind('nd'):
way_id = get_required_attribute(nd, 'ref')
feature.components.append(makeOsmUniqueID('node', way_id))
for tag_list in el.iterfind('tag'):
kv = get_tag(tag_list)
if kv != None:
feature.props.append(kv)
gmap.features.append(feature)
# get relations
for el in osm.iterfind('relation'):
feature = MapFeature()
el_id = el.get('id')
if el_id is None:
raise ValueError('relation id missing')
feature.id = makeOsmUniqueID('relation', el_id)
for mbr in el.iterfind('member'):
mbr_type = get_required_attribute(mbr, 'type')
if mbr_type in set(['node', 'way', 'relation']):
mbr_id = get_required_attribute(mbr, 'ref')
feature.components.append(makeOsmUniqueID(mbr_type, mbr_id))
else:
print('unknown relation member type: ' + mbr_type)
for tag_list in el.iterfind('tag'):
kv = get_tag(tag_list)
if kv != None:
feature.props.append(kv)
gmap.features.append(feature)
return gmap
def makeWayPoint(id, lat, lon):
w = WayPoint()
w.id = UniqueID(id)
w.position = GeoPoint(latitude=lat, longitude=lon)
return w
