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()
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
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
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)
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)
