实现行为树执行器、任务协调和技能接口

2026-09-20 12:18:52 +08:00
commit 492676344a
143 changed files with 13010 additions and 0 deletions
@@ -0,0 +1,20 @@
+"""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)