|
|
|
@@ -0,0 +1,747 @@
|
|
|
|
|
"""Bounded rclpy action fixtures scoped to /sim/<robot>; never issue robot commands.
|
|
|
|
|
|
|
|
|
|
VLA completion and verification are separate fixtures. Default verification is
|
|
|
|
|
UNKNOWN, so a default successful VLA result cannot count a delivered item.
|
|
|
|
|
"""
|
|
|
|
|
import copy
|
|
|
|
|
import json
|
|
|
|
|
import math
|
|
|
|
|
import threading
|
|
|
|
|
import time
|
|
|
|
|
|
|
|
|
|
import rclpy
|
|
|
|
|
from builtin_interfaces.msg import Duration, Time
|
|
|
|
|
from rclpy.action import ActionServer, CancelResponse, GoalResponse
|
|
|
|
|
from rclpy.callback_groups import ReentrantCallbackGroup
|
|
|
|
|
from rclpy.executors import MultiThreadedExecutor
|
|
|
|
|
from rclpy.node import Node
|
|
|
|
|
|
|
|
|
|
from bt_skill_interfaces.action import (
|
|
|
|
|
AssessGrasp, CheckFreeSpace, ExecuteManipulation, ExecutePosture,
|
|
|
|
|
EvaluateProgress, ExecuteTask, LocalizeTarget3D, LocateShelfColumn,
|
|
|
|
|
Navigate, NavigateSemantic, PlanTask, VerifyState,
|
|
|
|
|
)
|
|
|
|
|
from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, RobotState, SafetyState,
|
|
|
|
|
VerificationEvidence, VisualObservation)
|
|
|
|
|
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
|
|
|
|
|
from std_msgs.msg import String
|
|
|
|
|
from .scenarios import duration_seconds, fixture_at, fixed_plan, parse_scenarios, strict_json, validate_trace
|
|
|
|
|
|
|
|
|
|
ACTION_TYPES = {
|
|
|
|
|
"navigate": Navigate, "execute_manipulation": ExecuteManipulation,
|
|
|
|
|
"plan_task": PlanTask, "locate_shelf_column": LocateShelfColumn,
|
|
|
|
|
"localize_target_3d": LocalizeTarget3D, "check_free_space": CheckFreeSpace,
|
|
|
|
|
"assess_grasp": AssessGrasp, "execute_posture": ExecutePosture, "verify_state": VerifyState,
|
|
|
|
|
"navigate_semantic": NavigateSemantic, "evaluate_progress": EvaluateProgress,
|
|
|
|
|
"execute_task": ExecuteTask,
|
|
|
|
|
}
|
|
|
|
|
ACTION_ENDPOINTS = {
|
|
|
|
|
**{name: "skills/" + name for name in ACTION_TYPES},
|
|
|
|
|
"plan_task": "tasks/plan",
|
|
|
|
|
"execute_task": "tasks/execute",
|
|
|
|
|
"evaluate_progress": "monitor/evaluate_progress",
|
|
|
|
|
}
|
|
|
|
|
MOTION = frozenset(("navigate", "navigate_semantic", "execute_manipulation", "execute_posture", "execute_task"))
|
|
|
|
|
SUCCESS_PHASES = {
|
|
|
|
|
"navigate": (Navigate.Feedback.CHECKING, Navigate.Feedback.PLANNING,
|
|
|
|
|
Navigate.Feedback.NAVIGATING, Navigate.Feedback.ARRIVING),
|
|
|
|
|
"execute_manipulation": (ExecuteManipulation.Feedback.PREPARING,
|
|
|
|
|
ExecuteManipulation.Feedback.WAITING_OBSERVATION,
|
|
|
|
|
ExecuteManipulation.Feedback.INFERRING,
|
|
|
|
|
ExecuteManipulation.Feedback.EXECUTING,
|
|
|
|
|
ExecuteManipulation.Feedback.COMPLETING),
|
|
|
|
|
"plan_task": tuple(range(3)), "locate_shelf_column": (0, 1),
|
|
|
|
|
"localize_target_3d": (0, 1), "check_free_space": (0, 1),
|
|
|
|
|
"assess_grasp": (0,), "execute_posture": (ExecutePosture.Feedback.CHECKING,
|
|
|
|
|
ExecutePosture.Feedback.MOVING,
|
|
|
|
|
ExecutePosture.Feedback.SETTLING),
|
|
|
|
|
"verify_state": (0,), "navigate_semantic": (0,), "evaluate_progress": (0,),
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def ros_time(nanoseconds):
|
|
|
|
|
return Time(sec=max(0, nanoseconds) // 1000000000, nanosec=max(0, nanoseconds) % 1000000000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def elapsed_message(seconds):
|
|
|
|
|
nanoseconds = max(0, int(seconds * 1e9))
|
|
|
|
|
return Duration(sec=nanoseconds // 1000000000, nanosec=nanoseconds % 1000000000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def lifecycle_phases(name, kind):
|
|
|
|
|
if name == "navigate" and kind == "obstacle_recovery":
|
|
|
|
|
return (Navigate.Feedback.CHECKING, Navigate.Feedback.PLANNING,
|
|
|
|
|
Navigate.Feedback.NAVIGATING, Navigate.Feedback.WAITING_OBSTACLE,
|
|
|
|
|
Navigate.Feedback.RECOVERING, Navigate.Feedback.ARRIVING)
|
|
|
|
|
return SUCCESS_PHASES.get(name, (0,))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class MockSkills(Node):
|
|
|
|
|
def __init__(self):
|
|
|
|
|
super().__init__("bt_mock_skills", namespace="/sim/robot_01")
|
|
|
|
|
namespace = self.get_namespace()
|
|
|
|
|
if not namespace.startswith("/sim/") or len(namespace.split("/")) != 3:
|
|
|
|
|
raise RuntimeError("Mock servers require the dedicated /sim/<robot> namespace")
|
|
|
|
|
self.robot_id = namespace.rsplit("/", 1)[1]
|
|
|
|
|
self.declare_parameter("scenarios_json", "{}")
|
|
|
|
|
self.declare_parameter("max_goal_seconds", 30.0)
|
|
|
|
|
self.declare_parameter("allowed_postures", ["pregrasp", "transport", "home"])
|
|
|
|
|
self.declare_parameter("initial_holding_state", "UNKNOWN")
|
|
|
|
|
self.declare_parameter("enabled_actions", list(ACTION_TYPES))
|
|
|
|
|
self.declare_parameter("enabled_topics", ["robot_state", "safety_state", "visual_observation",
|
|
|
|
|
"dense_progress", "goal_registry"])
|
|
|
|
|
self.scenarios = parse_scenarios(self.get_parameter("scenarios_json").value)
|
|
|
|
|
self.max_seconds = self.get_parameter("max_goal_seconds").value
|
|
|
|
|
if not math.isfinite(self.max_seconds) or not 0 < self.max_seconds <= 120:
|
|
|
|
|
raise ValueError("max_goal_seconds must be finite in (0,120]")
|
|
|
|
|
self.allowed_postures = frozenset(self.get_parameter("allowed_postures").value)
|
|
|
|
|
if not self.allowed_postures or any(not isinstance(p, str) or not p for p in self.allowed_postures):
|
|
|
|
|
raise ValueError("allowed_postures must contain nonempty whitelist keys")
|
|
|
|
|
self.enabled_actions = tuple(self.get_parameter("enabled_actions").value)
|
|
|
|
|
self.enabled_topics = frozenset(self.get_parameter("enabled_topics").value)
|
|
|
|
|
if len(set(self.enabled_actions)) != len(self.enabled_actions) or set(self.enabled_actions) - set(ACTION_TYPES):
|
|
|
|
|
raise ValueError("enabled_actions contains an unknown or duplicate action")
|
|
|
|
|
topic_names = {"robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry"}
|
|
|
|
|
if set(self.enabled_topics) - topic_names:
|
|
|
|
|
raise ValueError("enabled_topics contains an unknown topic")
|
|
|
|
|
self.lock = threading.Lock()
|
|
|
|
|
self.motion_reserved = False
|
|
|
|
|
self.inflight = 0
|
|
|
|
|
self.counts = {name: 0 for name in ACTION_TYPES}
|
|
|
|
|
self.topic_counts = {name: 0 for name in
|
|
|
|
|
("robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry")}
|
|
|
|
|
self.accepted = {}
|
|
|
|
|
self.unresolved_motion = {}
|
|
|
|
|
self.motion_owner = None
|
|
|
|
|
self.geometry_epoch = 1
|
|
|
|
|
initial_holding = self.get_parameter("initial_holding_state").value
|
|
|
|
|
if initial_holding not in ("UNKNOWN", "EMPTY"):
|
|
|
|
|
raise ValueError("initial_holding_state must be an explicit UNKNOWN or EMPTY fixture")
|
|
|
|
|
self.holding_state = RobotState.EMPTY if initial_holding == "EMPTY" else RobotState.HOLDING_UNKNOWN
|
|
|
|
|
self.held_target = ""
|
|
|
|
|
self.posture_id = "home"
|
|
|
|
|
self.group = ReentrantCallbackGroup()
|
|
|
|
|
self.servers = []
|
|
|
|
|
for name in self.enabled_actions:
|
|
|
|
|
action = ACTION_TYPES[name]
|
|
|
|
|
endpoint = ACTION_ENDPOINTS[name]
|
|
|
|
|
self._check_endpoint(endpoint)
|
|
|
|
|
self.servers.append(ActionServer(
|
|
|
|
|
self, action, endpoint, callback_group=self.group,
|
|
|
|
|
goal_callback=lambda request, n=name: self._goal(n, request),
|
|
|
|
|
cancel_callback=lambda _: CancelResponse.ACCEPT,
|
|
|
|
|
handle_accepted_callback=lambda handle, n=name: self._accepted(n, handle),
|
|
|
|
|
execute_callback=lambda handle, n=name: self._execute(n, handle),
|
|
|
|
|
))
|
|
|
|
|
endpoints = {"robot_state": "robot_state", "safety_state": "safety_state",
|
|
|
|
|
"visual_observation": "observations/scene", "dense_progress": "monitor/dense_progress",
|
|
|
|
|
"goal_registry": "goal_registry"}
|
|
|
|
|
for name in [endpoints[item] for item in self.enabled_topics] + ["get_robot_state", "reconcile_goal"]:
|
|
|
|
|
self._check_endpoint(name)
|
|
|
|
|
if "robot_state" in self.enabled_topics: self.state_pub = self.create_publisher(RobotState, "robot_state", 10)
|
|
|
|
|
if "safety_state" in self.enabled_topics: self.safety_pub = self.create_publisher(SafetyState, "safety_state", 10)
|
|
|
|
|
if "visual_observation" in self.enabled_topics: self.observation_pub = self.create_publisher(VisualObservation, "observations/scene", 10)
|
|
|
|
|
if "dense_progress" in self.enabled_topics: self.progress_pub = self.create_publisher(DenseProgress, "monitor/dense_progress", 10)
|
|
|
|
|
if "goal_registry" in self.enabled_topics: self.registry_pub = self.create_publisher(String, "goal_registry", 10)
|
|
|
|
|
self.state_service = self.create_service(GetRobotState, "get_robot_state", self._get_state, callback_group=self.group)
|
|
|
|
|
self.reconcile_service = self.create_service(ReconcileGoal, "reconcile_goal", self._reconcile, callback_group=self.group)
|
|
|
|
|
self.timer = self.create_timer(0.2, self._publish_states, callback_group=self.group)
|
|
|
|
|
self.get_logger().warning("SIMULATION ONLY: no hardware commands; verification defaults to UNKNOWN")
|
|
|
|
|
|
|
|
|
|
def _check_endpoint(self, name):
|
|
|
|
|
resolved = self.resolve_topic_name(name)
|
|
|
|
|
if not resolved.startswith(self.get_namespace() + "/"):
|
|
|
|
|
raise RuntimeError("Mock endpoint remappings must remain in this simulation namespace")
|
|
|
|
|
|
|
|
|
|
def _goal(self, name, request):
|
|
|
|
|
try:
|
|
|
|
|
if duration_seconds(request.timeout) > self.max_seconds:
|
|
|
|
|
raise ValueError("requested timeout exceeds mock bound")
|
|
|
|
|
if hasattr(request, "trace"):
|
|
|
|
|
validate_trace(request.trace)
|
|
|
|
|
elif not request.task_id or (hasattr(request, "subtask_id") and not request.subtask_id):
|
|
|
|
|
raise ValueError("task/subtask is required")
|
|
|
|
|
if hasattr(request, "capture_after"):
|
|
|
|
|
capture = request.capture_after
|
|
|
|
|
if capture.sec < 0 or not 0 <= capture.nanosec < 1000000000:
|
|
|
|
|
raise ValueError("capture boundary is invalid")
|
|
|
|
|
capture_ns = capture.sec * 1000000000 + capture.nanosec
|
|
|
|
|
if capture_ns > self.get_clock().now().nanoseconds:
|
|
|
|
|
raise ValueError("capture boundary is in the future")
|
|
|
|
|
if name == "navigate":
|
|
|
|
|
p, q = request.target_pose.pose.position, request.target_pose.pose.orientation
|
|
|
|
|
values = [p.x, p.y, p.z, q.x, q.y, q.z, q.w, request.position_tolerance, request.orientation_tolerance]
|
|
|
|
|
if not all(math.isfinite(v) for v in values) or not request.target_pose.header.frame_id:
|
|
|
|
|
raise ValueError("navigation pose is invalid")
|
|
|
|
|
if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi:
|
|
|
|
|
raise ValueError("navigation tolerances are invalid")
|
|
|
|
|
if abs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1.0) > 0.001:
|
|
|
|
|
raise ValueError("navigation quaternion must have unit norm")
|
|
|
|
|
elif name == "execute_manipulation":
|
|
|
|
|
if request.skill not in ("pick", "place") or not request.instruction.strip():
|
|
|
|
|
raise ValueError("manipulation skill/instruction is invalid")
|
|
|
|
|
if not request.target.object_ref or not request.target.description:
|
|
|
|
|
raise ValueError("manipulation target is required")
|
|
|
|
|
destination = (bool(request.destination.region_ref), bool(request.destination.description))
|
|
|
|
|
if destination != ((False, False) if request.skill == "pick" else (True, True)):
|
|
|
|
|
raise ValueError("destination must be empty for pick and complete for place")
|
|
|
|
|
elif name == "navigate_semantic":
|
|
|
|
|
if request.kind not in ("LOCATION", "OBJECT", "CELL") or not request.reference or not request.registry_version:
|
|
|
|
|
raise ValueError("semantic navigation binding is invalid")
|
|
|
|
|
if request.kind == "CELL" and any(
|
|
|
|
|
not isinstance(value, str) or not value or "/" in value
|
|
|
|
|
for value in (request.shelf_id, request.side_id, request.column_id, request.tier_id)):
|
|
|
|
|
raise ValueError("semantic cell binding is incomplete")
|
|
|
|
|
if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi:
|
|
|
|
|
raise ValueError("semantic navigation tolerances are invalid")
|
|
|
|
|
elif name == "execute_posture":
|
|
|
|
|
if request.posture_id not in self.allowed_postures or not request.expected_geometry_epoch:
|
|
|
|
|
raise ValueError("posture or geometry epoch is invalid")
|
|
|
|
|
elif name == "plan_task":
|
|
|
|
|
if not request.instruction or not request.task_revision or not request.planning_generation:
|
|
|
|
|
raise ValueError("planning identity/instruction is invalid")
|
|
|
|
|
for raw in (request.known_info_json, request.context_snapshot_json, request.constraints_json):
|
|
|
|
|
if not isinstance(strict_json(raw), dict):
|
|
|
|
|
raise ValueError("planning JSON must be an object")
|
|
|
|
|
elif name == "verify_state" and request.check not in range(5):
|
|
|
|
|
raise ValueError("unsupported verification check")
|
|
|
|
|
elif name == "verify_state":
|
|
|
|
|
if not request.expected_geometry_epoch or not request.target.object_ref or not request.target.description:
|
|
|
|
|
raise ValueError("verification target and geometry epoch are required")
|
|
|
|
|
if request.check in (request.PICK, request.TRANSPORT, request.PLACE) and not request.source_goal_id:
|
|
|
|
|
raise ValueError("verification source goal is required")
|
|
|
|
|
if request.check == request.PLACE and (not request.destination.region_ref or not request.destination.description):
|
|
|
|
|
raise ValueError("place verification destination is required")
|
|
|
|
|
elif name == "locate_shelf_column":
|
|
|
|
|
if not all((request.target_ref, request.target_description, request.source_region_ref,
|
|
|
|
|
request.observation_station_id)) or not request.station_registry_version:
|
|
|
|
|
raise ValueError("shelf localization binding is incomplete")
|
|
|
|
|
elif name == "localize_target_3d":
|
|
|
|
|
if not all((request.target_ref, request.target_description, request.shelf_id,
|
|
|
|
|
request.column_id, request.station_binding_ref)) or not request.expected_geometry_epoch:
|
|
|
|
|
raise ValueError("3D localization binding is incomplete")
|
|
|
|
|
elif name == "check_free_space":
|
|
|
|
|
if not all((request.destination_ref, request.destination_description,
|
|
|
|
|
request.object_ref, request.object_description)):
|
|
|
|
|
raise ValueError("free-space target is incomplete")
|
|
|
|
|
if not isinstance(strict_json(request.placement_constraints_json), dict):
|
|
|
|
|
raise ValueError("placement constraints must be an object")
|
|
|
|
|
elif name == "assess_grasp":
|
|
|
|
|
context = request.target_binding.context
|
|
|
|
|
valid_until_ns = (request.robot_state.valid_until.sec * 1000000000 +
|
|
|
|
|
request.robot_state.valid_until.nanosec)
|
|
|
|
|
if (not request.target_binding.target.object_ref or not request.target_binding.target.description or
|
|
|
|
|
not context.schema_version or not context.geometry_epoch or
|
|
|
|
|
not request.robot_state.robot_id or not request.allowed_posture_ids or
|
|
|
|
|
valid_until_ns <= self.get_clock().now().nanoseconds or
|
|
|
|
|
any(p not in self.allowed_postures for p in request.allowed_posture_ids)):
|
|
|
|
|
raise ValueError("grasp assessment inputs are incomplete")
|
|
|
|
|
elif name == "evaluate_progress":
|
|
|
|
|
window = strict_json(request.window_json)
|
|
|
|
|
if not request.task_description.strip() or not request.sequence or not isinstance(window, list) or not window:
|
|
|
|
|
raise ValueError("progress window is invalid")
|
|
|
|
|
elif name == "execute_task":
|
|
|
|
|
plan, context = strict_json(request.approved_plan_json), strict_json(request.context_json)
|
|
|
|
|
if not isinstance(plan, dict) or not isinstance(plan.get("subtasks"), list) or not isinstance(context, dict):
|
|
|
|
|
raise ValueError("task boundary JSON is invalid")
|
|
|
|
|
with self.lock:
|
|
|
|
|
scenario = fixture_at(self.scenarios, name, self.counts[name])
|
|
|
|
|
if scenario.get("kind") == "reject":
|
|
|
|
|
self.counts[name] += 1
|
|
|
|
|
return GoalResponse.REJECT
|
|
|
|
|
# Four active work callbacks leave threads for cancellation and state.
|
|
|
|
|
if self.inflight >= 4 or (name in MOTION and self.motion_reserved):
|
|
|
|
|
return GoalResponse.REJECT
|
|
|
|
|
self.inflight += 1
|
|
|
|
|
if name in MOTION:
|
|
|
|
|
self.motion_reserved = True
|
|
|
|
|
return GoalResponse.ACCEPT
|
|
|
|
|
except (ValueError, TypeError, OverflowError) as exc:
|
|
|
|
|
self.get_logger().warning("Rejecting " + name + ": " + str(exc))
|
|
|
|
|
return GoalResponse.REJECT
|
|
|
|
|
|
|
|
|
|
def _accepted(self, name, handle):
|
|
|
|
|
goal_id = bytes(handle.goal_id.uuid).hex()
|
|
|
|
|
with self.lock:
|
|
|
|
|
fixture = fixture_at(self.scenarios, name, self.counts[name])
|
|
|
|
|
self.counts[name] += 1
|
|
|
|
|
self.accepted[goal_id] = (time.monotonic(), fixture)
|
|
|
|
|
if name in MOTION:
|
|
|
|
|
self.motion_owner = (goal_id, name, copy.deepcopy(handle.request.trace))
|
|
|
|
|
handle.execute()
|
|
|
|
|
|
|
|
|
|
def _execute(self, name, handle):
|
|
|
|
|
goal_id = bytes(handle.goal_id.uuid).hex()
|
|
|
|
|
with self.lock:
|
|
|
|
|
started, fixture = self.accepted.pop(goal_id)
|
|
|
|
|
kind = fixture.get("kind", "normal")
|
|
|
|
|
budget = duration_seconds(handle.request.timeout)
|
|
|
|
|
deadline = started + budget
|
|
|
|
|
finish = started + fixture.get("duration_seconds", 0.2)
|
|
|
|
|
outcome, stop_state = ExecutionResult.COMPLETED, ExecutionResult.CONFIRMED
|
|
|
|
|
sequence = 0
|
|
|
|
|
emitted_phase = -1
|
|
|
|
|
result = ACTION_TYPES[name].Result()
|
|
|
|
|
try:
|
|
|
|
|
while True:
|
|
|
|
|
now = time.monotonic()
|
|
|
|
|
if handle.is_cancel_requested:
|
|
|
|
|
outcome = ExecutionResult.CANCELED
|
|
|
|
|
self._stopping_feedback(name, handle, sequence + 1, now - started)
|
|
|
|
|
if kind == "cancel_stop_unknown":
|
|
|
|
|
stop_state = ExecutionResult.UNKNOWN
|
|
|
|
|
else:
|
|
|
|
|
time.sleep(fixture.get("stop_delay_seconds", 0.0))
|
|
|
|
|
break
|
|
|
|
|
if now >= deadline or not rclpy.ok():
|
|
|
|
|
outcome = ExecutionResult.TIMED_OUT
|
|
|
|
|
self._stopping_feedback(name, handle, sequence + 1, now - started)
|
|
|
|
|
if kind == "timeout_stop_unknown":
|
|
|
|
|
stop_state = ExecutionResult.UNKNOWN
|
|
|
|
|
break
|
|
|
|
|
if now >= finish and kind not in ("timeout", "timeout_stop_unknown", "silence"):
|
|
|
|
|
break
|
|
|
|
|
if kind != "silence":
|
|
|
|
|
phases = lifecycle_phases(name, kind)
|
|
|
|
|
duration = max(0.001, finish - started)
|
|
|
|
|
phase_index = min(len(phases) - 1, int((now - started) / duration * len(phases)))
|
|
|
|
|
if phase_index > emitted_phase:
|
|
|
|
|
sequence += 1
|
|
|
|
|
self._feedback(name, handle, sequence, now - started, phases[phase_index])
|
|
|
|
|
emitted_phase = phase_index
|
|
|
|
|
time.sleep(min(0.02, max(0, deadline - now)))
|
|
|
|
|
if kind == "failed" and outcome == ExecutionResult.COMPLETED:
|
|
|
|
|
outcome = ExecutionResult.FAILED
|
|
|
|
|
if kind == "stop_unknown" and outcome == ExecutionResult.COMPLETED:
|
|
|
|
|
outcome, stop_state = ExecutionResult.FAILED, ExecutionResult.UNKNOWN
|
|
|
|
|
self._fill_result(name, handle.request, result, fixture, goal_id, outcome, stop_state)
|
|
|
|
|
self._finish_native(handle, outcome, kind)
|
|
|
|
|
except Exception as exc:
|
|
|
|
|
# Unknown execution state stays reserved. A client cannot infer stop from this exception.
|
|
|
|
|
stop_state = ExecutionResult.UNKNOWN if name in MOTION else ExecutionResult.CONFIRMED
|
|
|
|
|
if hasattr(result, "result"):
|
|
|
|
|
result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc))
|
|
|
|
|
elif hasattr(result, "evidence"):
|
|
|
|
|
result.evidence.status = VerificationEvidence.UNKNOWN
|
|
|
|
|
result.evidence.error_code = "MOCK_EXCEPTION"
|
|
|
|
|
elif hasattr(result, "status"):
|
|
|
|
|
result.status = result.FAILED
|
|
|
|
|
result.error_code = "MOCK_EXCEPTION"
|
|
|
|
|
result.message = str(exc)
|
|
|
|
|
elif hasattr(result, "decision"):
|
|
|
|
|
result.decision = result.UNKNOWN
|
|
|
|
|
result.error_code = "MOCK_EXCEPTION"
|
|
|
|
|
if handle.is_active:
|
|
|
|
|
handle.abort()
|
|
|
|
|
self.get_logger().error("Mock execution exception: " + str(exc))
|
|
|
|
|
finally:
|
|
|
|
|
with self.lock:
|
|
|
|
|
self.inflight -= 1
|
|
|
|
|
if name in MOTION and stop_state == ExecutionResult.CONFIRMED:
|
|
|
|
|
self.motion_reserved = False
|
|
|
|
|
self.unresolved_motion.pop(goal_id, None)
|
|
|
|
|
if self.motion_owner and self.motion_owner[0] == goal_id:
|
|
|
|
|
self.motion_owner = None
|
|
|
|
|
elif name in MOTION:
|
|
|
|
|
self.unresolved_motion[goal_id] = (name, copy.deepcopy(handle.request.trace))
|
|
|
|
|
return result
|
|
|
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
|
def _finish_native(handle, outcome, kind):
|
|
|
|
|
if kind == "native_mismatch":
|
|
|
|
|
handle.abort() # Intentional protocol-negative fixture: payload remains COMPLETED.
|
|
|
|
|
elif outcome == ExecutionResult.CANCELED:
|
|
|
|
|
handle.canceled()
|
|
|
|
|
elif outcome == ExecutionResult.COMPLETED:
|
|
|
|
|
handle.succeed()
|
|
|
|
|
else:
|
|
|
|
|
handle.abort()
|
|
|
|
|
|
|
|
|
|
def _feedback(self, name, handle, sequence, elapsed, phase=None):
|
|
|
|
|
feedback = ACTION_TYPES[name].Feedback()
|
|
|
|
|
feedback.stamp = self.get_clock().now().to_msg()
|
|
|
|
|
feedback.sequence = sequence
|
|
|
|
|
if hasattr(feedback, "phase"):
|
|
|
|
|
feedback.phase = SUCCESS_PHASES[name][0] if phase is None else phase
|
|
|
|
|
if hasattr(feedback, "message"):
|
|
|
|
|
feedback.message = "SIMULATED " + name
|
|
|
|
|
if name == "execute_task":
|
|
|
|
|
feedback.stage = "SIMULATED_STAGE_" + str(sequence)
|
|
|
|
|
feedback.status_json = json.dumps({"sequence": sequence}, allow_nan=False)
|
|
|
|
|
if hasattr(feedback, "elapsed_time"):
|
|
|
|
|
feedback.elapsed_time = elapsed_message(elapsed)
|
|
|
|
|
if name == "navigate":
|
|
|
|
|
feedback.pose_valid = True
|
|
|
|
|
feedback.current_pose = copy.deepcopy(handle.request.target_pose)
|
|
|
|
|
feedback.errors_valid = True
|
|
|
|
|
feedback.position_error = 0.0
|
|
|
|
|
feedback.orientation_error = 0.0
|
|
|
|
|
feedback.blocked_valid = True
|
|
|
|
|
feedback.blocked = False
|
|
|
|
|
if name == "execute_manipulation":
|
|
|
|
|
feedback.progress_valid = False
|
|
|
|
|
handle.publish_feedback(feedback)
|
|
|
|
|
|
|
|
|
|
def _stopping_feedback(self, name, handle, sequence, elapsed):
|
|
|
|
|
stopping = {"navigate": Navigate.Feedback.STOPPING,
|
|
|
|
|
"execute_manipulation": ExecuteManipulation.Feedback.STOPPING,
|
|
|
|
|
"execute_posture": ExecutePosture.Feedback.STOPPING}.get(name)
|
|
|
|
|
if stopping is not None:
|
|
|
|
|
self._feedback(name, handle, sequence, elapsed, stopping)
|
|
|
|
|
|
|
|
|
|
def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only"):
|
|
|
|
|
result = ExecutionResult()
|
|
|
|
|
result.status, result.stop_state = outcome, stop_state
|
|
|
|
|
result.error_code = error or ("" if outcome == ExecutionResult.COMPLETED else "MOCK_TERMINATED")
|
|
|
|
|
result.message = message
|
|
|
|
|
if stop_state == ExecutionResult.CONFIRMED:
|
|
|
|
|
result.stopped_at = self.get_clock().now().to_msg()
|
|
|
|
|
result.stop_evidence_ref = "sim://stop/" + goal_id
|
|
|
|
|
return result
|
|
|
|
|
|
|
|
|
|
def _fill_result(self, name, request, result, fixture, goal_id, outcome, stop_state):
|
|
|
|
|
kind = fixture.get("kind", "normal")
|
|
|
|
|
stamp = self.get_clock().now().nanoseconds
|
|
|
|
|
if kind == "stale_observation":
|
|
|
|
|
stamp = max(0, stamp - 60000000000)
|
|
|
|
|
observed = ros_time(stamp)
|
|
|
|
|
record = "sim://" + name + "/" + goal_id
|
|
|
|
|
ok = outcome == ExecutionResult.COMPLETED
|
|
|
|
|
if hasattr(result, "result"):
|
|
|
|
|
result.result = self._execution_result(outcome, stop_state, goal_id)
|
|
|
|
|
if name in ("navigate", "navigate_semantic"):
|
|
|
|
|
if ok and stop_state == ExecutionResult.CONFIRMED:
|
|
|
|
|
with self.lock:
|
|
|
|
|
self.geometry_epoch += 1
|
|
|
|
|
if name == "navigate":
|
|
|
|
|
result.pose_valid = result.errors_valid = ok
|
|
|
|
|
result.final_pose = copy.deepcopy(request.target_pose)
|
|
|
|
|
else:
|
|
|
|
|
pose = fixture.get("final_pose")
|
|
|
|
|
result.pose_valid = result.errors_valid = ok and pose is not None
|
|
|
|
|
if pose is not None:
|
|
|
|
|
result.final_pose.header.frame_id = pose["frame_id"]
|
|
|
|
|
result.final_pose.pose.position.x = pose["x"]
|
|
|
|
|
result.final_pose.pose.position.y = pose["y"]
|
|
|
|
|
result.final_pose.pose.position.z = pose["z"]
|
|
|
|
|
result.final_pose.pose.orientation.x = pose["qx"]
|
|
|
|
|
result.final_pose.pose.orientation.y = pose["qy"]
|
|
|
|
|
result.final_pose.pose.orientation.z = pose["qz"]
|
|
|
|
|
result.final_pose.pose.orientation.w = pose["qw"]
|
|
|
|
|
result.final_pose.header.stamp = observed
|
|
|
|
|
elif name == "execute_manipulation":
|
|
|
|
|
result.execution_record_ref = record
|
|
|
|
|
# Deliberately no holding/verification state mutation here.
|
|
|
|
|
elif name == "plan_task":
|
|
|
|
|
plan = fixture.get("plan") or fixed_plan(request.instruction, strict_json(request.known_info_json), fixture.get("plan_version", 1))
|
|
|
|
|
result.status = result.FAILED if not ok else (result.NEEDS_CLARIFICATION if plan.get("missing_information") else result.PLAN_READY)
|
|
|
|
|
result.task_plan_json = json.dumps(plan, ensure_ascii=False, allow_nan=False)
|
|
|
|
|
result.planning_record_ref = record
|
|
|
|
|
if result.status == result.NEEDS_CLARIFICATION:
|
|
|
|
|
result.error_code, result.message = "MOCK_MISSING_INFORMATION", "SIMULATED plan needs clarification"
|
|
|
|
|
elif name == "locate_shelf_column":
|
|
|
|
|
result.status = result.SUCCEEDED if ok else result.FAILED
|
|
|
|
|
if kind == "not_found": result.status = result.NOT_FOUND
|
|
|
|
|
if kind == "ambiguous": result.status = result.AMBIGUOUS
|
|
|
|
|
result.shelf_id = fixture.get("shelf_id", "shelf_A")
|
|
|
|
|
result.side_id = fixture.get("side_id", "FRONT")
|
|
|
|
|
result.column_id = fixture.get("column_id", "1")
|
|
|
|
|
result.tier_id = fixture.get("tier_id", "1")
|
|
|
|
|
result.confidence = 0.99 if result.status == result.SUCCEEDED else 0.0
|
|
|
|
|
result.observation_id, result.observed_at, result.record_ref = record, observed, record
|
|
|
|
|
if result.status != result.SUCCEEDED:
|
|
|
|
|
label = ("NOT_FOUND" if result.status == result.NOT_FOUND else
|
|
|
|
|
"AMBIGUOUS" if result.status == result.AMBIGUOUS else "FAILED")
|
|
|
|
|
result.error_code = "MOCK_" + label
|
|
|
|
|
result.message = "SIMULATED shelf localization " + label.lower()
|
|
|
|
|
elif name == "localize_target_3d":
|
|
|
|
|
result.status = result.SUCCEEDED if ok else result.FAILED
|
|
|
|
|
if kind == "not_found": result.status = result.NOT_FOUND
|
|
|
|
|
if kind == "ambiguous": result.status = result.AMBIGUOUS
|
|
|
|
|
result.target_ref = "wrong_object" if kind == "wrong_object" else request.target_ref
|
|
|
|
|
result.grasp_region_ref = "sim_grasp:" + result.target_ref
|
|
|
|
|
result.target_point.header.frame_id = "base_link"
|
|
|
|
|
result.target_point.header.stamp = observed
|
|
|
|
|
result.target_point.point.x, result.target_point.point.z = 0.5, 0.8
|
|
|
|
|
result.grasp_point = copy.deepcopy(result.target_point)
|
|
|
|
|
result.grasp_point_valid = True
|
|
|
|
|
result.measurement_source = (result.MODEL_ESTIMATE if kind == "model_estimate" else
|
|
|
|
|
result.FUSED if kind == "fused" else result.RGBD)
|
|
|
|
|
result.geometry_valid = result.status == result.SUCCEEDED
|
|
|
|
|
result.quality_code = "OK" if result.geometry_valid else "MOCK_INVALID"
|
|
|
|
|
result.position_error_bound, result.position_error_bound_valid = 0.005, True
|
|
|
|
|
result.observation_id = record
|
|
|
|
|
result.rgb_stamp = result.depth_stamp = observed
|
|
|
|
|
result.calibration_id = "sim_calibration_v1"
|
|
|
|
|
result.geometry_epoch = request.expected_geometry_epoch
|
|
|
|
|
result.record_ref = record
|
|
|
|
|
if result.status != result.SUCCEEDED:
|
|
|
|
|
result.error_code = "MOCK_" + ("NOT_FOUND" if result.status == result.NOT_FOUND else
|
|
|
|
|
"AMBIGUOUS" if result.status == result.AMBIGUOUS else "FAILED")
|
|
|
|
|
result.message = "SIMULATED 3D localization did not produce a usable target"
|
|
|
|
|
elif name == "check_free_space":
|
|
|
|
|
result.status = result.SUCCEEDED if ok else result.FAILED
|
|
|
|
|
if kind == "no_free_space": result.status = result.NO_FREE_SPACE
|
|
|
|
|
if kind == "ambiguous": result.status = result.AMBIGUOUS
|
|
|
|
|
result.destination_ref = "wrong_container" if kind == "wrong_destination" else request.destination_ref
|
|
|
|
|
result.placement_region_ref = "sim_place:" + result.destination_ref
|
|
|
|
|
result.placement_point.header.frame_id = "base_link"
|
|
|
|
|
result.placement_point.header.stamp = observed
|
|
|
|
|
result.placement_point.point.x, result.placement_point.point.z = 0.5, 0.6
|
|
|
|
|
result.placement_point_valid = True
|
|
|
|
|
result.placement_pose_valid = False
|
|
|
|
|
result.geometry_valid = result.status == result.SUCCEEDED
|
|
|
|
|
result.confidence = 0.99 if result.geometry_valid else 0.0
|
|
|
|
|
result.quality_code = "OK" if result.geometry_valid else "MOCK_INVALID"
|
|
|
|
|
result.observation_id, result.observed_at, result.valid_until = record, observed, ros_time(stamp + 5000000000)
|
|
|
|
|
result.record_ref = record
|
|
|
|
|
if result.status != result.SUCCEEDED:
|
|
|
|
|
result.error_code = "MOCK_" + ("NO_FREE_SPACE" if result.status == result.NO_FREE_SPACE else
|
|
|
|
|
"AMBIGUOUS" if result.status == result.AMBIGUOUS else "FAILED")
|
|
|
|
|
result.message = "SIMULATED placement search did not produce usable free space"
|
|
|
|
|
elif name == "assess_grasp":
|
|
|
|
|
result.decision = result.DIRECT if ok else result.UNKNOWN
|
|
|
|
|
if kind == "unknown": result.decision = result.UNKNOWN
|
|
|
|
|
if kind == "not_reachable": result.decision = result.NOT_REACHABLE
|
|
|
|
|
if kind == "adjust_posture":
|
|
|
|
|
result.decision = result.ADJUST_POSTURE
|
|
|
|
|
result.posture_id = fixture.get("posture_id", "pregrasp")
|
|
|
|
|
if result.posture_id not in request.allowed_posture_ids:
|
|
|
|
|
result.decision, result.posture_id = result.UNKNOWN, ""
|
|
|
|
|
result.geometry_epoch = request.target_binding.context.geometry_epoch
|
|
|
|
|
result.evidence_ref = record
|
|
|
|
|
if result.decision != result.DIRECT:
|
|
|
|
|
labels = {result.ADJUST_POSTURE: "ADJUST_POSTURE", result.NOT_REACHABLE: "NOT_REACHABLE",
|
|
|
|
|
result.UNKNOWN: "UNKNOWN"}
|
|
|
|
|
label = labels[result.decision]
|
|
|
|
|
result.error_code = "MOCK_" + label
|
|
|
|
|
result.message = "SIMULATED grasp assessment " + label.lower()
|
|
|
|
|
elif name == "execute_posture":
|
|
|
|
|
if ok and stop_state == ExecutionResult.CONFIRMED:
|
|
|
|
|
with self.lock:
|
|
|
|
|
self.geometry_epoch = max(self.geometry_epoch, request.expected_geometry_epoch) + 1
|
|
|
|
|
self.posture_id = request.posture_id
|
|
|
|
|
result.geometry_epoch = self.geometry_epoch
|
|
|
|
|
result.robot_state = self._robot_state()
|
|
|
|
|
if ok and stop_state == ExecutionResult.CONFIRMED:
|
|
|
|
|
# Completion snapshot is stopped; registry ownership is released only after native terminal.
|
|
|
|
|
result.robot_state.base_stopped_valid = result.robot_state.base_stopped = True
|
|
|
|
|
result.robot_state.posture_settled_valid = result.robot_state.posture_settled = True
|
|
|
|
|
result.robot_state.evidence_ref = "sim://posture_settled/" + goal_id
|
|
|
|
|
elif name == "verify_state":
|
|
|
|
|
self._verify(request, result.evidence, fixture, observed, stamp, record, ok)
|
|
|
|
|
elif name == "evaluate_progress":
|
|
|
|
|
result.feedback_state.trace = copy.deepcopy(request.trace)
|
|
|
|
|
result.feedback_state.observed_at = observed
|
|
|
|
|
result.feedback_state.sequence = request.sequence
|
|
|
|
|
result.feedback_state.state = fixture.get("state", "IN_PROGRESS") if ok else "UNKNOWN"
|
|
|
|
|
result.feedback_state.progress = float(fixture.get("progress", 0.5)) if ok else 0.0
|
|
|
|
|
result.feedback_state.progress_valid = ok
|
|
|
|
|
result.feedback_state.hop_json = fixture.get("hop_json", "{}")
|
|
|
|
|
result.feedback_state.record_ref = record
|
|
|
|
|
result.error_code = "" if ok else "MOCK_TERMINATED"
|
|
|
|
|
elif name == "execute_task":
|
|
|
|
|
result.completed_quantity = int(fixture.get("completed_quantity", 1 if ok else 0))
|
|
|
|
|
result.evidence_json = fixture.get("evidence_json", "{}")
|
|
|
|
|
if hasattr(result, "error_code") and not ok:
|
|
|
|
|
result.error_code = "MOCK_TERMINATED"
|
|
|
|
|
result.message = "SIMULATED non-completion"
|
|
|
|
|
|
|
|
|
|
def _verify(self, request, evidence, fixture, observed, stamp, record, ok):
|
|
|
|
|
kind = fixture.get("kind", "unknown")
|
|
|
|
|
evidence.context.schema_version = 1
|
|
|
|
|
evidence.context.trace = copy.deepcopy(request.trace)
|
|
|
|
|
evidence.context.source_goal_id = request.source_goal_id
|
|
|
|
|
evidence.context.geometry_epoch = request.expected_geometry_epoch
|
|
|
|
|
evidence.context.observed_at = observed
|
|
|
|
|
evidence.context.valid_until = ros_time(stamp + 5000000000)
|
|
|
|
|
evidence.context.writer = "bt_mock_verifier"
|
|
|
|
|
evidence.context.observation_id = record
|
|
|
|
|
evidence.source, evidence.evidence_ref = "SIMULATOR_INDEPENDENT_FIXTURE", record
|
|
|
|
|
evidence.status, evidence.holding_state = evidence.UNKNOWN, evidence.HOLDING_UNKNOWN
|
|
|
|
|
evidence.target_ref, evidence.destination_ref = request.target.object_ref, request.destination.region_ref
|
|
|
|
|
if not ok or kind not in ("passed", "wrong_object", "wrong_destination", "stale_observation"):
|
|
|
|
|
evidence.error_code = "MOCK_VERIFICATION_UNKNOWN"
|
|
|
|
|
evidence.message = "SIMULATED verification is unknown"
|
|
|
|
|
return
|
|
|
|
|
evidence.status = evidence.PASSED
|
|
|
|
|
evidence.stopped_valid = evidence.stopped = True
|
|
|
|
|
evidence.target_match_valid = True
|
|
|
|
|
evidence.target_match = kind != "wrong_object"
|
|
|
|
|
if kind == "wrong_object": evidence.target_ref = "wrong_object"
|
|
|
|
|
if request.check in (request.PRECHECK, request.PLACE):
|
|
|
|
|
evidence.holding_state = evidence.EMPTY
|
|
|
|
|
evidence.hand_empty_valid = evidence.hand_empty = True
|
|
|
|
|
elif request.check in (request.PICK, request.TRANSPORT):
|
|
|
|
|
evidence.holding_state = evidence.HOLDING_OTHER if kind == "wrong_object" else evidence.HOLDING_TARGET
|
|
|
|
|
evidence.grasp_stable_valid = evidence.grasp_stable = True
|
|
|
|
|
if request.check == request.PLACE:
|
|
|
|
|
evidence.target_in_destination_valid = True
|
|
|
|
|
evidence.target_in_destination = kind != "wrong_destination"
|
|
|
|
|
if kind == "wrong_destination": evidence.destination_ref = "wrong_container"
|
|
|
|
|
if kind in ("wrong_object", "wrong_destination"):
|
|
|
|
|
evidence.status = evidence.FAILED
|
|
|
|
|
evidence.error_code = "MOCK_VERIFICATION_FAILED"
|
|
|
|
|
evidence.message = "SIMULATED verification contradicted the requested state"
|
|
|
|
|
with self.lock:
|
|
|
|
|
self.holding_state = evidence.holding_state
|
|
|
|
|
self.held_target = evidence.target_ref if evidence.holding_state == evidence.HOLDING_TARGET else ""
|
|
|
|
|
|
|
|
|
|
def _robot_state(self):
|
|
|
|
|
state = RobotState()
|
|
|
|
|
now = self.get_clock().now().nanoseconds
|
|
|
|
|
state.robot_id = self.robot_id
|
|
|
|
|
state.stamp, state.valid_until = ros_time(now), ros_time(now + 1000000000)
|
|
|
|
|
with self.lock:
|
|
|
|
|
state.geometry_epoch, state.holding_state = self.geometry_epoch, self.holding_state
|
|
|
|
|
state.held_target_ref, state.posture_id = self.held_target, self.posture_id
|
|
|
|
|
state.base_stopped_valid = state.posture_settled_valid = not self.motion_reserved
|
|
|
|
|
state.base_stopped = state.posture_settled = not self.motion_reserved
|
|
|
|
|
state.pose.header.frame_id = "map"
|
|
|
|
|
state.pose.header.stamp = state.stamp
|
|
|
|
|
state.pose.pose.orientation.w = 1.0
|
|
|
|
|
state.pose_valid = True
|
|
|
|
|
state.evidence_ref = "sim://state/" + self.robot_id
|
|
|
|
|
return state
|
|
|
|
|
|
|
|
|
|
def _safety_state(self):
|
|
|
|
|
state = SafetyState()
|
|
|
|
|
now = self.get_clock().now().nanoseconds
|
|
|
|
|
state.robot_id = self.robot_id
|
|
|
|
|
state.stamp, state.valid_until = ros_time(now), ros_time(now + 1000000000)
|
|
|
|
|
state.safety_valid = state.motion_allowed = True
|
|
|
|
|
state.evidence_ref = "sim://safety/" + self.robot_id
|
|
|
|
|
return state
|
|
|
|
|
|
|
|
|
|
def _publish_states(self):
|
|
|
|
|
state_fixture = self._next_topic_fixture("robot_state")
|
|
|
|
|
safety_fixture = self._next_topic_fixture("safety_state")
|
|
|
|
|
state, safety = self._robot_state(), self._safety_state()
|
|
|
|
|
if state_fixture.get("kind") == "stale_observation":
|
|
|
|
|
state.valid_until = ros_time(1)
|
|
|
|
|
elif state_fixture.get("kind") == "invalid_pose":
|
|
|
|
|
state.pose_valid = False
|
|
|
|
|
elif state_fixture.get("kind") == "stop_unknown":
|
|
|
|
|
state.base_stopped_valid = state.posture_settled_valid = False
|
|
|
|
|
if safety_fixture.get("kind") == "unavailable":
|
|
|
|
|
safety.safety_valid = safety.motion_allowed = False
|
|
|
|
|
safety.error_code = "MOCK_SAFETY_UNAVAILABLE"
|
|
|
|
|
elif safety_fixture.get("kind") == "emergency_stop":
|
|
|
|
|
safety.emergency_stop_active, safety.motion_allowed = True, False
|
|
|
|
|
elif safety_fixture.get("kind") == "protective_stop":
|
|
|
|
|
safety.protective_stop_active, safety.motion_allowed = True, False
|
|
|
|
|
if "robot_state" in self.enabled_topics: self.state_pub.publish(state)
|
|
|
|
|
if "safety_state" in self.enabled_topics: self.safety_pub.publish(safety)
|
|
|
|
|
if "visual_observation" in self.enabled_topics:
|
|
|
|
|
self.observation_pub.publish(self._visual_observation(self._next_topic_fixture("visual_observation")))
|
|
|
|
|
if "dense_progress" in self.enabled_topics:
|
|
|
|
|
self.progress_pub.publish(self._dense_progress(self._next_topic_fixture("dense_progress")))
|
|
|
|
|
if "goal_registry" in self.enabled_topics:
|
|
|
|
|
registry = self._next_topic_fixture("goal_registry")
|
|
|
|
|
default_registry = json.dumps(
|
|
|
|
|
{"robot_id": self.robot_id, "motion_reserved": self.motion_reserved}, allow_nan=False)
|
|
|
|
|
self.registry_pub.publish(String(data=registry.get("status_json", default_registry)))
|
|
|
|
|
|
|
|
|
|
def _next_topic_fixture(self, name):
|
|
|
|
|
with self.lock:
|
|
|
|
|
fixture = fixture_at(self.scenarios, name, self.topic_counts[name])
|
|
|
|
|
self.topic_counts[name] += 1
|
|
|
|
|
return fixture
|
|
|
|
|
|
|
|
|
|
def _visual_observation(self, fixture):
|
|
|
|
|
message = VisualObservation()
|
|
|
|
|
now = self.get_clock().now().nanoseconds
|
|
|
|
|
message.header.stamp = ros_time(now)
|
|
|
|
|
message.header.frame_id = "camera_link"
|
|
|
|
|
message.observation_id = fixture.get("observation_id", "sim_observation")
|
|
|
|
|
message.image_path = fixture.get("image_path", "/tmp/bt_mock_scene.png")
|
|
|
|
|
message.station_id = fixture.get("station_id", "station_A")
|
|
|
|
|
message.registry_version = int(fixture.get("registry_version", 1))
|
|
|
|
|
message.shelf_id = fixture.get("shelf_id", "shelf_A")
|
|
|
|
|
message.calibration_id = fixture.get("calibration_id", "sim_calibration_v1")
|
|
|
|
|
message.geometry_epoch = int(fixture.get("geometry_epoch", self.geometry_epoch))
|
|
|
|
|
if fixture.get("kind") == "stale_observation":
|
|
|
|
|
message.header.stamp = ros_time(max(0, now - 60000000000))
|
|
|
|
|
return message
|
|
|
|
|
|
|
|
|
|
def _dense_progress(self, fixture):
|
|
|
|
|
message = DenseProgress()
|
|
|
|
|
message.observed_at = self.get_clock().now().to_msg()
|
|
|
|
|
message.sequence = self.topic_counts["dense_progress"]
|
|
|
|
|
message.state = fixture.get("state", "UNKNOWN")
|
|
|
|
|
message.progress = float(fixture.get("progress", 0.0))
|
|
|
|
|
message.progress_valid = "progress" in fixture and fixture.get("kind") != "unknown"
|
|
|
|
|
message.hop_json = fixture.get("hop_json", "{}")
|
|
|
|
|
message.record_ref = "sim://dense_progress/" + str(message.sequence)
|
|
|
|
|
return message
|
|
|
|
|
|
|
|
|
|
def _get_state(self, request, response):
|
|
|
|
|
response.available = request.robot_id == self.robot_id
|
|
|
|
|
if response.available:
|
|
|
|
|
response.robot_state, response.safety_state = self._robot_state(), self._safety_state()
|
|
|
|
|
else:
|
|
|
|
|
response.error_code = "UNKNOWN_ROBOT"
|
|
|
|
|
return response
|
|
|
|
|
|
|
|
|
|
def _reconcile(self, request, response):
|
|
|
|
|
"""Simulation-only release requires fresh evidence bound to this exact goal and trace."""
|
|
|
|
|
try:
|
|
|
|
|
validate_trace(request.trace)
|
|
|
|
|
evidence = request.evidence
|
|
|
|
|
same_trace = all(getattr(evidence.context.trace, field) == getattr(request.trace, field)
|
|
|
|
|
for field in ("task_id", "subtask_id", "attempt", "task_revision",
|
|
|
|
|
"plan_version", "run_id", "execution_generation"))
|
|
|
|
|
now = self.get_clock().now().nanoseconds
|
|
|
|
|
observed_ns = evidence.context.observed_at.sec * 1000000000 + evidence.context.observed_at.nanosec
|
|
|
|
|
valid_until_ns = evidence.context.valid_until.sec * 1000000000 + evidence.context.valid_until.nanosec
|
|
|
|
|
fresh = 0 < observed_ns <= now < valid_until_ns
|
|
|
|
|
with self.lock:
|
|
|
|
|
owned = self.unresolved_motion.get(request.goal_id)
|
|
|
|
|
owned_trace_matches = owned is not None and all(
|
|
|
|
|
getattr(owned[1], field) == getattr(request.trace, field)
|
|
|
|
|
for field in ("task_id", "subtask_id", "attempt", "task_revision",
|
|
|
|
|
"plan_version", "run_id", "execution_generation"))
|
|
|
|
|
owner_matches = self.motion_owner is not None and self.motion_owner[0] == request.goal_id
|
|
|
|
|
bound = (owner_matches and owned_trace_matches and evidence.context.source_goal_id == request.goal_id and
|
|
|
|
|
same_trace and evidence.status == evidence.PASSED and
|
|
|
|
|
evidence.stopped_valid and evidence.stopped and evidence.evidence_ref and
|
|
|
|
|
evidence.context.writer and evidence.source == "SIMULATOR_INDEPENDENT_FIXTURE" and fresh and
|
|
|
|
|
request.operator_id and request.reason)
|
|
|
|
|
if not bound:
|
|
|
|
|
raise ValueError("reconciliation evidence is not bound and stopped")
|
|
|
|
|
with self.lock:
|
|
|
|
|
if request.goal_id not in self.unresolved_motion:
|
|
|
|
|
raise ValueError("goal reservation is no longer unresolved")
|
|
|
|
|
self.unresolved_motion.pop(request.goal_id)
|
|
|
|
|
self.motion_reserved = False
|
|
|
|
|
self.motion_owner = None
|
|
|
|
|
response.accepted = True
|
|
|
|
|
response.message = "SIMULATED evidence-bound reconciliation"
|
|
|
|
|
except (ValueError, TypeError):
|
|
|
|
|
response.accepted = False
|
|
|
|
|
response.error_code = "UNBOUND_EVIDENCE"
|
|
|
|
|
response.message = "Reconciliation requires exact goal/trace and stopped evidence"
|
|
|
|
|
return response
|
|
|
|
|
|
|
|
|
|
def destroy_node(self):
|
|
|
|
|
for server in self.servers:
|
|
|
|
|
server.destroy()
|
|
|
|
|
super().destroy_node()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main(args=None):
|
|
|
|
|
rclpy.init(args=args)
|
|
|
|
|
node = None
|
|
|
|
|
executor = MultiThreadedExecutor(num_threads=8)
|
|
|
|
|
try:
|
|
|
|
|
node = MockSkills()
|
|
|
|
|
executor.add_node(node)
|
|
|
|
|
executor.spin()
|
|
|
|
|
except KeyboardInterrupt:
|
|
|
|
|
pass
|
|
|
|
|
finally:
|
|
|
|
|
if rclpy.ok():
|
|
|
|
|
rclpy.shutdown()
|
|
|
|
|
executor.shutdown(timeout_sec=2.0)
|
|
|
|
|
if node is not None:
|
|
|
|
|
node.destroy_node()
|
