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

2026-09-20 12:18:52 +08:00
commit 492676344a
143 changed files with 13010 additions and 0 deletions
@@ -0,0 +1,65 @@
+"""DR semantic regression tests independent of ROS transport."""
+import copy
+import json
+import math
+import sys
+import tempfile
+import unittest
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path[:0] = [str(ROOT), str(ROOT / 'coordinator'), str(ROOT / 'robobrain')]
+
+
+class DrSemanticsTests(unittest.TestCase):
+    def test_localization_adapter_preserves_all_dr_context_fields(self):
+        sys.path.insert(0,str(ROOT/'ros2/robobrain_services'))
+        from robobrain_services.nodes import perception_goal
+        from types import SimpleNamespace as S
+        goal=S(task_id='t',subtask_id='s',target_ref='water',target_description='bottle',
+               shelf_id='shelf_A',column_id='2',tier_id='3',station_binding_ref='binding-9',
+               expected_geometry_epoch=7,capture_after=S(sec=1,nanosec=5),timeout=S(sec=2,nanosec=0))
+        result=perception_goal(goal,'localize')
+        for key in ('shelf_id','column_id','tier_id','station_binding_ref','expected_geometry_epoch'):
+            self.assertEqual(result[key],getattr(goal,key))
+
+    def test_semantic_navigation_accepts_positive_subsecond_budget(self):
+        from navigation_gateway.semantic_proxy import duration_seconds
+        from types import SimpleNamespace
+        self.assertAlmostEqual(duration_seconds(SimpleNamespace(sec=0,nanosec=500000000)),.5)
+        for sec,nanosec in [(0,0),(-1,1),(3600,1),(1,1000000000),(1,-1)]:
+            with self.assertRaises(ValueError):duration_seconds(SimpleNamespace(sec=sec,nanosec=nanosec))
+
+    def test_shelf_tier_can_be_empty_without_guessing(self):
+        # BT p15 and RB02 allow an unknown tier; execution must decide whether
+        # its chosen route can use the observation without that calibration.
+        from robot_robobrain.service import BrainService
+        from robot_robobrain.backends import FixtureBackend
+        from robot_robobrain.observations import Observation
+        with tempfile.TemporaryDirectory() as folder:
+            image = Path(folder) / 'frame.jpg'; image.write_bytes(b'fixture')
+            raw = dict(status='SUCCEEDED', shelf_id='shelf_A', side_id='FRONT',
+                       column_id='1', tier_id='', confidence=.95)
+            service = BrainService(FixtureBackend(json.dumps(raw)), folder)
+            observation = Observation('obs', 100, 'camera', str(image), 'observe_A', 1, 'shelf_A')
+            goal = dict(task_id='t', subtask_id='s', target_ref='water', source_region_ref='shelf_A',
+                        observation_station_id='observe_A', station_registry_version=1, capture_after=90, timeout=1)
+            result = service.shelf(goal, observation, 110, max_age_ns=50)
+            self.assertEqual(result['status'], 'SUCCEEDED')
+            self.assertEqual(result['tier_id'], '')
+
+    def test_navigation_reports_signed_shortest_yaw_error(self):
+        from navigation_gateway.gateway import pose_errors
+        from test_navigation_gateway import request
+        target = request()['target_pose']
+        current = copy.deepcopy(target)
+        current['orientation'].update(z=math.sin(.2/2), w=math.cos(.2/2))
+        distance, yaw = pose_errors(target, current)
+        self.assertEqual(distance, 0)
+        self.assertAlmostEqual(yaw, -.2)
+        current['orientation'].update(z=math.sin(-.2/2), w=math.cos(-.2/2))
+        self.assertAlmostEqual(pose_errors(target, current)[1], .2)
+
+
+if __name__ == '__main__':
+    unittest.main()