Esempio n. 1
0
def get_transform(desc: MapFormat1Object, W: int,
                  tile_size: float) -> SE2Transform:
    rotate_deg = desc.get("rotate", 0)
    rotate = np.deg2rad(rotate_deg)
    if "pos" in desc:

        pos = desc["pos"]
        x = float(pos[0]) * tile_size
        # account for non-righthanded
        y = float(W - 1 - pos[1]) * tile_size
        # account for non-righthanded
        rotate = -rotate
        transform = SE2Transform([x, y], rotate)
        return transform

    elif "pose" in desc:
        # noinspection PyTypedDict
        pose = Serializable.from_json_dict(desc["pose"])
        return pose
    elif "place" in desc:
        # noinspection PyTypedDict
        place = desc["place"]
        tile_coords = tuple(place["tile"])
        relative = Serializable.from_json_dict(place["relative"])
        p, theta = relative.p, relative.theta
        i, j = tile_coords

        fx = (i + 0.5) * tile_size + p[0]
        fy = (j + 0.5) * tile_size + p[1]
        transform = SE2Transform([fx, fy], theta)
        # logger.info(tile_coords=tile_coords, tile_size=tile_size, transform=transform)
        return transform

    elif "attach" in desc:
        # noinspection PyTypedDict
        attach = desc["attach"]
        tile_coords = tuple(attach["tile"])
        slot = str(attach["slot"])

        x, y = get_xy_slot(slot)
        i, j = tile_coords

        u, v = (x + i) * tile_size, (y + j) * tile_size
        transform = SE2Transform([u, v], rotate)

        q = transform.as_SE2()

        return SE2Transform.from_SE2(q)
    else:
        msg = "Cannot find positiong"
        raise ZValueError(msg, desc=desc)
def wb2():
    root = PlacedObject()

    for map_name in list_maps():
        tm = load_map(map_name)
        root.set_object(map_name, tm)

    d = root.as_json_dict()
    # print(json.dumps(d, indent=4))
    # print(yaml.safe_dump(d, default_flow_style=False))

    # print('------')
    r1 = Serializable.from_json_dict(d)
    d1 = r1.as_json_dict()
Esempio n. 3
0
def read_log(filename):
    with open(filename) as i:
        for k, line in enumerate(i.readlines()):
            try:
                j = json.loads(line)
            except BaseException:
                msg = 'Cannot interpret json in line %s: "%s"' % (k, line)
                raise Exception(msg)
            try:
                ob = Serializable.from_json_dict(j)
            except BaseException:
                msg = 'Cannot de-serialize in line %s:\n%s' % (k, j)
                raise Exception(msg)

            yield ob
Esempio n. 4
0
def get_object(desc: MapFormat1Object):
    kind = desc["kind"]

    attrs = {}
    if "tag" in desc:
        tag_desc = desc["tag"]
        # tag_id = tag_desc.get('tag_id')
        # size = tag_desc.get('size', DEFAULT_TAG_SIZE)
        # family = tag_desc.get('family', DEFAULT_FAMILY)
        attrs["tag"] = Serializable.from_json_dict(tag_desc)

    kind2klass = {
        "trafficlight": TrafficLight,
        "duckie": Duckie,
        "cone": Cone,
        "barrier": Barrier,
        "building": Building,
        "duckiebot": DB18,
        "tree": Tree,
        "house": House,
        "bus": Bus,
        "truck": Truck,
        "floor_tag": FloorTag,
    }
    kind2klass.update(SIGNS)
    if kind in kind2klass:
        klass = kind2klass[kind]
        try:
            obj = klass(**attrs)
        except TypeError as e:
            msg = "Could not initialize %s with attrs %s:\n%s" % (
                klass.__name__,
                attrs,
                traceback.format_exc(),
            )
            raise Exception(msg) from e

    else:
        logger.debug("Do not know special kind %s" % kind)
        obj = GenericObject(kind=kind)
    return obj
def get_object(desc):
    kind = desc['kind']

    attrs = {}
    if 'tag' in desc:
        tag_desc = desc['tag']
        # tag_id = tag_desc.get('tag_id')
        # size = tag_desc.get('size', DEFAULT_TAG_SIZE)
        # family = tag_desc.get('family', DEFAULT_FAMILY)
        attrs['tag'] = Serializable.from_json_dict(tag_desc)

    kind2klass = {
        'trafficlight': TrafficLight,
        'duckie': Duckie,
        'cone': Cone,
        'barrier': Barrier,
        'building': Building,
        'duckiebot': DB18,
        'tree': Tree,
        'house': House,
        'bus': Bus,
        'truck': Truck,
        'floor_tag': FloorTag,
    }
    kind2klass.update(SIGNS)
    if kind in kind2klass:
        klass = kind2klass[kind]
        try:
            obj = klass(**attrs)
        except TypeError:
            msg = 'Could not initialize %s with attrs %s:\n%s' % (
                klass.__name__, attrs, traceback.format_exc())
            raise Exception(msg)

    else:
        logger.debug('Do not know special kind %s' % kind)
        obj = GenericObject(kind=kind)
    return obj
def get_transform(desc, tm, tile_size):
    rotate_deg = desc.get('rotate', 0)
    rotate = np.deg2rad(rotate_deg)

    if 'pos' in desc:
        pos = desc['pos']
        x = float(pos[0]) * tile_size
        # account for non-righthanded
        y = float(tm.W - pos[1]) * tile_size
        # account for non-righthanded
        rotate = -rotate
        transform = SE2Transform([x, y], rotate)
        return transform

    else:

        if 'pose' in desc:
            pose = Serializable.from_json_dict(desc['pose'])
        else:
            pose = SE2Transform.identity()

        if 'attach' in desc:
            attach = desc['attach']
            tile_coords = tuple(attach['tile'])
            slot = str(attach['slot'])

            x, y = get_xy_slot(slot)
            i, j = tile_coords

            u, v = (x + i) * tile_size, (y + j) * tile_size
            transform = SE2Transform([u, v], rotate)

            q = geo.SE2.multiply(transform.as_SE2(), pose.as_SE2())

            return SE2Transform.from_SE2(q)
        else:
            return pose
def wb1():
    outdir = get_comptests_output_dir()
    root = PlacedObject()
    tile_map = create_map(H=3, W=3)

    world = PlacedObject()
    root.set_object('world', world)

    placement = Constant[SE2Transform](SE2Transform.identity())

    world.set_object('map1', tile_map, ground_truth=placement)

    ego = PlacedObject()
    world_coordinates = Constant[SE2Transform](SE2Transform([0, 0], 0))

    world.set_object('ego', ego, ground_truth=world_coordinates)

    d = root.as_json_dict()
    # print(json.dumps(DW.root.as_json_dict(), indent=4))
    # print(yaml.safe_dump(d, default_flow_style=False))
    # print('------')
    r1 = Serializable.from_json_dict(d)
    # print('read: %s' % r1)
    d1 = r1.as_json_dict()
def load_tile_types():
    s = yaml.load(data)
    templates = Serializable.from_json_dict(s)
    return templates
Esempio n. 9
0
 def params_from_json_dict(cls, d):
     a = d.pop("a", [])
     b = d.pop("b")
     transform = d.pop("transform")
     transform = Serializable.from_json_dict(transform)
     return dict(a=a, b=b, transform=transform)
Esempio n. 10
0
 def params_from_json_dict(cls, d):
     a = d.pop('a', [])
     b = d.pop('b')
     transform = d.pop('transform')
     transform = Serializable.from_json_dict(transform)
     return dict(a=a, b=b, transform=transform)