509 lines
31 KiB
Python
509 lines
31 KiB
Python
import json
|
|
import math
|
|
import pathlib
|
|
import sys
|
|
import threading
|
|
import time
|
|
import unittest
|
|
|
|
sys.path.insert(0, str(pathlib.Path(__file__).resolve().parent.parent))
|
|
from tests.helpers.ros_shim import GoalHandle, load_mock_module
|
|
|
|
|
|
class MockRuntimeTests(unittest.TestCase):
|
|
def setUp(self):
|
|
self.module = load_mock_module({
|
|
"scenarios_json": "{}", "max_goal_seconds": 1.0,
|
|
"allowed_postures": ["pregrasp", "transport", "home"],
|
|
"initial_holding_state": "UNKNOWN",
|
|
})
|
|
self.node = self.module.MockSkills()
|
|
|
|
def trace(self):
|
|
T = sys.modules["bt_skill_interfaces.msg"].TaskTrace
|
|
return T(task_id="t", subtask_id="s", attempt=1, task_revision=1,
|
|
plan_version=1, run_id="r", execution_generation=1)
|
|
|
|
def goal(self, name):
|
|
action = self.module.ACTION_TYPES[name]
|
|
goal = action.Goal()
|
|
goal.timeout.sec = 1
|
|
if hasattr(goal, "trace"): goal.trace = self.trace()
|
|
if hasattr(goal, "task_id"): goal.task_id = "t"
|
|
if hasattr(goal, "subtask_id"): goal.subtask_id = "s"
|
|
if name == "navigate":
|
|
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 == "execute_manipulation":
|
|
goal.skill, goal.instruction = "pick", "pick bottle"
|
|
goal.target.object_ref, goal.target.description = "bottle", "bottle"
|
|
elif name == "execute_posture": goal.posture_id, goal.expected_geometry_epoch = "home", 1
|
|
elif name == "plan_task":
|
|
goal.instruction, goal.task_revision, goal.planning_generation = "fetch", 1, 1
|
|
goal.known_info_json = goal.context_snapshot_json = goal.constraints_json = "{}"
|
|
elif name == "verify_state":
|
|
goal.check, goal.expected_geometry_epoch = goal.PRECHECK, 1
|
|
goal.target.object_ref, goal.target.description = "bottle", "bottle"
|
|
elif name == "locate_shelf_column":
|
|
goal.target_ref, goal.target_description = "bottle", "bottle"
|
|
goal.source_region_ref, goal.observation_station_id, goal.station_registry_version = "shelf_zone", "station_A", 1
|
|
elif name == "localize_target_3d":
|
|
goal.expected_geometry_epoch, goal.target_ref, goal.target_description = 1, "bottle", "bottle"
|
|
goal.shelf_id, goal.column_id, goal.station_binding_ref = "shelf_A", "1", "station_A"
|
|
elif name == "check_free_space":
|
|
goal.destination_ref, goal.destination_description = "bin_A", "bin A"
|
|
goal.object_ref, goal.object_description, goal.placement_constraints_json = "bottle", "bottle", "{}"
|
|
elif name == "assess_grasp":
|
|
goal.allowed_posture_ids = ["pregrasp"]
|
|
goal.target_binding.target.object_ref, goal.target_binding.target.description = "bottle", "bottle"
|
|
goal.target_binding.context.schema_version = goal.target_binding.context.geometry_epoch = 1
|
|
goal.robot_state.robot_id = "robot_01"
|
|
goal.robot_state.valid_until.sec = 20
|
|
elif name == "evaluate_progress":
|
|
goal.task_description, goal.sequence = "fetch bottle", 1
|
|
goal.window_json = '[{"stamp":1,"views":{"front":"/tmp/frame.png"}}]'
|
|
elif name == "execute_task":
|
|
goal.approved_plan_json = '{"subtasks":[]}'
|
|
goal.context_json = "{}"
|
|
return goal
|
|
|
|
def execute(self, name, fixture, cancel=False):
|
|
self.node.scenarios[name] = [fixture]
|
|
goal = self.goal(name)
|
|
self.assertEqual(self.node._goal(name, goal), self.module.GoalResponse.ACCEPT)
|
|
handle = GoalHandle(goal, cancel=cancel)
|
|
self.node._accepted(name, handle)
|
|
return handle, self.node._execute(name, handle)
|
|
|
|
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")
|
|
for name in self.module.ACTION_TYPES:
|
|
with self.subTest(action=name, kind="normal"):
|
|
positive_kind = "passed" if name == "verify_state" else "normal"
|
|
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.stop_state, result.result.CONFIRMED)
|
|
elif hasattr(result, "status"):
|
|
self.assertNotEqual(result.status, result.FAILED)
|
|
elif hasattr(result, "decision"):
|
|
self.assertEqual(result.decision, result.DIRECT)
|
|
elif hasattr(result, "evidence"):
|
|
self.assertEqual(result.evidence.status, result.evidence.PASSED)
|
|
elif hasattr(result, "feedback_state"):
|
|
self.assertTrue(result.feedback_state.progress_valid)
|
|
self.assertEqual(result.error_code, "")
|
|
self.node.counts[name] = 0
|
|
with self.subTest(action=name, kind="failed"):
|
|
handle, result = self.execute(name, {"kind": "failed", "duration_seconds": 0})
|
|
self.assertEqual(handle.native, "aborted")
|
|
if hasattr(result, "result"):
|
|
self.assertEqual(result.result.status, result.result.FAILED)
|
|
elif hasattr(result, "status"):
|
|
self.assertEqual(result.status, result.FAILED)
|
|
elif hasattr(result, "decision"):
|
|
self.assertEqual(result.decision, result.UNKNOWN)
|
|
elif hasattr(result, "evidence"):
|
|
self.assertEqual(result.evidence.status, result.evidence.UNKNOWN)
|
|
elif hasattr(result, "feedback_state"):
|
|
self.assertFalse(result.feedback_state.progress_valid)
|
|
self.assertEqual(result.error_code, "MOCK_TERMINATED")
|
|
|
|
def test_success_feedback_follows_normal_lifecycle_and_populates_fields(self):
|
|
cases = {
|
|
"navigate": [0, 1, 2, 3],
|
|
"execute_manipulation": [0, 1, 2, 3, 4],
|
|
"execute_posture": [0, 1, 2],
|
|
}
|
|
for name, expected in cases.items():
|
|
self.node.counts[name] = 0
|
|
handle, _ = self.execute(name, {"kind": "normal", "duration_seconds": 0.55})
|
|
with self.subTest(action=name):
|
|
self.assertEqual([item.phase for item in handle.feedback], expected)
|
|
self.assertEqual([item.sequence for item in handle.feedback], list(range(1, len(expected) + 1)))
|
|
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 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]
|
|
self.assertFalse(manipulation.progress_valid)
|
|
|
|
def test_each_action_feedback_lifecycle_and_navigation_recovery_execute(self):
|
|
expected = {
|
|
"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],
|
|
"evaluate_progress": [0],
|
|
}
|
|
for name, phases in expected.items():
|
|
self.node.counts[name] = 0
|
|
handle, _ = self.execute(name, {"kind": "passed" if name == "verify_state" else "normal",
|
|
"duration_seconds": 0.55})
|
|
with self.subTest(action=name):
|
|
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, 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)
|
|
self.assertEqual(json.loads(task.feedback[0].status_json)["sequence"], 1)
|
|
|
|
def test_cancel_timeout_and_stop_unknown_have_consistent_terminals(self):
|
|
self.node.scenarios["navigate"] = [{"kind": "normal", "duration_seconds": 1, "stop_delay_seconds": 0.05}]
|
|
goal = self.goal("navigate")
|
|
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
|
|
handle = GoalHandle(goal); self.node._accepted("navigate", handle)
|
|
box = {}
|
|
thread = threading.Thread(target=lambda: box.setdefault("result", self.node._execute("navigate", handle)))
|
|
thread.start(); time.sleep(0.02)
|
|
self.assertTrue(handle.request_cancel())
|
|
self.assertTrue(handle.cancel_acknowledged)
|
|
self.assertTrue(thread.is_alive(), "cancel ACK must precede delayed stop termination")
|
|
thread.join(1)
|
|
result = box["result"]
|
|
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.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.stop_state, unknown.STOP_UNKNOWN)
|
|
self.assertTrue(self.node.motion_reserved)
|
|
self.assertEqual(unknown.stop_evidence_ref, "")
|
|
|
|
def test_cancel_and_timeout_can_acknowledge_without_confirming_stop(self):
|
|
handle, canceled = self.execute(
|
|
"execute_manipulation", {"kind": "cancel_stop_unknown", "duration_seconds": 1}, cancel=True)
|
|
self.assertEqual((handle.native, canceled.result.status), ("canceled", canceled.result.CANCELED))
|
|
self.assertEqual(canceled.result.stop_state, canceled.result.UNKNOWN)
|
|
self.assertEqual(handle.feedback[-1].phase, self.module.ExecuteManipulation.Feedback.STOPPING)
|
|
self.assertTrue(self.node.motion_reserved)
|
|
self.node.motion_reserved = False; self.node.unresolved_motion.clear(); 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_stop_unknown"}]
|
|
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.stop_state, timed_out.STOP_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", "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
|
|
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.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.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 = (
|
|
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.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", ""),
|
|
"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", "[]"),
|
|
"verify_state": lambda g: setattr(g.target, "object_ref", ""),
|
|
"locate_shelf_column": lambda g: setattr(g, "observation_station_id", ""),
|
|
"localize_target_3d": lambda g: setattr(g, "station_binding_ref", ""),
|
|
"check_free_space": lambda g: setattr(g, "placement_constraints_json", "[]"),
|
|
"assess_grasp": lambda g: setattr(g.robot_state, "robot_id", ""),
|
|
"evaluate_progress": lambda g: setattr(g, "window_json", "[]"),
|
|
"execute_task": lambda g: setattr(g, "approved_plan_json", "{}"),
|
|
}
|
|
for name, mutate in mutations.items():
|
|
goal = self.goal(name); mutate(goal)
|
|
with self.subTest(action=name):
|
|
self.assertEqual(self.node._goal(name, goal), self.module.GoalResponse.REJECT)
|
|
|
|
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}
|
|
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"],
|
|
"enabled_topics": ["robot_state"],
|
|
})
|
|
node = module.MockSkills()
|
|
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"))
|
|
self.assertFalse(hasattr(node, "progress_pub"))
|
|
|
|
def test_verification_default_is_unknown(self):
|
|
_, result = self.execute("verify_state", {"kind": "unknown", "duration_seconds": 0})
|
|
self.assertEqual(result.evidence.status, result.evidence.UNKNOWN)
|
|
self.assertFalse(result.evidence.stopped_valid)
|
|
|
|
def test_get_state_and_reconcile_require_bound_evidence(self):
|
|
srv = sys.modules["bt_skill_interfaces.srv"]
|
|
response = self.node._get_state(srv.GetRobotState.Request(robot_id="robot_01"), srv.GetRobotState.Response())
|
|
self.assertTrue(response.available)
|
|
missing = self.node._get_state(srv.GetRobotState.Request(robot_id="other"), srv.GetRobotState.Response())
|
|
self.assertFalse(missing.available)
|
|
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.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"
|
|
req.evidence.context.trace = self.trace()
|
|
req.evidence.context.writer = "independent_sim_observer"
|
|
req.evidence.context.observed_at.sec = 10
|
|
req.evidence.context.valid_until.sec = 20
|
|
req.evidence.stopped_valid = req.evidence.stopped = True
|
|
req.evidence.source = "SIMULATOR_INDEPENDENT_FIXTURE"
|
|
req.evidence.evidence_ref = "sim://independent/1"
|
|
rejected = self.node._reconcile(req, srv.ReconcileGoal.Response())
|
|
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)
|
|
replay = self.node._reconcile(req, srv.ReconcileGoal.Response())
|
|
self.assertFalse(replay.accepted)
|
|
|
|
def test_reconcile_rejects_expired_or_unidentified_evidence(self):
|
|
srv = sys.modules["bt_skill_interfaces.srv"]
|
|
handle, _ = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
|
|
goal_id = bytes(handle.goal_id.uuid).hex()
|
|
request = srv.ReconcileGoal.Request(trace=self.trace(), goal_id=goal_id, operator_id="op", reason="review")
|
|
evidence = request.evidence
|
|
evidence.status, evidence.stopped_valid, evidence.stopped = evidence.PASSED, True, True
|
|
evidence.context.source_goal_id, evidence.context.trace = goal_id, self.trace()
|
|
evidence.context.observed_at.sec, evidence.context.valid_until.sec = 1, 2
|
|
evidence.context.writer, evidence.source, evidence.evidence_ref = "observer", "SIMULATOR_INDEPENDENT_FIXTURE", "sim://e/1"
|
|
response = self.node._reconcile(request, srv.ReconcileGoal.Response())
|
|
self.assertFalse(response.accepted)
|
|
|
|
def test_precheck_passed_is_fresh_empty_hand_evidence_for_requested_target(self):
|
|
goal = self.goal("verify_state")
|
|
goal.check, goal.source_goal_id = goal.PRECHECK, "goal-preflight"
|
|
goal.target.object_ref = "bottle"
|
|
self.node.scenarios["verify_state"] = [{"kind": "passed", "duration_seconds": 0}]
|
|
self.assertEqual(self.node._goal("verify_state", goal), self.module.GoalResponse.ACCEPT)
|
|
handle = GoalHandle(goal); self.node._accepted("verify_state", handle)
|
|
evidence = self.node._execute("verify_state", handle).evidence
|
|
self.assertEqual(evidence.holding_state, evidence.EMPTY)
|
|
self.assertTrue(evidence.hand_empty_valid and evidence.hand_empty)
|
|
self.assertEqual(evidence.target_ref, "bottle")
|
|
observed = evidence.context.observed_at.sec * 1_000_000_000 + evidence.context.observed_at.nanosec
|
|
valid_until = evidence.context.valid_until.sec * 1_000_000_000 + evidence.context.valid_until.nanosec
|
|
self.assertGreater(valid_until, observed)
|
|
|
|
def test_business_result_enums_execute_through_handlers(self):
|
|
cases = (
|
|
("locate_shelf_column", "not_found", "status", "NOT_FOUND"),
|
|
("locate_shelf_column", "ambiguous", "status", "AMBIGUOUS"),
|
|
("localize_target_3d", "not_found", "status", "NOT_FOUND"),
|
|
("localize_target_3d", "ambiguous", "status", "AMBIGUOUS"),
|
|
("check_free_space", "no_free_space", "status", "NO_FREE_SPACE"),
|
|
("check_free_space", "ambiguous", "status", "AMBIGUOUS"),
|
|
("assess_grasp", "adjust_posture", "decision", "ADJUST_POSTURE"),
|
|
("assess_grasp", "not_reachable", "decision", "NOT_REACHABLE"),
|
|
("assess_grasp", "unknown", "decision", "UNKNOWN"),
|
|
)
|
|
for name, kind, field, constant in cases:
|
|
self.node.counts[name] = 0
|
|
_, result = self.execute(name, {"kind": kind, "duration_seconds": 0})
|
|
with self.subTest(action=name, kind=kind):
|
|
self.assertEqual(getattr(result, field), getattr(result, constant))
|
|
self.assertTrue(result.error_code)
|
|
self.assertTrue(result.message)
|
|
for kind in ("wrong_object", "wrong_destination"):
|
|
self.node.counts["verify_state"] = 0
|
|
_, result = self.execute("verify_state", {"kind": kind, "duration_seconds": 0})
|
|
self.assertEqual(result.evidence.status, result.evidence.FAILED)
|
|
for kind, expected in (("model_estimate", "MODEL_ESTIMATE"), ("fused", "FUSED")):
|
|
self.node.counts["localize_target_3d"] = 0
|
|
_, result = self.execute("localize_target_3d", {"kind": kind, "duration_seconds": 0})
|
|
self.assertEqual(result.measurement_source, getattr(result, expected))
|
|
self.node.counts["plan_task"] = 0
|
|
ready_plan = {"schema_version": 1, "plan_version": 1, "task_type": "pick_transport_place",
|
|
"goal": "fetch", "slots": {}, "missing_information": [], "subtasks": []}
|
|
_, ready = self.execute("plan_task", {"kind": "normal", "duration_seconds": 0, "plan": ready_plan})
|
|
self.assertEqual(ready.status, ready.PLAN_READY)
|
|
self.node.counts["plan_task"] = 0
|
|
_, clarification = self.execute("plan_task", {"kind": "normal", "duration_seconds": 0})
|
|
self.assertEqual(clarification.status, clarification.NEEDS_CLARIFICATION)
|
|
|
|
def test_reject_and_native_mismatch_fixtures_are_explicit(self):
|
|
goal = self.goal("navigate")
|
|
self.node.scenarios["navigate"] = [{"kind": "reject"}]
|
|
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.REJECT)
|
|
self.assertEqual(self.node.inflight, 0)
|
|
self.node.counts["navigate"] = 0
|
|
handle, result = self.execute("navigate", {"kind": "native_mismatch", "duration_seconds": 0})
|
|
self.assertEqual(handle.native, "aborted")
|
|
self.assertEqual(result.status, result.SUCCEEDED)
|
|
|
|
def test_topic_and_result_fixture_values_are_finite_and_bounded(self):
|
|
for raw in (
|
|
'{"dense_progress":{"progress":-0.1}}',
|
|
'{"dense_progress":{"progress":1.1}}',
|
|
'{"execute_task":{"completed_quantity":-1}}',
|
|
'{"goal_registry":{"status_json":"[]"}}',
|
|
'{"visual_observation":{"image_path":7}}',
|
|
):
|
|
with self.subTest(raw=raw), self.assertRaises(ValueError):
|
|
self.module.parse_scenarios(raw)
|
|
|
|
def test_fixture_kinds_must_have_effect_for_the_selected_interface(self):
|
|
for raw in ('{"navigate":{"kind":"no_free_space"}}',
|
|
'{"goal_registry":{"kind":"wrong_object"}}',
|
|
'{"assess_grasp":{"kind":"stale_observation"}}'):
|
|
with self.subTest(raw=raw), self.assertRaises(ValueError):
|
|
self.module.parse_scenarios(raw)
|
|
|
|
def test_all_mock_topics_publish_configurable_positive_and_negative_fixtures(self):
|
|
self.node.scenarios.update({
|
|
"robot_state": [{"kind": "invalid_pose"}],
|
|
"safety_state": [{"kind": "emergency_stop"}],
|
|
"visual_observation": [{"kind": "normal", "observation_id": "obs-7", "image_path": "/tmp/scene-7.png"}],
|
|
"dense_progress": [{"kind": "normal", "state": "IN_PROGRESS", "progress": 0.4}],
|
|
"goal_registry": [{"kind": "normal", "status_json": '{"goal_id":"g-7","state":"ACTIVE"}'}],
|
|
})
|
|
self.node._publish_states()
|
|
robot, safety = self.node.state_pub.values[-1], self.node.safety_pub.values[-1]
|
|
self.assertFalse(robot.pose_valid)
|
|
self.assertTrue(safety.emergency_stop_active)
|
|
self.assertFalse(safety.motion_allowed)
|
|
self.assertEqual(self.node.observation_pub.values[-1].observation_id, "obs-7")
|
|
self.assertEqual(self.node.observation_pub.values[-1].image_path, "/tmp/scene-7.png")
|
|
self.assertTrue(self.node.progress_pub.values[-1].progress_valid)
|
|
self.assertAlmostEqual(self.node.progress_pub.values[-1].progress, 0.4)
|
|
self.assertEqual(json.loads(self.node.registry_pub.values[-1].data)["goal_id"], "g-7")
|
|
self.assertEqual(self.node.observation_pub.name, "observations/scene")
|
|
self.assertEqual(self.node.progress_pub.name, "monitor/dense_progress")
|
|
self.assertEqual(self.node.registry_pub.name, "goal_registry")
|
|
|
|
def test_canceled_result_cannot_be_overwritten_by_failed_fixture(self):
|
|
handle, result = self.execute("execute_task", {"kind": "failed", "duration_seconds": 1}, cancel=True)
|
|
self.assertEqual(handle.native, "canceled")
|
|
self.assertEqual(result.result.status, result.result.CANCELED)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|