Unify navigation on NavigateToPose and remove legacy proxies
This commit is contained in:
+80
-69
@@ -1,4 +1,5 @@
|
||||
import json
|
||||
import math
|
||||
import pathlib
|
||||
import sys
|
||||
import threading
|
||||
@@ -34,9 +35,6 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
goal.target_pose.header.frame_id = "map"
|
||||
goal.target_pose.pose.orientation.w = 1.0
|
||||
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
|
||||
elif name == "execute_manipulation":
|
||||
goal.skill, goal.instruction = "pick", "pick bottle"
|
||||
goal.target.object_ref, goal.target.description = "bottle", "bottle"
|
||||
@@ -78,8 +76,8 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.node._accepted(name, handle)
|
||||
return handle, self.node._execute(name, handle)
|
||||
|
||||
def test_all_twelve_actions_execute_success_and_failure(self):
|
||||
self.assertEqual(len(self.module.ACTION_TYPES), 12)
|
||||
def test_all_eleven_actions_execute_success_and_failure(self):
|
||||
self.assertEqual(len(self.module.ACTION_TYPES), 11)
|
||||
self.assertEqual(self.module.ACTION_ENDPOINTS["plan_task"], "tasks/plan")
|
||||
self.assertEqual(self.module.ACTION_ENDPOINTS["execute_task"], "tasks/execute")
|
||||
self.assertEqual(self.module.ACTION_ENDPOINTS["evaluate_progress"], "monitor/evaluate_progress")
|
||||
@@ -89,7 +87,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.SUCCEEDED if name == "navigate" else result.result.COMPLETED)
|
||||
self.assertEqual(result.result.status, result.result.COMPLETED)
|
||||
self.assertEqual(result.result.stop_state, result.result.CONFIRMED)
|
||||
elif hasattr(result, "status"):
|
||||
self.assertNotEqual(result.status, result.FAILED)
|
||||
@@ -118,7 +116,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
|
||||
def test_success_feedback_follows_normal_lifecycle_and_populates_fields(self):
|
||||
cases = {
|
||||
"navigate": [0, 1, 2],
|
||||
"navigate": [0, 1, 2, 3],
|
||||
"execute_manipulation": [0, 1, 2, 3, 4],
|
||||
"execute_posture": [0, 1, 2],
|
||||
}
|
||||
@@ -131,7 +129,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.current_pose_valid and nav.error_valid)
|
||||
self.assertTrue(nav.current_pose_valid and nav.error_valid and nav.blocked_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,11 +137,11 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
|
||||
def test_each_action_feedback_lifecycle_and_navigation_recovery_execute(self):
|
||||
expected = {
|
||||
"navigate": [0, 1, 2], "execute_manipulation": [0, 1, 2, 3, 4],
|
||||
"navigate": [0, 1, 2, 3], "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],
|
||||
"navigate_semantic": [0], "evaluate_progress": [0],
|
||||
"evaluate_progress": [0],
|
||||
}
|
||||
for name, phases in expected.items():
|
||||
self.node.counts[name] = 0
|
||||
@@ -153,8 +151,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, 2])
|
||||
self.assertEqual([item.blocked for item in recovery.feedback], [False, False, False, True, False])
|
||||
self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 4, 3])
|
||||
self.assertEqual([item.blocked for item in recovery.feedback], [False, 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)
|
||||
@@ -173,22 +171,22 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.assertTrue(thread.is_alive(), "cancel ACK must precede delayed stop termination")
|
||||
thread.join(1)
|
||||
result = box["result"]
|
||||
self.assertEqual((handle.native, result.result.status, result.result.stop_state),
|
||||
("canceled", result.result.CANCELED, result.result.CONFIRMED))
|
||||
self.assertEqual(handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING)
|
||||
self.assertEqual((handle.native, result.status, result.stop_state),
|
||||
("canceled", result.CANCELED, result.STOP_CONFIRMED))
|
||||
self.assertEqual(handle.feedback[-1].phase, self.module.NavigateToPose.Feedback.STOPPING)
|
||||
self.node.motion_reserved = False; self.node.counts["navigate"] = 0
|
||||
goal = self.goal("navigate"); goal.timeout.sec = 0; goal.timeout.nanosec = 1_000_000
|
||||
self.node.scenarios["navigate"] = [{"kind": "timeout"}]
|
||||
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.TIMEOUT))
|
||||
self.assertEqual(timeout_handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING)
|
||||
self.assertEqual((timeout_handle.native, timeout_result.status), ("aborted", timeout_result.TIMEOUT))
|
||||
self.assertEqual(timeout_handle.feedback[-1].phase, self.module.NavigateToPose.Feedback.STOPPING)
|
||||
self.node.motion_reserved = False; self.node.counts["navigate"] = 0
|
||||
unknown_handle, unknown = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
|
||||
self.assertEqual(unknown.result.stop_state, unknown.result.UNKNOWN)
|
||||
self.assertEqual(unknown.stop_state, unknown.STOP_UNKNOWN)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
self.assertEqual(unknown.result.stop_evidence_ref, "")
|
||||
self.assertEqual(unknown.stop_evidence_ref, "")
|
||||
|
||||
def test_cancel_and_timeout_can_acknowledge_without_confirming_stop(self):
|
||||
handle, canceled = self.execute(
|
||||
@@ -203,7 +201,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)
|
||||
timed_out = self.node._execute("navigate", timeout_handle)
|
||||
self.assertEqual(timed_out.result.stop_state, timed_out.result.UNKNOWN)
|
||||
self.assertEqual(timed_out.stop_state, timed_out.STOP_UNKNOWN)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
|
||||
def test_navigation_status_mapping_and_readiness_reasons(self):
|
||||
@@ -211,7 +209,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
("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",
|
||||
("INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "EXECUTION_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
|
||||
@@ -220,17 +218,17 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
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.status, handle.native), (status, native))
|
||||
self.assertEqual(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, ""))
|
||||
self.assertEqual((result.status, result.stop_state), (5, 0))
|
||||
self.assertEqual((result.stopped_at.sec, result.stopped_at.nanosec,
|
||||
result.stop_evidence_ref), (0, 0, ""))
|
||||
|
||||
def test_navigation_goal_requires_map_and_finite_nonzero_quaternion(self):
|
||||
mutations = (
|
||||
@@ -256,7 +254,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
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.assertEqual(result.status, 0)
|
||||
self.assertTrue(result.final_pose_valid)
|
||||
self.assertEqual(result.final_pose.pose.orientation.w, 1.0)
|
||||
self.assertTrue(handle.feedback)
|
||||
@@ -266,7 +264,6 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
def test_action_specific_malformed_goals_are_rejected(self):
|
||||
mutations = {
|
||||
"navigate": lambda g: setattr(g.target_pose.header, "frame_id", ""),
|
||||
"navigate_semantic": lambda g: setattr(g, "registry_version", 0),
|
||||
"execute_manipulation": lambda g: setattr(g.target, "object_ref", ""),
|
||||
"execute_posture": lambda g: setattr(g, "expected_geometry_epoch", 0),
|
||||
"plan_task": lambda g: setattr(g, "known_info_json", "[]"),
|
||||
@@ -283,56 +280,65 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
with self.subTest(action=name):
|
||||
self.assertEqual(self.node._goal(name, goal), self.module.GoalResponse.REJECT)
|
||||
|
||||
def test_semantic_navigation_accepts_production_kind_matrix_only(self):
|
||||
accepted = (
|
||||
("LOCATION", "destination_A", {}),
|
||||
("OBJECT", "bottle", {}),
|
||||
("CELL", "bottle", {"shelf_id": "shelf_A", "side_id": "FRONT",
|
||||
"column_id": "1", "tier_id": "2"}),
|
||||
)
|
||||
for kind, reference, fields in accepted:
|
||||
goal = self.goal("navigate_semantic")
|
||||
goal.kind, goal.reference = kind, reference
|
||||
for field, value in fields.items(): setattr(goal, field, value)
|
||||
with self.subTest(kind=kind):
|
||||
self.assertEqual(self.node._goal("navigate_semantic", goal), self.module.GoalResponse.ACCEPT)
|
||||
self.node.inflight, self.node.motion_reserved = 0, False
|
||||
cell = self.goal("navigate_semantic"); cell.kind = "CELL"
|
||||
self.assertEqual(self.node._goal("navigate_semantic", cell), self.module.GoalResponse.REJECT)
|
||||
for invented in ("region", "shelf", "station"):
|
||||
goal = self.goal("navigate_semantic"); goal.kind = invented
|
||||
with self.subTest(invented=invented):
|
||||
self.assertEqual(self.node._goal("navigate_semantic", goal), self.module.GoalResponse.REJECT)
|
||||
|
||||
def test_semantic_navigation_pose_is_explicit_and_validated(self):
|
||||
def test_navigation_explicit_pose_preserves_measurements_and_epoch(self):
|
||||
pose = {"frame_id": "map", "x": 1.25, "y": -2.5, "z": 0.0,
|
||||
"qx": 0.0, "qy": 0.0, "qz": 0.0, "qw": 1.0}
|
||||
_, explicit = self.execute("navigate_semantic", {"kind": "normal", "duration_seconds": 0,
|
||||
"final_pose": pose})
|
||||
self.assertTrue(explicit.pose_valid and explicit.errors_valid)
|
||||
self.assertEqual(explicit.final_pose.header.frame_id, "map")
|
||||
self.assertEqual((explicit.final_pose.pose.position.x, explicit.final_pose.pose.position.y), (1.25, -2.5))
|
||||
self.node.counts["navigate_semantic"] = 0
|
||||
_, unspecified = self.execute("navigate_semantic", {"kind": "normal", "duration_seconds": 0})
|
||||
self.assertFalse(unspecified.pose_valid)
|
||||
self.assertFalse(unspecified.errors_valid)
|
||||
for raw in (
|
||||
'{"navigate_semantic":{"final_pose":{"frame_id":"map","x":NaN,"y":0,"z":0,"qx":0,"qy":0,"qz":0,"qw":1}}}',
|
||||
'{"navigate_semantic":{"final_pose":{"frame_id":"map","x":1,"y":2,"z":0,"qx":0,"qy":0,"qz":0,"qw":0}}}',
|
||||
):
|
||||
with self.subTest(raw=raw), self.assertRaises(ValueError):
|
||||
self.module.parse_scenarios(raw)
|
||||
fixture = {"kind": "normal", "duration_seconds": 0, "final_pose": pose}
|
||||
self.module.parse_scenarios(json.dumps({"navigate": fixture}))
|
||||
before = self.node.geometry_epoch
|
||||
_, result = self.execute("navigate", fixture)
|
||||
self.assertEqual(result.status, result.SUCCEEDED)
|
||||
self.assertTrue(result.final_pose_valid)
|
||||
self.assertEqual((result.final_pose.pose.position.x, result.final_pose.pose.position.y), (1.25, -2.5))
|
||||
self.assertGreater(result.final_position_error, 2.5)
|
||||
self.assertEqual(result.final_yaw_error, 0.0)
|
||||
self.assertEqual(self.node.geometry_epoch, before + 1)
|
||||
self.assertEqual(result.stop_state, result.STOP_CONFIRMED)
|
||||
self.assertTrue(result.stop_evidence_ref)
|
||||
self.assertFalse(hasattr(result, "result"))
|
||||
for invalid in (dict(pose, x=float("nan")), dict(pose, qw=0.0), dict(pose, frame_id="odom")):
|
||||
with self.subTest(pose=invalid), self.assertRaises(ValueError):
|
||||
self.module.parse_scenarios(json.dumps({"navigate": {"final_pose": invalid}}))
|
||||
|
||||
def test_navigation_final_yaw_error_is_signed_shortest_target_minus_current(self):
|
||||
for target_deg, current_deg, expected_deg in ((30, 10, 20), (10, 30, -20),
|
||||
(-170, 170, 20), (170, -170, -20)):
|
||||
with self.subTest(target=target_deg, current=current_deg):
|
||||
target, current = math.radians(target_deg), math.radians(current_deg)
|
||||
goal = self.goal("navigate")
|
||||
goal.target_pose.pose.orientation.z = math.sin(target / 2)
|
||||
goal.target_pose.pose.orientation.w = math.cos(target / 2)
|
||||
fixture = {"kind": "normal", "duration_seconds": 0, "final_pose": {
|
||||
"frame_id": "map", "x": 0.0, "y": 0.0, "z": 0.0,
|
||||
"qx": 0.0, "qy": 0.0, "qz": math.sin(current / 2), "qw": math.cos(current / 2)}}
|
||||
self.module.parse_scenarios(json.dumps({"navigate": fixture}))
|
||||
self.node.scenarios["navigate"] = [fixture]
|
||||
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.status, result.SUCCEEDED)
|
||||
self.assertAlmostEqual(result.final_yaw_error, math.radians(expected_deg), places=12)
|
||||
|
||||
def test_navigation_goal_has_native_identity_without_trace(self):
|
||||
goal = self.goal("navigate")
|
||||
self.assertFalse(hasattr(goal, "trace"))
|
||||
for field in ("task_id", "subtask_id"):
|
||||
invalid = self.goal("navigate")
|
||||
setattr(invalid, field, "")
|
||||
with self.subTest(field=field):
|
||||
self.assertEqual(self.node._goal("navigate", invalid), self.module.GoalResponse.REJECT)
|
||||
|
||||
def test_enabled_interfaces_can_exclude_executor_owned_endpoints(self):
|
||||
module = load_mock_module({
|
||||
"scenarios_json": "{}", "max_goal_seconds": 1.0,
|
||||
"allowed_postures": ["pregrasp", "transport", "home"],
|
||||
"initial_holding_state": "UNKNOWN",
|
||||
"enabled_actions": ["plan_task", "navigate_semantic"],
|
||||
"enabled_actions": ["plan_task", "navigate"],
|
||||
"enabled_topics": ["robot_state"],
|
||||
})
|
||||
node = module.MockSkills()
|
||||
self.assertEqual(node.enabled_actions, ("plan_task", "navigate_semantic"))
|
||||
self.assertEqual(node.enabled_actions, ("plan_task", "navigate"))
|
||||
self.assertEqual(len(node.servers), 2)
|
||||
self.assertTrue(hasattr(node, "state_pub"))
|
||||
self.assertFalse(hasattr(node, "registry_pub"))
|
||||
@@ -352,7 +358,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.assertEqual(missing.error_code, "UNKNOWN_ROBOT")
|
||||
handle, unresolved = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
|
||||
goal_id = bytes(handle.goal_id.uuid).hex()
|
||||
self.assertEqual(unresolved.result.stop_state, unresolved.result.UNKNOWN)
|
||||
self.assertEqual(unresolved.stop_state, unresolved.STOP_UNKNOWN)
|
||||
req = srv.ReconcileGoal.Request(trace=self.trace(), goal_id="never-accepted", operator_id="op", reason="review")
|
||||
req.evidence.status = req.evidence.PASSED
|
||||
req.evidence.context.source_goal_id = "never-accepted"
|
||||
@@ -367,6 +373,11 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.assertFalse(rejected.accepted)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
req.goal_id = req.evidence.context.source_goal_id = goal_id
|
||||
req.trace.subtask_id = req.evidence.context.trace.subtask_id = "other"
|
||||
wrong_identity = self.node._reconcile(req, srv.ReconcileGoal.Response())
|
||||
self.assertFalse(wrong_identity.accepted)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
req.trace.subtask_id = req.evidence.context.trace.subtask_id = "s"
|
||||
accepted = self.node._reconcile(req, srv.ReconcileGoal.Response())
|
||||
self.assertTrue(accepted.accepted)
|
||||
self.assertFalse(self.node.motion_reserved)
|
||||
@@ -445,7 +456,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.SUCCEEDED)
|
||||
self.assertEqual(result.status, result.SUCCEEDED)
|
||||
|
||||
def test_topic_and_result_fixture_values_are_finite_and_bounded(self):
|
||||
for raw in (
|
||||
|
||||
Reference in New Issue
Block a user