21 lines
1.5 KiB
Python
21 lines
1.5 KiB
Python
"""Navigation-owned immutable, versioned exact-match lookup. No fuzzy guesses."""
|
|
import copy,math
|
|
class Catalog:
|
|
def __init__(self,site):
|
|
self.site=copy.deepcopy(site)
|
|
if type(site.get('registry_version')) is not int or site['registry_version']<=0:raise ValueError('registry version required')
|
|
def resolve(self,kind,reference,version,*,shelf='',side='',column='',tier=''):
|
|
if type(version) is not int or version!=self.site['registry_version']:raise ValueError('REGISTRY_VERSION_MISMATCH')
|
|
if kind=='LOCATION':location=reference
|
|
elif kind=='OBJECT':location=self.site.get('object_locations',{}).get(reference)
|
|
elif kind=='CELL':
|
|
if any(not isinstance(x,str) or not x or '/' in x for x in (shelf,side,column,tier)):raise ValueError('INVALID_CELL')
|
|
location=self.site.get('cell_locations',{}).get('/'.join((shelf,side,column,tier)))
|
|
else:raise ValueError('UNKNOWN_LOOKUP_KIND')
|
|
pose=self.site.get('locations',{}).get(location)
|
|
if not isinstance(pose,dict) or pose.get('frame_id')!='map':raise ValueError('NOT_FOUND')
|
|
for k in ('x','y','z','qx','qy','qz','qw'):
|
|
if type(pose.get(k)) not in (int,float) or not math.isfinite(pose[k]):raise ValueError('INVALID_POSE')
|
|
if abs(sum(pose[k]**2 for k in ('qx','qy','qz','qw'))-1)>.001:raise ValueError('INVALID_QUATERNION')
|
|
return dict(location_id=location,pose=copy.deepcopy(pose),registry_version=version)
|