def grid_graph(min_lat, min_lon, max_lat, max_lon, step=0.001):
"""Generate a fully-connected rectangular grid.
:param min_lat: Initial latitude [degrees].
:param min_lon: Initial longitude [degrees].
:param max_lat: Latitude limit [degrees].
:param max_lon: Longitude limit [degrees].
:param step: Step size [degrees].
:returns: RouteNetwork message.
"""
nid = geodesy.gen_uuid.makeUniqueID(
unique_id.fromURL(PKG_URL + '/test_network'))
r = RouteNetwork(id=nid)
prev_row = None
for latitude in float_range(min_lat, max_lat, step):
prev_col = None
this_row = len(r.points)
for longitude in float_range(min_lon, max_lon, step):
fake_url = 'fake://point/' + str(latitude) + '/' + str(longitude)
pt_id = unique_id.fromURL(fake_url)
r.points.append(makeWayPoint(pt_id, latitude, longitude))
if prev_col is not None:
s = makeSeg(prev_col, pt_id)
r.segments.append(s)
s = makeSeg(pt_id, prev_col)
r.segments.append(s)
prev_col = pt_id
if prev_row is not None:
prev_id = r.points[prev_row].id.uuid
s = makeSeg(prev_id, pt_id)
r.segments.append(s)
s = makeSeg(pt_id, prev_id)
r.segments.append(s)
prev_row += 1
prev_row = this_row
return r
def grid_graph(min_lat, min_lon, max_lat, max_lon, step=0.001):
"""Generate a fully-connected rectangular grid.
:param min_lat: Initial latitude [degrees].
:param min_lon: Initial longitude [degrees].
:param max_lat: Latitude limit [degrees].
:param max_lon: Longitude limit [degrees].
:param step: Step size [degrees].
:returns: RouteNetwork message.
"""
nid = unique_id.toMsg(unique_id.fromURL(PKG_URL + '/test_network'))
r = RouteNetwork(id=nid)
prev_row = None
for latitude in float_range(min_lat, max_lat, step):
prev_col = None
this_row = len(r.points)
for longitude in float_range(min_lon, max_lon, step):
fake_url = 'fake://point/' + str(latitude) + '/' + str(longitude)
pt_id = unique_id.fromURL(fake_url)
r.points.append(makeWayPoint(pt_id, latitude, longitude))
if prev_col is not None:
s = makeSeg(prev_col, pt_id)
r.segments.append(s)
s = makeSeg(pt_id, prev_col)
r.segments.append(s)
prev_col = pt_id
if prev_row is not None:
prev_id = r.points[prev_row].id.uuid
s = makeSeg(prev_id, pt_id)
r.segments.append(s)
s = makeSeg(pt_id, prev_id)
r.segments.append(s)
prev_row += 1
prev_row = this_row
return r
def makeOsmUniqueID(namespace, el_id):
"""Make UniqueID message for *el_id* number in OSM sub-namespace *namespace*.
:param namespace: OSM sub-namespace
:type namespace: string
:param el_id: OSM identifier within that namespace
:type el_id: int or string containing an integer
:returns: corresponding `geographic_msgs/UniqueID`_ message.
:raises: :exc:`ValueError`
"""
if namespace not in set(['node', 'way', 'relation']):
raise ValueError('invalid OSM namespace: ' + namespace)
ns = 'http://openstreetmap.org/' + namespace + '/'
return unique_id.toMsg(unique_id.fromURL(ns + str(el_id)))
def makeOsmUniqueID(namespace, el_id):
"""Make UniqueID message for *el_id* number in OSM sub-namespace *namespace*.
:param namespace: OSM sub-namespace
:type namespace: string
:param el_id: OSM identifier within that namespace
:type el_id: int or string containing an integer
:returns: corresponding `geographic_msgs/UniqueID`_ message.
:raises: :exc:`ValueError`
"""
if not namespace in set(['node', 'way', 'relation']):
raise ValueError('invalid OSM namespace: ' + namespace)
ns = 'http://openstreetmap.org/' + namespace + '/'
return unique_id.toMsg(unique_id.fromURL(ns + str(el_id)))
def makeSeg(start, end, oneway=False):
""" Make RouteSegment message.
:param start: Initial UUID.
:param end: Final UUID.
:param oneway: True if segment is one-way.
:returns: RouteSegment message.
"""
uu = unique_id.toMsg(unique_id.fromURL(PKG_URL + '/'
+ str(start) + '/' + str(end)))
seg = RouteSegment(id = uu,
start = UniqueID(uuid=start),
end = UniqueID(uuid=end))
if oneway:
seg.props.append(KeyValue(key = 'oneway', value = 'yes'))
return seg
def makeSeg(start, end, oneway=False):
""" Make RouteSegment message.
:param start: Initial UUID.
:param end: Final UUID.
:param oneway: True if segment is one-way.
:returns: RouteSegment message.
"""
uu = unique_id.toMsg(unique_id.fromURL(PKG_URL + '/'
+ str(start) + '/' + str(end)))
seg = RouteSegment(id = uu,
start = UniqueID(uuid=start),
end = UniqueID(uuid=end))
if oneway:
seg.props.append(KeyValue(key = 'oneway', value = 'yes'))
return seg
# ROS dependencies
import unique_id
from rocon_app_manager_msgs.msg import Rapp
from rocon_std_msgs.msg import PlatformInfo
from concert_msgs.msg import ConcertClient, ConcertClientState
from scheduler_msgs.msg import Request, Resource
from scheduler_msgs.msg import CurrentStatus, KnownResources
from concert_scheduler_requests.transitions import ActiveRequest
# module being tested:
from concert_resource_pool.resource_pool import *
# some definitions for testing
RQ_UUID = uuid.UUID('01234567-89ab-cdef-0123-456789abcdef')
TEST_UUID = unique_id.fromURL('package://concert_simple_scheduler/test_uuid')
DIFF_UUID = unique_id.fromURL('package://concert_simple_scheduler/diff_uuid')
EXAMPLE_RAPP = 'tests/example_rapp'
TELEOP_RAPP = 'rocon_apps/teleop'
TEST_RAPPS = [TELEOP_RAPP, EXAMPLE_RAPP]
TEST_STATUS = CurrentStatus(uri='rocon:/segbot/roberto', rapps=[EXAMPLE_RAPP])
TEST_RESOURCE_NAME = 'rocon:/segbot/roberto'
TEST_RESOURCE = Resource(uri=TEST_RESOURCE_NAME,
id=unique_id.toMsg(TEST_UUID),
rapp=EXAMPLE_RAPP)
TEST_RESOURCE_STRING = ("""rocon:/segbot/roberto, status: 0
owner: None
priority: 0
rapps:
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=unique_id.toMsg(unique_id.fromURL(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 is not 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 is not 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 is not None:
feature.props.append(kv)
gmap.features.append(feature)
return gmap
# ROS dependencies
import unique_id
from rocon_app_manager_msgs.msg import Rapp
from rocon_std_msgs.msg import PlatformInfo
from concert_msgs.msg import ConcertClient, ConcertClientState
from scheduler_msgs.msg import Request, Resource
from scheduler_msgs.msg import CurrentStatus, KnownResources
from concert_scheduler_requests.transitions import ActiveRequest
# module being tested:
from concert_resource_pool.resource_pool import *
# some definitions for testing
RQ_UUID = uuid.UUID('01234567-89ab-cdef-0123-456789abcdef')
TEST_UUID = unique_id.fromURL('package://concert_simple_scheduler/test_uuid')
DIFF_UUID = unique_id.fromURL('package://concert_simple_scheduler/diff_uuid')
EXAMPLE_RAPP = 'tests/example_rapp'
TELEOP_RAPP = 'rocon_apps/teleop'
TEST_RAPPS = [TELEOP_RAPP, EXAMPLE_RAPP]
TEST_STATUS = CurrentStatus(uri='rocon:/segbot/roberto', rapps=[EXAMPLE_RAPP])
TEST_RESOURCE_NAME = 'rocon:/segbot/roberto'
TEST_RESOURCE = Resource(uri=TEST_RESOURCE_NAME,
id=unique_id.toMsg(TEST_UUID),
rapp=EXAMPLE_RAPP)
TEST_RESOURCE_STRING = (
"""rocon:/segbot/roberto, status: 0
owner: None
priority: 0
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=unique_id.toMsg(unique_id.fromURL(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