167 lines
9.0 KiB
Python
167 lines
9.0 KiB
Python
"""Pure-Python, bounded fixture parsing; no ROS or hardware dependency."""
|
|
import json
|
|
import math
|
|
|
|
ACTION_NAMES = (
|
|
"navigate", "execute_manipulation", "plan_task", "locate_shelf_column",
|
|
"localize_target_3d", "check_free_space", "assess_grasp", "execute_posture",
|
|
"verify_state",
|
|
"navigate_semantic", "evaluate_progress", "execute_task",
|
|
"robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry",
|
|
)
|
|
KINDS = {
|
|
"normal", "failed", "timeout", "silence", "stop_unknown", "reject",
|
|
"stale_observation", "wrong_object", "wrong_destination", "unknown",
|
|
"passed", "not_found", "ambiguous", "no_free_space", "adjust_posture",
|
|
"not_reachable", "native_mismatch",
|
|
"unavailable", "invalid_pose", "emergency_stop", "protective_stop",
|
|
"model_estimate", "fused",
|
|
"cancel_stop_unknown", "timeout_stop_unknown",
|
|
"obstacle_recovery", "blocked", "not_ready",
|
|
}
|
|
FIXTURE_FIELDS = {
|
|
"kind", "duration_seconds", "shelf_id", "side_id", "column_id", "tier_id",
|
|
"posture_id", "plan", "plan_version", "target_ref", "destination_ref",
|
|
"completed_quantity", "progress", "state", "hop_json", "evidence_json",
|
|
"observation_id", "image_path", "station_id", "registry_version",
|
|
"calibration_id", "geometry_epoch", "status_json",
|
|
"stop_delay_seconds",
|
|
"final_pose", "error_code",
|
|
}
|
|
|
|
BASE_KINDS = {"normal", "failed", "timeout", "silence", "reject", "native_mismatch"}
|
|
ACTION_KINDS = {
|
|
"navigate": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown", "obstacle_recovery", "blocked", "not_ready"},
|
|
"navigate_semantic": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
|
|
"execute_manipulation": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
|
|
"execute_posture": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
|
|
"execute_task": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
|
|
"plan_task": BASE_KINDS,
|
|
"locate_shelf_column": BASE_KINDS | {"not_found", "ambiguous", "stale_observation"},
|
|
"localize_target_3d": BASE_KINDS | {"not_found", "ambiguous", "wrong_object", "stale_observation", "model_estimate", "fused"},
|
|
"check_free_space": BASE_KINDS | {"no_free_space", "ambiguous", "wrong_destination", "stale_observation"},
|
|
"assess_grasp": BASE_KINDS | {"adjust_posture", "not_reachable", "unknown"},
|
|
"verify_state": BASE_KINDS | {"passed", "unknown", "wrong_object", "wrong_destination", "stale_observation"},
|
|
"evaluate_progress": BASE_KINDS | {"unknown", "stale_observation"},
|
|
"robot_state": {"normal", "stale_observation", "invalid_pose", "stop_unknown"},
|
|
"safety_state": {"normal", "unavailable", "emergency_stop", "protective_stop"},
|
|
"visual_observation": {"normal", "stale_observation"},
|
|
"dense_progress": {"normal", "unknown", "stale_observation"},
|
|
"goal_registry": {"normal"},
|
|
}
|
|
|
|
|
|
def strict_json(raw):
|
|
if not isinstance(raw, str) or len(raw.encode("utf-8")) > 262144:
|
|
raise ValueError("JSON fixture must be at most 256 KiB")
|
|
|
|
def pairs(items):
|
|
result = {}
|
|
for key, value in items:
|
|
if key in result:
|
|
raise ValueError("duplicate JSON key")
|
|
result[key] = value
|
|
return result
|
|
|
|
return json.loads(raw, object_pairs_hook=pairs, parse_constant=lambda _: invalid())
|
|
|
|
|
|
def invalid():
|
|
raise ValueError("nonfinite JSON number")
|
|
|
|
|
|
def parse_scenarios(raw):
|
|
data = strict_json(raw)
|
|
if not isinstance(data, dict) or set(data) - set(ACTION_NAMES):
|
|
raise ValueError("scenario keys must name a supported mock action")
|
|
result = {}
|
|
for name, value in data.items():
|
|
entries = value if isinstance(value, list) else [value]
|
|
if not entries or len(entries) > 100:
|
|
raise ValueError("each action requires 1..100 fixtures")
|
|
for fixture in entries:
|
|
if not isinstance(fixture, dict) or set(fixture) - FIXTURE_FIELDS:
|
|
raise ValueError("invalid fixture fields")
|
|
if fixture.get("kind", "normal") not in KINDS:
|
|
raise ValueError("unsupported fixture kind")
|
|
if fixture.get("kind", "normal") not in ACTION_KINDS[name]:
|
|
raise ValueError("fixture kind has no effect for " + name)
|
|
if "error_code" in fixture:
|
|
code = fixture["error_code"]
|
|
if (name != "navigate" or fixture.get("kind") != "not_ready" or
|
|
code not in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED",
|
|
"ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED"}):
|
|
raise ValueError("error_code requires a supported navigation readiness reason")
|
|
delay = fixture.get("duration_seconds", 0.2)
|
|
if type(delay) not in (int, float) or not math.isfinite(delay) or not 0 <= delay <= 120:
|
|
raise ValueError("fixture duration must be finite in [0,120]")
|
|
stop_delay = fixture.get("stop_delay_seconds", 0.0)
|
|
if type(stop_delay) not in (int, float) or not math.isfinite(stop_delay) or not 0 <= stop_delay <= 5:
|
|
raise ValueError("stop delay must be finite in [0,5]")
|
|
if "progress" in fixture:
|
|
progress = fixture["progress"]
|
|
if type(progress) not in (int, float) or not math.isfinite(progress) or not 0 <= progress <= 1:
|
|
raise ValueError("fixture progress must be finite in [0,1]")
|
|
if "completed_quantity" in fixture:
|
|
quantity = fixture["completed_quantity"]
|
|
if type(quantity) is not int or not 0 <= quantity <= 1000000:
|
|
raise ValueError("completed quantity must be an integer in [0,1000000]")
|
|
for field in ("hop_json", "evidence_json", "status_json"):
|
|
if field in fixture and not isinstance(strict_json(fixture[field]), dict):
|
|
raise ValueError(field + " must encode a JSON object")
|
|
for field in ("image_path", "observation_id", "station_id", "calibration_id", "state"):
|
|
if field in fixture and (not isinstance(fixture[field], str) or not fixture[field]):
|
|
raise ValueError(field + " must be a nonempty string")
|
|
if "final_pose" in fixture:
|
|
pose = fixture["final_pose"]
|
|
required = {"frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"}
|
|
if not isinstance(pose, dict) or set(pose) != required or not isinstance(pose["frame_id"], str) or not pose["frame_id"]:
|
|
raise ValueError("final_pose requires an exact frame and pose")
|
|
values = [pose[key] for key in ("x", "y", "z", "qx", "qy", "qz", "qw")]
|
|
if any(type(value) not in (int, float) or not math.isfinite(value) for value in values):
|
|
raise ValueError("final_pose values must be finite")
|
|
if abs(sum(pose[key] ** 2 for key in ("qx", "qy", "qz", "qw")) - 1.0) > 0.001:
|
|
raise ValueError("final_pose quaternion must have unit norm")
|
|
result[name] = entries
|
|
return result
|
|
|
|
|
|
def fixture_at(scenarios, name, index):
|
|
# Exhausted arrays hold their last scenario, making repeated calls deterministic.
|
|
fixtures = scenarios.get(name, [{"kind": "unknown" if name == "verify_state" else "normal"}])
|
|
return dict(fixtures[min(index, len(fixtures) - 1)])
|
|
|
|
|
|
def duration_seconds(value):
|
|
if value.sec < 0 or not 0 <= value.nanosec < 1000000000:
|
|
raise ValueError("invalid ROS Duration")
|
|
seconds = value.sec + value.nanosec / 1e9
|
|
if seconds <= 0:
|
|
raise ValueError("timeout must be positive")
|
|
return seconds
|
|
|
|
|
|
def validate_trace(trace):
|
|
if not all((trace.task_id, trace.subtask_id, trace.run_id)):
|
|
raise ValueError("trace identifiers are required")
|
|
if not all((trace.attempt, trace.task_revision, trace.plan_version, trace.execution_generation)):
|
|
raise ValueError("trace counters begin at one")
|
|
|
|
|
|
def fixed_plan(instruction, slots, version=1):
|
|
required = ("target_name", "source_location", "destination")
|
|
missing = [name for name in required if not slots.get(name)]
|
|
plan = {"schema_version": 1, "plan_version": version, "task_type": "pick_transport_place",
|
|
"goal": instruction, "slots": dict(slots), "missing_information": missing, "subtasks": []}
|
|
if missing:
|
|
return plan
|
|
plan["slots"].setdefault("quantity", 1)
|
|
target, source, dest = (slots[name] for name in required)
|
|
steps = [("NAVIGATE", {"destination": source}), ("GROUND_TARGET", {"target": target}),
|
|
("PICK", {"target": target}), ("NAVIGATE", {"destination": dest}),
|
|
("CHECK_FREE_SPACE", {"destination": dest}), ("PLACE", {"target": target, "destination": dest})]
|
|
for index, (skill, arguments) in enumerate(steps):
|
|
plan["subtasks"].append({"id": "s" + str(index + 1), "skill": skill,
|
|
"arguments": arguments, "depends_on": [] if index == 0 else ["s" + str(index)]})
|
|
return plan
|