wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: align navigation contract and readiness handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_mock_runtime.py
+66 -8
View File
@@ -33,7 +33,7 @@ class MockRuntimeTests(unittest.TestCase):
if name == "navigate":
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.orientation.w = 1.0
goal.position_tolerance = goal.orientation_tolerance = 0.1
goal.position_tolerance = goal.yaw_tolerance = 0.1
elif name == "navigate_semantic":
goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
goal.position_tolerance = goal.orientation_tolerance = 0.1
@@ -89,7 +89,7 @@ class MockRuntimeTests(unittest.TestCase):
handle, result = self.execute(name, {"kind": positive_kind, "duration_seconds": 0})
self.assertEqual(handle.native, "succeeded")
if hasattr(result, "result"):
self.assertEqual(result.result.status, result.result.COMPLETED)
self.assertEqual(result.result.status, result.result.SUCCEEDED if name == "navigate" else result.result.COMPLETED)
self.assertEqual(result.result.stop_state, result.result.CONFIRMED)
elif hasattr(result, "status"):
self.assertNotEqual(result.status, result.FAILED)
@@ -118,7 +118,7 @@ class MockRuntimeTests(unittest.TestCase):
def test_success_feedback_follows_normal_lifecycle_and_populates_fields(self):
cases = {
"navigate": [0, 1, 2, 5],
"navigate": [0, 1, 2],
"execute_manipulation": [0, 1, 2, 3, 4],
"execute_posture": [0, 1, 2],
}
@@ -131,7 +131,7 @@ class MockRuntimeTests(unittest.TestCase):
self.assertTrue(all(item.stamp.sec > 0 and item.message for item in handle.feedback))
self.assertNotIn(getattr(self.module.ACTION_TYPES[name].Feedback, "STOPPING"), expected)
nav = self.execute("navigate", {"kind": "normal", "duration_seconds": 0.05})[0].feedback[-1]
self.assertTrue(nav.pose_valid and nav.errors_valid and nav.blocked_valid)
self.assertTrue(nav.current_pose_valid and nav.error_valid)
self.assertFalse(nav.blocked)
self.assertGreaterEqual(nav.elapsed_time.nanosec, 0)
manipulation = self.execute("execute_manipulation", {"kind": "normal", "duration_seconds": 0.05})[0].feedback[-1]
@@ -139,7 +139,7 @@ class MockRuntimeTests(unittest.TestCase):
def test_each_action_feedback_lifecycle_and_navigation_recovery_execute(self):
expected = {
"navigate": [0, 1, 2, 5], "execute_manipulation": [0, 1, 2, 3, 4],
"navigate": [0, 1, 2], "execute_manipulation": [0, 1, 2, 3, 4],
"plan_task": [0, 1, 2], "locate_shelf_column": [0, 1],
"localize_target_3d": [0, 1], "check_free_space": [0, 1],
"assess_grasp": [0], "execute_posture": [0, 1, 2], "verify_state": [0],
@@ -153,7 +153,8 @@ class MockRuntimeTests(unittest.TestCase):
self.assertEqual([item.phase for item in handle.feedback], phases)
self.node.counts["navigate"] = 0
recovery, _ = self.execute("navigate", {"kind": "obstacle_recovery", "duration_seconds": 0.7})
self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 4, 5])
self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 2])
self.assertEqual([item.blocked for item in recovery.feedback], [False, False, False, True, False])
self.node.counts["execute_task"] = 0
task, _ = self.execute("execute_task", {"kind": "normal", "duration_seconds": 0.05})
self.assertTrue(task.feedback[0].stage)
@@ -181,7 +182,7 @@ class MockRuntimeTests(unittest.TestCase):
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
timeout_handle = GoalHandle(goal); self.node._accepted("navigate", timeout_handle)
timeout_result = self.node._execute("navigate", timeout_handle)
self.assertEqual((timeout_handle.native, timeout_result.result.status), ("aborted", timeout_result.result.TIMED_OUT))
self.assertEqual((timeout_handle.native, timeout_result.result.status), ("aborted", timeout_result.result.TIMEOUT))
self.assertEqual(timeout_handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING)
self.node.motion_reserved = False; self.node.counts["navigate"] = 0
unknown_handle, unknown = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
@@ -205,6 +206,63 @@ class MockRuntimeTests(unittest.TestCase):
self.assertEqual(timed_out.result.stop_state, timed_out.result.UNKNOWN)
self.assertTrue(self.node.motion_reserved)
def test_navigation_status_mapping_and_readiness_reasons(self):
cases = [("normal", 0, "succeeded", ""), ("canceled", 1, "canceled", "MOCK_TERMINATED"),
("timeout", 2, "aborted", "MOCK_TERMINATED"), ("blocked", 3, "aborted", "BLOCKED"),
("failed", 5, "aborted", "MOCK_TERMINATED")]
cases += [("not_ready", 4, "aborted", code) for code in
("INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED",
"ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED")]
for kind, status, native, code in cases:
self.node.counts["navigate"] = 0
fixture = {"kind": "normal" if kind == "canceled" else kind, "duration_seconds": 0}
if kind == "not_ready": fixture["error_code"] = code
self.module.parse_scenarios(json.dumps({"navigate": fixture}))
handle, result = self.execute("navigate", fixture, cancel=kind == "canceled")
with self.subTest(kind=kind, code=code):
self.assertEqual((result.result.status, handle.native), (status, native))
self.assertEqual(result.result.error_code, code)
self.assertEqual(result.final_pose_valid, status == 0)
if result.final_pose_valid:
self.assertEqual((result.final_position_error, result.final_yaw_error), (0.0, 0.0))
self.assertFalse(hasattr(result, "errors_valid"))
self.node.counts["navigate"] = 0
_, result = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
self.assertEqual((result.result.status, result.result.stop_state), (5, 0))
self.assertEqual((result.result.stopped_at.sec, result.result.stopped_at.nanosec,
result.result.stop_evidence_ref), (0, 0, ""))
def test_navigation_goal_requires_map_and_finite_nonzero_quaternion(self):
mutations = (
lambda g: setattr(g.target_pose.header, "frame_id", "odom"),
lambda g: setattr(g.target_pose.pose.orientation, "w", 0.0),
lambda g: setattr(g.target_pose.pose.orientation, "w", float("nan")),
lambda g: setattr(g.target_pose.pose.position, "x", float("nan")),
lambda g: setattr(g, "yaw_tolerance", float("nan")),
)
for mutate in mutations:
self.node.inflight, self.node.motion_reserved = 0, False
goal = self.goal("navigate"); mutate(goal)
with self.subTest(mutate=mutate):
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.REJECT)
def test_navigation_accepts_large_yaw_tolerance_and_normalizes_quaternion(self):
for magnitude in (2.0, 1e308, 1e-308):
goal = self.goal("navigate")
goal.yaw_tolerance = 4.0
goal.target_pose.pose.orientation.w = magnitude
self.node.scenarios["navigate"] = [{"kind": "normal", "duration_seconds": 0.1}]
with self.subTest(magnitude=magnitude):
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
handle = GoalHandle(goal); self.node._accepted("navigate", handle)
result = self.node._execute("navigate", handle)
self.assertEqual(result.result.status, 0)
self.assertTrue(result.final_pose_valid)
self.assertEqual(result.final_pose.pose.orientation.w, 1.0)
self.assertTrue(handle.feedback)
self.assertTrue(all(item.current_pose.pose.orientation.w == 1.0 for item in handle.feedback))
self.assertEqual(goal.target_pose.pose.orientation.w, magnitude)
def test_action_specific_malformed_goals_are_rejected(self):
mutations = {
"navigate": lambda g: setattr(g.target_pose.header, "frame_id", ""),
@@ -387,7 +445,7 @@ class MockRuntimeTests(unittest.TestCase):
self.node.counts["navigate"] = 0
handle, result = self.execute("navigate", {"kind": "native_mismatch", "duration_seconds": 0})
self.assertEqual(handle.native, "aborted")
self.assertEqual(result.result.status, result.result.COMPLETED)
self.assertEqual(result.result.status, result.result.SUCCEEDED)
def test_topic_and_result_fixture_values_are_finite_and_bounded(self):
for raw in (