Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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)))
Exemplo n.º 4
0
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)))
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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
Exemplo n.º 7
0
# 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:
Exemplo n.º 8
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 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
Exemplo n.º 10
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